# 使用 ECL EKF
本文主要回答使用 ECL EKF 算法的常见问题。
PX4 State Estimation Overview (opens new window) 视频来自 PX4 Developer Summit 2019 (Dr. Paul Riseborough) 概要介绍了估计器，并另外说明了2018/2019年的主要变化以及2020年的预期改进。
# 什么是 ecl EKF？
- 四元数定义从北，东，地局部地球坐标系到 X，Y，Z 机体坐标系的旋转
- Velocity at the IMU - North, East, Down (m/s)
- Position at the IMU - North, East, Down (m)
- IMU delta angle bias estimates - X, Y, Z (rad)
- IMU delta velocity bias estimates - X, Y, Z (m/s)
- 地球磁场组分 - 北，东，地 (gauss)
- 飞行器机体坐标系磁场偏差 - X, Y, Z (gauss)
- 风速-北, 东(m/s)
EKF 在延迟的“融合时间范围”上运行，以允许相对于 IMU 的每次测量的不同时间延迟。 为了保证所有传感器数据都能在正确的时间内使用，每个传感器的数据都是按照先入先出（FIFO）队列进行缓存，并由EKF从缓存区中读取。 每个传感器的延迟补偿由 EKF2 * DELAY 参数控制。
互补滤波器用于使用缓冲的 IMU 数据将状态从“融合时间范围”向前传播到当前时间。 该滤波器的时间常数由 EKF2_TAU_VEL 和 EKF2_TAU_POS 参数控制。
EKF2_*_DELAY 参数决定。 如果未使用传感器，建议将其时间延迟设置为零。 减少“融合时间范围”延迟减少了用于将状态向前传播到当前时间的互补滤波器中的误差。
位置及速度状态变量在输出至控制回路之前会根据IMU与机体坐标系之间的偏差量进行修正。 IMU 相对于机体坐标系的位置由
EKF仅将IMU数据用于状态预测。 在EKF推导中，IMU数据不作为观测值使用。 The algebraic equations for the covariance prediction, state update and covariance update were derived using the Matlab symbolic toolbox and can be found here: Matlab Symbolic Derivation (opens new window).
# Running a Single EKF Instance
The default behaviour is to run a single instance of the EKF. In this case sensor selection and failover is performed before data is received by the EKF. This provides protection against a limited number of sensor faults, such as loss of data, but does not protect against the sensor providing inaccurate data that exceeds the ability of the EKF and control loops to compensate.
The parameter settings for running a single EKF instance are:
- EKF2_MULTI_IMU = 0
- EKF2_MULTI_MAG = 0
- SENS_IMU_MODE = 1
- SENS_MAG_MODE = 1
# Running Multiple EKF Instances
Depending on the number of IMUs and magnetometers and the autopilot's CPU capacity, multiple instances of the EKF can be run. This provides protection against a wider range of sensor errors and is achieved by each EKF instance using a different sensor combination. By comparing the internal consistency of each EKF instance, the EKF selector is able to determine the EKF and sensor combination with the best data consistency. This enables faults such as sudden changes in IMU bias, saturation or stuck data to be detected and isolated.
The total number of EKF instances is the product of the number of IMU's and number of magnetometers selected by EKF2_MULTI_IMU and EKF2_MULTI_MAG and is given by the following formula:
N_instances = MAX(EKF2_MULTI_IMU , 1) x MAX(EKF2_MULTI_MAG , 1)
For example an autopilot with 2 IMUs and 2 magnetometers could run with EKF2_MULTI_IMU = 2 and EKF2_MULTI_MAG = 2 for a total of 4 EKF instances where each instance uses the following combination of sensors:
- EKF instance 1 : IMU 1, magnetometer 1
- EKF instance 2 : IMU 1, magnetometer 2
- EKF instance 3 : IMU 2, magnetometer 1
- EKF instance 4 : IMU 2, magnetometer 2
The maximum number of IMU or magnetometer sensors that can be handled is 4 of each for a theoretical maximum of 4 x 4 = 16 EKF instances. In practice this is limited by available computing resources. During development of this feature, testing with STM32F7 CPU based HW demonstrated 4 EKF instances with acceptable processing load and memory utilisation margin.
Ground based testing to check CPU and memory utilisation should be performed before flying.
If EKF2_MULTI_IMU >= 3, then the failover time for large rate gyro errors is further reduced because the EKF selector is able to apply a median select strategy for faster isolation of the faulty IMU.
The setup for multiple EKF instances is controlled by the following parameters:
SENS_IMU_MODE: Set to 0 if running multiple EKF instances with IMU sensor diversity, ie EKF2_MULTI_IMU > 1.
When set to 1 (default for single EKF operation) the sensor module selects IMU data used by the EKF. This provides protection against loss of data from the sensor but does not protect against bad sensor data. When set to 0, the sensor module does not make a selection.
SENS_MAG_MODE: Set to 0 if running multiple EKF instances with magnetometer sensor diversity, ie EKF2_MULTI_MAG > 1.
When set to 1 (default for single EKF operation) the sensor module selects Magnetometer data used by the EKF. This provides protection against loss of data from the sensor but does not protect against bad sensor data. When set to 0, the sensor module does not make a selection.
EKF2_MULTI_IMU: This parameter specifies the number of IMU sensors used by the multiple EKF's. If
EKF2_MULTI_IMU<= 1, then only the first IMU sensor will be used. When SENS_IMU_MODE = 1, this will be the sensor selected by the sensor module. If
EKF2_MULTI_IMU>= 2, then a separate EKF instance will run for the specified number of IMU sensors up to the lesser of 4 or the number of IMU's present.
EKF2_MULTI_MAG: This parameter specifies the number of magnetometer sensors used by the multiple EKF's. If
EKF2_MULTI_MAG<= 1, then only the first magnetometer sensor will be used. When SENS_MAG_MODE = 1, this will be the sensor selected by the sensor module. If
EKF2_MULTI_MAG>= 2, then a separate EKF instance will run for the specified number of magnetometer sensors up to the lesser of 4 or the number of magnetometers present.
The recording and EKF2 replay of flight logs with multiple EKF instances is not supported. To enable recording for EKF replay you must set the parameters to enable a single EKF instance.
# What sensor measurements does it use?
The EKF has different modes of operation that allow for different combinations of sensor measurements. On start-up the filter checks for a minimum viable combination of sensors and after initial tilt, yaw and height alignment is completed, enters a mode that provides rotation, vertical velocity, vertical position, IMU delta angle bias and IMU delta velocity bias estimates.
This mode requires IMU data, a source of yaw (magnetometer or external vision) and a source of height data. This minimum data set is required for all EKF modes of operation. Other sensor data can then be used to estimate additional states.
- Three axis body fixed Inertial Measurement unit delta angle and delta velocity data at a minimum rate of 100Hz. Note: Coning corrections should be applied to the IMU delta angle data before it is used by the EKF.
Three axis body fixed magnetometer data (or external vision system pose data) at a minimum rate of 5Hz is required. Magnetometer data can be used in two ways:
- Magnetometer measurements are converted to a yaw angle using the tilt estimate and magnetic declination. This yaw angle is then used as an observation by the EKF. This method is less accurate and does not allow for learning of body frame field offsets, however it is more robust to magnetic anomalies and large start-up gyro biases. It is the default method used during start-up and on ground.
- The XYZ magnetometer readings are used as separate observations. This method is more accurate and allows body frame offsets to be learned, but assumes the earth magnetic field environment only changes slowly and performs less well when there are significant external magnetic anomalies.
The logic used to select these modes is set by the EKF2_MAG_TYPE parameter.
The option is available to operate without a magnetometer, either by replacing it using yaw from a dual antenna GPS or using the IMU measurements and GPS velocity data to estimate yaw from vehicle movement.
A source of height data - either GPS, barometric pressure, range finder or external vision at a minimum rate of 5Hz is required.
The primary source of height data is controlled by the EKF2_HGT_MODE parameter.
If these measurements are not present, the EKF will not start. When these measurements have been detected, the EKF will initialise the states and complete the tilt and yaw alignment. When tilt and yaw alignment is complete, the EKF can then transition to other modes of operation enabling use of additional sensor data:
Barometric pressure altitude is subject to errors generated by aerodynamic disturbances caused by vehicle wind relative velocity and orientation. This is known in aeronautics as static pressure position error. The EKF2 module that uses the ECL/EKF2 estimator library provides a method of compensating for these errors, provided wind speed state estimation is active.
For platforms operating in a fixed wing mode, wind speed state estimation requires either Airspeed and/or Synthetic Sideslip fusion to be enabled.
For multi-rotors, fusion of Drag Specific Forces can be enabled and tuned to provide the required wind velocity state estimates.
The EKF2 module models the error as a body fixed ellipsoid that specifies the fraction of dynamic pressure that is added to/subtracted from the barometric pressure - before it is converted to a height estimate. See the following parameter documentation for information on how to use this feature:
GPS measurements will be used for position and velocity if the following conditions are met:
- GPS use is enabled via setting of the EKF2_AID_MASK parameter.
- GPS quality checks have passed. These checks are controlled by the EKF2_GPS_CHECK and
- GPS height can be used directly by the EKF via setting of the EKF2_HGT_MODE parameter.
Some GPS receivers such as the Trimble MB-Two RTK GPS receiver (opens new window) can be used to provide a heading measurement that replaces the use of magnetometer data. This can be a significant advantage when operating in an environment where large magnetic anomalies are present, or at latitudes here the earth's magnetic field has a high inclination. Use of GPS yaw measurements is enabled by setting bit position 7 to 1 (adding 128) in the EKF2_AID_MASK parameter.
# 从 GPS 速度数据获取偏航角
The EKF runs an additional multi-hypothesis filter internally that uses multiple 3-state Extended Kalman Filters (EKF's) whose states are NE velocity and yaw angle. These individual yaw angle estimates are then combined using a Gaussian Sum Filter (GSF). The individual 3-state EKF's use IMU and GPS horizontal velocity data (plus optional airspeed data) and do not rely on any prior knowledge of the yaw angle or magnetometer measurements. This provides a backup to the yaw from the main filter and is used to reset the yaw for the main 24-state EKF when a post-takeoff loss of navigation indicates that the yaw estimate from the magnetometer is bad. This will result in an
Emergency yaw reset - magnetometer use stopped message information message at the GCS.
Data from this estimator is logged when ekf2 replay logging is enabled and can be viewed in the
yaw_estimator_status message. The individual yaw estimates from the individual 3-state EKF yaw estimators are in the
yaw fields. The GSF combined yaw estimate is in the
yaw_composite field. The variance for the GSF yaw estimate is in the
yaw_variance field. All angles are in radians. Weightings applied by the GSF to the individual 3-state EKF outputs are in the
This also makes it possible to operate without any magnetometer data or dual antenna GPS receiver for yaw provided some horizontal movement after takeoff can be performed to enable the yaw to become observable. To use this feature, set EKF2_MAG_TYPE to
none (5) to disable magnetometer use. Once the vehicle has performed sufficient horizontal movement to make the yaw observable, the main 24-state EKF will align it's yaw to the GSF estimate and commence use of GPS.
# 双 GPS 接收器
Data from GPS receivers can be blended using an algorithm that weights data based on reported accuracy (this works best if both receivers output data at the same rate and use the same accuracy). The mechanism also provides automatic failover if data from a receiver is lost (it allows, for example, a standard GPS to be used as a backup to a more accurate RTK receiver). This is controlled by the SENS_GPS_MASK parameter.
The SENS_GPS_MASK parameter is set by default to disable blending and always use the first receiver, so it will have to be set to select which receiver accuracy metrics are used to decide how much each receiver output contributes to the blended solution. Where different receiver models are used, it is important that the SENS_GPS_MASK parameter is set to a value that uses accuracy metrics that are supported by both receivers. For example do not set bit position 0 to
true unless the drivers for both receivers publish values in the
s_variance_m_s field of the
vehicle_gps_position message that are comparable. This can be difficult with receivers from different manufacturers due to the different way that accuracy is defined, e.g. CEP vs 1-sigma, etc.
The following items should be checked during setup:
- Verify that data for the second receiver is present. This will be logged as
vehicle_gps_position_1and can also be checked when connected via the nsh console using the command
listener vehicle_gps_position -i 1. The GPS_2_CONFIG parameter will need to be set correctly.
- Check the
epvdata from each receiver and decide which accuracy metrics can be used. If both receivers output sensible
ephdata, and GPS vertical position is not being used directly for navigation, then setting SENS_GPS_MASK to 3 is recommended. Where only
ephdata is available and both receivers do not output
s_variance_m_sdata, set SENS_GPS_MASK to 2. Bit position 2 would only be set if the GPS had been selected as a primary height source with the EKF2_HGT_MODE parameter and both receivers output sensible
- The output from the blended receiver data is logged as
ekf_gps_position, and can be checked whilst connect via the nsh terminal using the command
- Where receivers output at different rates, the blended output will be at the rate of slower receiver. Where possible receivers should be configured to output at the same rate.
For the ECL to accept GNSS data for navigation, certain minimum requirements need to be satisfied over a period of time, defined by EKF2_REQ_GPS_H (10 seconds by default).
Minima are defined in the EKF2_REQ_* parameters and each check can be en-/disabled using the EKF2_GPS_CHECK parameter.
The table below shows the different metrics directly reported or calculated from the GNSS data, and the minimum required values for the data to be used by ECL. In addition, the Average Value column shows typical values that might reasonably be obtained from a standard GNSS module (e.g. u-blox M8 series) - i.e. values that are considered good/acceptable.
|eph||< 3 (EKF2_REQ_EPH)||0.8||米||水平位置误差的标准偏差|
|epv||< 5 (EKF2_REQ_EPV)||1.5||米||垂直位置误差的标准偏差|
|Number of satellites||≥6 (EKF2_REQ_NSATS)||14||-|
|sacc||< 0.5 (EKF2_REQ_SACC)||0.2||米/秒||水平速度误差的标准偏差|
|fix type||≥ 3||4||-||0-1: 不修正, 2: 2D 修正, 3: 3D 修正, 4: RTCM 编码差分, 5: 实时动态定位, 浮动, 6: 实时动态定位, 固定, 8: 外推|
|PDOP||< 2.5 (EKF2_REQ_PDOP)||1.0||-||精度降低位置|
|hpos drift rate||< 0.1 (EKF2_REQ_HDRIFT)||0.01||米/秒||根据所报告的全球导航卫星系统位置计算出的漂移率（在固定状态时）。|
|vpos drift rate||< 0.2 (EKF2_REQ_VDRIFT)||0.02||2] Velocity NED (m/s)||根据所报告的全球导航卫星系统高度计算出的漂移率（在固定时）。|
|hspd||< 0.1 (EKF2_REQ_HDRIFT)||0.01||m/s||所报告的全球导航卫星系统横向速度的筛选星等。|
|vspd||< 0.2 (EKF2_REQ_VDRIFT)||0.02||m/s||所报告的全球导航卫星系统垂直速度的滤波量级。|
hspd are calculated over a period of 10 seconds and published in the
ekf2_gps_drift topic. Note that
ekf2_gps_drift is not logged!
Range finder distance to ground is used by a single state filter to estimate the vertical position of the terrain relative to the height datum.
If operating over a flat surface that can be used as a zero height datum, the range finder data can also be used directly by the EKF to estimate height by setting the EKF2_HGT_MODE parameter to 2.
Equivalent Airspeed (EAS) data can be used to estimate wind velocity and reduce drift when GPS is lost by setting EKF2_ARSP_THR to a positive value. Airspeed data will be used when it exceeds the threshold set by a positive value for EKF2_ARSP_THR and the vehicle type is not rotary wing.
Fixed wing platforms can take advantage of an assumed sideslip observation of zero to improve wind speed estimation and also enable wind speed estimation without an airspeed sensor. This is enabled by setting the EKF2_FUSE_BETA parameter to 1.
Multi-rotor platforms can take advantage of the relationship between airspeed and drag force along the X and Y body axes to estimate North/East components of wind velocity. This is enabled by setting bit position 5 in the EKF2_AID_MASK parameter to true. The relationship between airspeed and specific force (IMU acceleration) along the X and Y body axes is controlled by the EKF2_BCOEF_X and EKF2_BCOEF_Y parameters which set the ballistic coefficients for flight in the X and Y directions respectively. The amount of specific force observation noise is set by the EKF2_DRAG_NOISE parameter.
These can be tuned by flying the vehicle in Position mode repeatedly forwards/backwards between rest and maximum speed, adjusting EKF2_BCOEF_X so that the corresponding innovation sequence in the
ekf2_innovations_0.drag_innov log message is minimised. This is then repeated for right/left movement with adjustment of EKF2_BCOEF_Y to minimise the
ekf2_innovations_0.drag_innov innovation sequence. Tuning is easier if this testing is conducted in still conditions.
If you are able to log data without dropouts from boot using SDLOG_MODE = 1 and SDLOG_PROFILE = 2, have access to the development environment, and are able to build code, then we recommended you fly once and perform the tuning via EKF2 Replay of the flight log.
The recording and EKF2 replay of flight logs with multiple EKF instances is not supported. To enable recording for EKF replay you must set the parameters to enable a single EKF instance.
Optical flow data will be used if the following conditions are met:
- Valid range finder data is available.
- Bit position 1 in the EKF2_AID_MASK parameter is true.
- The quality metric returned by the flow sensor is greater than the minimum requirement set by the EKF2_OF_QMIN parameter.
Position, velocity or orientation measurements from an external vision system, e.g. Vicon, can be used:
- External vision system horizontal position data will be used if bit position 3 in the EKF2_AID_MASK parameter is true.
- External vision system vertical position data will be used if the EKF2_HGT_MODE parameter is set to 3.
- External vision system velocity data will be used if bit position 8 in the EKF2_AID_MASK parameter is true.
- External vision system orientation data will be used for yaw estimation if bit position 4 in the EKF2_AID_MASK parameter is true.
- External vision reference frame offset will be estimated and used to rotate the external vision system data if bit position 6 in the EKF2_AID_MASK parameter is true.
Either bit 4 (
EV_YAW) or bit 6 (
EV_ROTATE) should be set to true, but not both together. Following EKF2_AID_MASK values are supported when using with an external vision system.
|321||GPS + EV_VEL + ROTATE_EV||航向相关/以北为正(Recommended)|
|73||GPS + EV_POS + ROTATE_EV||航向相关/以北为正(Not recommended, 使用 |
|24||EV_POS + EV_YAW||航向相关/跟随外部视觉系统|
|72||EV_POS + ROTATE_EV||航向相关/以北为正|
|272||EV_VEL + EV_YAW||航向相关/跟随外部视觉系统|
|320||EV_VEL + ROTATE_EV||航向相关/以北为正|
|280||EV_POS + EV_VEL + EV_YAW||航向相关/跟随外部视觉系统|
|328||EV_POS + EV_VEL + ROTATE_EV||航向相关/以北为正|
The EKF considers uncertainty in the visual pose estimate. This uncertainty information can be sent via the covariance fields in the MAVLink ODOMETRY (opens new window) message or it can be set through the parameters EKF2_EVP_NOISE, EKF2_EVV_NOISE and EKF2_EVA_NOISE. You can choose the source of the uncertainty with EKF2_EV_NOISE_MD.
# How do I use the 'ecl' library EKF?
Set the SYS_MC_EST_GROUP parameter to 2 to use the ecl EKF.
# What are the advantages and disadvantages of the ecl EKF over other estimators?
Like all estimators, much of the performance comes from the tuning to match sensor characteristics. Tuning is a compromise between accuracy and robustness and although we have attempted to provide a tune that meets the needs of most users, there will be applications where tuning changes are required.
For this reason, no claims for accuracy relative to the legacy combination of
local_position_estimator have been made and the best choice of estimator will depend on the application and tuning.
- The ecl EKF is a complex algorithm that requires a good understanding of extended Kalman filter theory and its application to navigation problems to tune successfully. It is therefore more difficult for users that are not achieving good results to know what to change.
- The ecl EKF uses more RAM and flash space.
- The ecl EKF uses more logging space.
- The ecl EKF is able to fuse data from sensors with different time delays and data rates in a mathematically consistent way which improves accuracy during dynamic maneuvers once time delay parameters are set correctly.
- The ecl EKF is capable of fusing a large range of different sensor types.
- The ecl EKF detects and reports statistically significant inconsistencies in sensor data, assisting with diagnosis of sensor errors.
- For fixed wing operation, the ecl EKF estimates wind speed with or without an airspeed sensor and is able to use the estimated wind in combination with airspeed measurements and sideslip assumptions to extend the dead-reckoning time available if GPS is lost in flight.
- The ecl EKF estimates 3-axis accelerometer bias which improves accuracy for tailsitters and other vehicles that experience large attitude changes between flight phases.
- The federated architecture (combined attitude and position/velocity estimation) means that attitude estimation benefits from all sensor measurements. This should provide the potential for improved attitude estimation if tuned correctly.
# How do I check the EKF performance?
EKF outputs, states and status data are published to a number of uORB topics which are logged to the SD card during flight. The following guide assumes that data has been logged using the .ulog file format. The .ulog format data can be parsed in python by using the PX4 pyulog library (opens new window).
Most of the EKF data is found in the estimator_innovations (opens new window) and estimator_status (opens new window) uORB messages that are logged to the .ulog file.
A python script that automatically generates analysis plots and metadata can be found here (opens new window). To use this script file, cd to the
Tools/ecl_ekf directory and enter
python process_logdata_ekf.py <log_file.ulg>. This saves performance metadata in a csv file named <log_file>.mdat.csv and plots in a pdf file named
Multiple log files in a directory can be analysed using the batch_process_logdata_ekf.py (opens new window) script. When this has been done, the performance metadata files can be processed to provide a statistical assessment of the estimator performance across the population of logs using the batch_process_metadata_ekf.py (opens new window) script.
- Attitude output data is found in the vehicle_attitude (opens new window) message.
- Local position output data is found in the vehicle_local_position (opens new window) message.
- Global (WGS-84) output data is found in the vehicle_global_position (opens new window) message.
- Wind velocity output data is found in the wind_estimate (opens new window) message.
Refer to states in estimator_status (opens new window). The index map for states is as follows:
- [0 ... 3] Quaternions
- [4 ... 6] Velocity NED (m/s)
- [7 ... 9] Position NED (m)
- [10 ... 12] IMU delta angle bias XYZ (rad)
- [13 ... 15] IMU delta velocity bias XYZ (m/s)
- [16 ... 18] Earth magnetic field NED (gauss)
- [19 ... 21] Body magnetic field XYZ (gauss)
- [22 ... 23] Wind velocity NE (m/s)
- [24 ... 32] Not Used
Refer to covariances in estimator_status (opens new window). The index map for covariances is as follows:
- [0 ... 3] Quaternions
- [4 ... 6] Velocity NED (m/s)^2
- [7 ... 9] Position NED (m^2)
- [10 ... 12] IMU delta angle bias XYZ (rad^2)
- [13 ... 15] IMU delta velocity bias XYZ (m/s)^2
- [16 ... 18] Earth magnetic field NED (gauss^2)
- [19 ... 21] Body magnetic field XYZ (gauss^2)
- [22 ... 23] Wind velocity NE (m/s)^2
- [24 ... 28] Not Used
estimator_innovation_test_ratios message fields are defined in estimator_innovations.msg (opens new window). The messages all have the same field names/types (but different units).
The messages have the same fields because they are generated from the same field definition. The
# TOPICS line (at the end of the file (opens new window)) lists the names of the set of messages to be created):
# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios
Some of the observations are:
- Magnetometer XYZ (gauss, gauss^2) :
- Yaw angle (rad, rad^2) :
- True Airspeed (m/s, (m/s)^2) :
- Synthetic sideslip (rad, rad^2) :
- Optical flow XY (rad/sec, (rad/s)^2) :
- Height above ground (m, m^2) :
- Drag specific force ((m/s)^2):
- Velocity and position innovations : per sensor
In addition, each sensor has its own fields for horizontal and vertical position and/or velocity values (where appropriate). These are largely self documenting, and are reproduced below:
# GPS float32 gps_hvel # horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) float32 gps_vvel # vertical GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) float32 gps_hpos # horizontal GPS position innovation (m) and innovation variance (m**2) float32 gps_vpos # vertical GPS position innovation (m) and innovation variance (m**2) # External Vision float32 ev_hvel # horizontal external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) float32 ev_vvel # vertical external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) float32 ev_hpos # horizontal external vision position innovation (m) and innovation variance (m**2) float32 ev_vpos # vertical external vision position innovation (m) and innovation variance (m**2) # Fake Position and Velocity float32 fake_hvel # fake horizontal velocity innovation (m/s) and innovation variance ((m/s)**2) float32 fake_vvel # fake vertical velocity innovation (m/s) and innovation variance ((m/s)**2) float32 fake_hpos # fake horizontal position innovation (m) and innovation variance (m**2) float32 fake_vpos # fake vertical position innovation (m) and innovation variance (m**2) # Height sensors float32 rng_vpos # range sensor height innovation (m) and innovation variance (m**2) float32 baro_vpos # barometer height innovation (m) and innovation variance (m**2) # Auxiliary velocity float32 aux_hvel # horizontal auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) float32 aux_vvel # vertical auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
The output complementary filter is used to propagate states forward from the fusion time horizon to current time. To check the magnitude of the angular, velocity and position tracking errors measured at the fusion time horizon, refer to
output_tracking_error in the
The index map is as follows:
-  Angular tracking error magnitude (rad)
-  Velocity tracking error magnitude (m/s). The velocity tracking time constant can be adjusted using the EKF2_TAU_VEL parameter. Reducing this parameter reduces steady state errors but increases the amount of observation noise on the NED velocity outputs.
-  Position tracking error magnitude (m). The position tracking time constant can be adjusted using the EKF2_TAU_POS parameter. Reducing this parameter reduces steady state errors but increases the amount of observation noise on the NED position outputs.
# EKF 错误
The EKF contains internal error checking for badly conditioned state and covariance updates. Refer to the filter_fault_flags in estimator_status (opens new window).
There are two categories of observation faults:
- Loss of data. An example of this is a range finder failing to provide a return.
- The innovation, which is the difference between the state prediction and sensor observation is excessive. An example of this is excessive vibration causing a large vertical position error, resulting in the barometer height measurement being rejected.
Both of these can result in observation data being rejected for long enough to cause the EKF to attempt a reset of the states using the sensor observations. All observations have a statistical confidence checks applied to the innovations. The number of standard deviations for the check are controlled by the
EKF2_*_GATE parameter for each observation type.
Test levels are available in estimator_status (opens new window) as follows:
mag_test_ratio: ratio of the largest magnetometer innovation component to the innovation test limit
vel_test_ratio: ratio of the largest velocity innovation component to the innovation test limit
pos_test_ratio: ratio of the largest horizontal position innovation component to the innovation test limit
hgt_test_ratio: ratio of the vertical position innovation to the innovation test limit
tas_test_ratio: ratio of the true airspeed innovation to the innovation test limit
hagl_test_ratio: ratio of the height above ground innovation to the innovation test limit
For a binary pass/fail summary for each sensor, refer to innovation_check_flags in estimator_status (opens new window).
# GPS 数据质量检查
The EKF applies a number of GPS quality checks before commencing GPS aiding. These checks are controlled by the EKF2_GPS_CHECK and
EKF2_REQ_* parameters. The pass/fail status for these checks is logged in the estimator_status (opens new window).gps_check_fail_flags message. This integer will be zero when all required GPS checks have passed. If the EKF is not commencing GPS alignment, check the value of the integer against the bitmask definition
gps_check_fail_flags in estimator_status (opens new window).
# EKF 数值误差
The EKF uses single precision floating point operations for all of its computations and first order approximations for derivation of the covariance prediction and update equations in order to reduce processing requirements. This means that it is possible when re-tuning the EKF to encounter conditions where the covariance matrix operations become badly conditioned enough to cause divergence or significant errors in the state estimates.
To prevent this, every covariance and state update step contains the following error detection and correction steps:
- If the innovation variance is less than the observation variance (this requires a negative state variance which is impossible) or the covariance update will produce a negative variance for any of the states, then:
- The state and covariance update is skipped
- The corresponding rows and columns in the covariance matrix are reset
- The failure is recorded in the estimator_status (opens new window) filter_fault_flags message
- State variances (diagonals in the covariance matrix) are constrained to be non-negative.
- An upper limit is applied to state variances.
- Symmetry is forced on the covariance matrix.
After re-tuning the filter, particularly re-tuning that involve reducing the noise variables, the value of
estimator_status.gps_check_fail_flags should be checked to ensure that it remains zero.
# What should I do if the height estimate is diverging?
The most common cause of EKF height diverging away from GPS and altimeter measurements during flight is clipping and/or aliasing of the IMU measurements caused by vibration. If this is occurring, then the following signs should be evident in the data
- estimator_innovations (opens new window).vel_pos_innov and estimator_innovations (opens new window).vel_pos_innov will both have the same sign.
- estimator_status (opens new window).hgt_test_ratio will be greater than 1.0
The recommended first step is to ensure that the autopilot is isolated from the airframe using an effective isolation mounting system. An isolation mount has 6 degrees of freedom, and therefore 6 resonant frequencies. As a general rule, the 6 resonant frequencies of the autopilot on the isolation mount should be above 25Hz to avoid interaction with the autopilot dynamics and below the frequency of the motors.
An isolation mount can make vibration worse if the resonant frequencies coincide with motor or propeller blade passage frequencies.
The EKF can be made more resistant to vibration induced height divergence by making the following parameter changes:
- Double the value of the innovation gate for the primary height sensor. If using barometric height this is EKF2_BARO_GATE.
- Increase the value of EKF2_ACC_NOISE to 0.5 initially. If divergence is still occurring, increase in further increments of 0.1 but do not go above 1.0
Note that the effect of these changes will make the EKF more sensitive to errors in GPS vertical velocity and barometric pressure.
# What should I do if the position estimate is diverging?
The most common causes of position divergence are:
- High vibration levels.
- Fix by improving mechanical isolation of the autopilot.
- Increasing the value of EKF2_ACC_NOISE and EKF2_GYR_NOISE can help, but does make the EKF more vulnerable to GPS glitches.
- Large gyro bias offsets.
- Fix by re-calibrating the gyro. Check for excessive temperature sensitivity (> 3 deg/sec bias change during warm-up from a cold start and replace the sensor if affected of insulate to slow the rate of temperature change.
- Bad yaw alignment
- Check the magnetometer calibration and alignment.
- Check the heading shown QGC is within 15 deg truth
- Poor GPS accuracy
- Check for interference
- Improve separation and shielding
- Check flying location for GPS signal obstructions and reflectors (nearby tall buildings)
- Loss of GPS
Determining which of these is the primary cause requires a methodical approach to analysis of the EKF log data:
- Plot the velocity innovation test ratio - estimator_status (opens new window).vel_test_ratio
- Plot the horizontal position innovation test ratio - estimator_status (opens new window).pos_test_ratio
- Plot the height innovation test ratio - estimator_status (opens new window).hgt_test_ratio
- Plot the magnetometer innovation test ratio - estimator_status (opens new window).mag_test_ratio
- Plot the GPS receiver reported speed accuracy - vehicle_gps_position (opens new window).s_variance_m_s
- Plot the IMU delta angle state estimates - estimator_status (opens new window).states, states and states
- Plot the EKF internal high frequency vibration metrics:
- Delta angle coning vibration - estimator_status (opens new window).vibe
- High frequency delta angle vibration - estimator_status (opens new window).vibe
- High frequency delta velocity vibration - estimator_status (opens new window).vibe
During normal operation, all the test ratios should remain below 0.5 with only occasional spikes above this as shown in the example below from a successful flight:
The following plot shows the EKF vibration metrics for a multirotor with good isolation. The landing shock and the increased vibration during takeoff and landing can be seen. Insufficient data has been gathered with these metrics to provide specific advice on maximum thresholds.
The above vibration metrics are of limited value as the presence of vibration at a frequency close to the IMU sampling frequency (1 kHz for most boards) will cause offsets to appear in the data that do not show up in the high frequency vibration metrics. The only way to detect aliasing errors is in their effect on inertial navigation accuracy and the rise in innovation levels.
In addition to generating large position and velocity test ratios of > 1.0, the different error mechanisms affect the other test ratios in different ways:
High vibration levels normally affect vertical position and velocity innovations as well as the horizontal components. Magnetometer test levels are only affected to a small extent.
(insert example plots showing bad vibration here)
Large gyro bias offsets are normally characterised by a change in the value of delta angle bias greater than 5E-4 during flight (equivalent to ~3 deg/sec) and can also cause a large increase in the magnetometer test ratio if the yaw axis is affected. Height is normally unaffected other than extreme cases. Switch on bias value of up to 5 deg/sec can be tolerated provided the filter is given time settle before flying. Pre-flight checks performed by the commander should prevent arming if the position is diverging.
(insert example plots showing bad gyro bias here)
Bad yaw alignment causes a velocity test ratio that increases rapidly when the vehicle starts moving due inconsistency in the direction of velocity calculated by the inertial nav and the GPS measurement. Magnetometer innovations are slightly affected. Height is normally unaffected.
(insert example plots showing bad yaw alignment here)
# 确定较差的GPS 数据精度
Poor GPS accuracy is normally accompanied by a rise in the reported velocity error of the receiver in conjunction with a rise in innovations. Transient errors due to multipath, obscuration and interference are more common causes. Here is an example of a temporary loss of GPS accuracy where the multi-rotor started drifting away from its loiter location and had to be corrected using the sticks. The rise in estimator_status (opens new window).vel_test_ratio to greater than 1 indicates the GPs velocity was inconsistent with other measurements and has been rejected.
This is accompanied with rise in the GPS receivers reported velocity accuracy which indicates that it was likely a GPS error.
If we also look at the GPS horizontal velocity innovations and innovation variances, we can see the large spike in North velocity innovation that accompanies this GPS 'glitch' event.
# 确定 GPS 数据的丢失
Loss of GPS data will be shown by the velocity and position innovation test ratios 'flat-lining'. If this occurs, check the other GPS status data in
vehicle_gps_position for further information.
The following plot shows the NED GPS velocity innovations
ekf2_innovations_0.vel_pos_innov[0 ... 2], the GPS NE position innovations
ekf2_innovations_0.vel_pos_innov[3 ... 4] and the Baro vertical position innovation
ekf2_innovations_0.vel_pos_innov generated from a simulated VTOL flight using SITL Gazebo.
The simulated GPS was made to lose lock at 73 seconds. Note the NED velocity innovations and NE position innovations 'flat-line' after GPS is lost. Note that after 10 seconds without GPS data, the EKF reverts back to a static position mode using the last known position and the NE position innovations start to change again.
If the vehicle has the tendency during landing to climb back into the air when close to the ground, the most likely cause is barometer ground effect.
This is caused when air pushed down by the propellers hits the ground and creates a high pressure zone below the drone. The result is a lower reading of pressure altitude, leading to an unwanted climb being commanded. The figure below shows a typical situation where the ground effect is present. Note how the barometer signal dips at the beginning and end of the flight.
You can enable ground effect compensation to fix this problem:
- From the plot estimate the magnitude of the barometer dip during takeoff or landing. In the plot above one can read a barometer dip of about 6 meters during landing.
- Then set the parameter EKF2_GND_EFF_DZ to that value and add a 10 percent margin. Therefore, in this case a value of 6.6 meters would be a good starting point.
If a terrain estimate is available (e.g. the vehicle is equipped with a range finder) then you can additionally specify EKF2_GND_MAX_HGT, the above ground-level altitude below which ground effect compensation should be activated. If no terrain estimate is available this parameter will have no effect and the system will use heuristics to determine if ground effect compensation should be activated.
# Further Information
- PX4 State Estimation Overview (opens new window), PX4 Developer Summit 2019, Dr. Paul Riseborough): Overview of the estimator, and major changes from 2018/19, and the expected improvements through 2019/20.