Developer-info.md

   1 # Inertial position estimator (INAV)
   2
   3 Position estimator is a vital component for navigation subsystem. It takes data from all available sensors and fuses them together to figure out coordinates and velocities in the local frame of reference. All navigational decisions are made based on estimated position/velocity data.
   4
   5 It is currently desined for multirotors.
   6
   7 ## Principle of operation
   8
   9 The key sensor for INAV is an accelerometer. Measured acceleration is translated from body-fixed frame to local NEU coordinates and integrated to yield velocities in North, East and Up directions. Velocity is further integrated to produce coordinates.
  10
  11 As accelerometer tend to drift, estimated velocities and coordinates tend to drift as well. This accumulated error is corrected from various reference sources - GPS, BARO, SONAR. Position estimator also maintains estimated position error for horizontal (X-Y) and vertical (Z) position.
  12
  13 When reference source is not available for some reason, estimated position error increases until it reaches a certain threshold. Beyond that threshold position is no longer updated and marked invalid until a valid reference source is available avain. This allows, for example, to fly through short (measured in seconds) GPS outages.
  14
  15 Using multiple sensors for estimation allows to filter noisy data (e.g. from barometer), interpolate between rare readings (e.g. from GPS), and immediately react on fast motion changes (using accelerometers) in the same time.
  16
  17 ![](images/inertial_estimator_diagram.jpg)
  18
  19 ## Data soures
  20
  21 The following reference sources (with corresponding parameters for weight) are available for altitude and climb rate:
  22
  23 * Barometer - altitude (**inav_w_z_baro_p**)
  24 * Barometer - velocity (**inav_w_z_baro_v**)
  25 * Sonar - altitude (**inav_w_z_sonar_p**)
  26 * Sonar - velocity (**inav_w_z_sonar_v**)
  27 * GPS - altitude (**inav_w_z_gps_p**)
  28 * GPS - velocity (**inav_w_z_gps_v**)
  29
  30 Sonar is optional source, it's only used when available and valid data received. GPS altitude is very noisy and is limited to FIXED_WING aircraft (experimental and untested).
  31
  32 The following reference sources (with corresponding parameters for weight) are available for position and velocity:
  33
  34 * GPS - position (**inav_w_xy_gps_p**)
  35 * GPS - velocity (**inav_w_xy_gps_v**)
  36
  37 ## Dead reckoning and handling sensor unavailability
  38
  39 * Enable dead reckoning (**inav_enable_dead_reckoning**)
  40
  41 * Dead reckoning - position (**inav_w_xy_dr_p**)
  42 * Dead reckoning - velocity (**inav_w_xy_dr_v**)
  43
  44 * Velocity decay rate, XY-axis (**inav_w_xy_res_v**)
  45 * Velocity decay rate, Z-axis (**inav_w_z_res_v**)
  46
  47 ## Estimated position error thresholds
  48
  49 * Maximum acceptable position error (**inav_max_eph_epv**)
  50 * Position error for SONAR sensor (**inav_sonar_epv**)
  51 * Position error for BARO sensor (**inav_baro_epv**)
  52
  53 ## GPS delay compensation
  54
  55 GPS data is not updated instantly. GPS module needs time to calculate new position and velocity. INAV has means of compensating for this delay. Expected GPS delay in milliseconds is controlled by **inav_gps_delay_ms** parameter. Typical value for GPS delay is 200ms.
  56
  57
  58 # Position and altitude PID controllers
  59
  60 ## PID regulators in ALTHOLD mode (Z-controller)
  61
  62 ALTHOLD mode uses two PIDs - **ALT** and **VEL**. Navigation Z-controller functional diagram is shown below:
  63
  64 ![](images/nav_althold_pids_diagram.jpg)
  65
  66 ### ALT PID
  67 Actually ALT PID parameters control two P-controllers: Position-to-Velocity and Velocity-to-Acceleration
  68
  69 * **ALT_P** - defines how fast quad will attempt to compensate for altitude error, converts altitude error to desired vertical velocity (climb rate)
  70 * **ALT_I** - not used
  71 * **ALT_D** - not used
  72
  73 ### VEL PID
  74 This PID-controller is an Acceleration-to-Throttle controller
  75
  76 * **VEL_P** - defined how much throttle quad will add/reduce to achieve desired velocity
  77 * **VEL_I** - controls compensation for hover throttle (and vertical air movement, thermals). This can be zero if hover throttle is precisely 1500us. Too much VEL I will lead to vertical oscillations, too low VEL I will cause drops or jumps when ALTHOLD is enabled, very low VEL I can result in total inability to maintain altitude
  78 * **VEL_D** - acts as a dampener for acceleration. VEL D will resist any velocity change regardless of its nature (requested by VEL P and VEL I or induced by wind).
  79
  80 ## PID regulators in POSHOLD/RTH/WP modes (XY-controller)
  81
  82 XY-controller uses two PIDs - **POS** and **POSR**
  83
  84 ![](images/nav_poshold_pids_diagram.jpg)
  85
  86 ### POS PID
  87 This is a Position-to-Velocity P-controller active in POSHOLD, RTH and WP modes
  88
  89 * **POS_P** - translates position error to desired velocity to reach the target
  90 * **POS_I** - not used
  91 * **POS_D** - not used
  92
  93 ### POSR PID
  94 Position rate PID-controller, controls Velocity-to-Acceleration
  95
  96 * **POSR_P** - controls acceleration to achieve desired velocity
  97 * **POSR_I** - controls compensation for side wind or other disturbances. In totally calm air POSR I can be close to zero
  98 * **POSR_D** - dampens response from P and I components; Tests indicate that this one can be zero at all times
  99
 100 Output of POSR PID-controller is desired acceleration which is directly translated to desired lean angles.
 101
 102
 103
 104 # Coordinate systems
 105
 106 Navigation operates in 3 different coordinate systems.
 107
 108 ## LLH (Geographic) Coordinate System
 109
 110 Represents position on or above earth with a latitude, longitude and height value. Height is defined as altitude above the mean sea level.
 111
 112 ![](https://upload.wikimedia.org/wikipedia/commons/6/62/Latitude_and_Longitude_of_the_Earth.svg)
 113
 114 ## NEU Coordinate System
 115
 116 * The x axis is aligned with the vector to the north pole (tangent to meridians).
 117 * The y axis points to the east side (tangent to parallels)
 118 * The z axis points up from the center of the earth
 119
 120 This is a classical cartesian coordinate system where the 3 axes are orthogonal to each other.
 121
 122 ![](https://upload.wikimedia.org/wikipedia/commons/7/73/ECEF_ENU_Longitude_Latitude_relationships.svg)
 123
 124 The units for the NEU coordinate system are centimetres.
 125
 126 # Frames of reference
 127
 128 ## Global (geodetic) frame of reference
 129
 130 This frame of reference defines coordinates in LLH coordinate system. This frame of reference is not used directly by the code and is provided as an interface for defining waypoints and receiving reference data from GPS.
 131
 132 ## Local frame of reference
 133
 134 This frame of reference defines coordinate in NEU coordinate system relative to a GPS Origin point. GPS origin is defined as a point where GPS fix with sufficient accuracy was firstly acquired. GPS Origin is usually a point of launch. Most calculations are done in this frame of reference.
 135
 136 ## Body-fixed frame of reference
 137
 138 Attached to the aircraft.
 139
 140 * The x axis points in forward direction (as defined by geometry and not by movement) (roll axis)
 141 * The y axis points to the right (geometrically) (pitch axis)
 142 * The z axis points upwards (geometrically) (yaw axis)
 143
 144 This frame of reference is used to read sensor data and calculate lean angles. Usually the only operations in this frame of reference are coordinate transformations to/from local frame of reference.