PX4 自动驾驶用户指南 (v1.13)

# 使用 ECL EKF

本文主要回答使用 ECL EKF 算法的常见问题。

提示

The PX4 State Estimation Overview (opens new window) video from the PX4 Developer Summit 2019 (Dr. Paul Riseborough) provides an overview of the estimator, and additionally describes both the major changes from 2018/2019, and the expected improvements through 2020.

# 什么是 ecl EKF？

估计和控制库（ECL）使用扩展卡尔曼滤波算法（EKF）来处理传感器的测量信息，并提供如下状态量的估计值：

  • 四元数定义从北，东，地局部地球坐标系到 X，Y，Z 机体坐标系的旋转
  • IMU 处的速度 - 北，东，地 (m/s)
  • IMU 处的位置 - 北，东，地 (m)
  • IMU 增量角度偏差估计 - X, Y, Z (rad)
  • IMU 增量速度偏差估计 - 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 相对于机体坐标系的位置由 EKF2_IMU_POS_X,Y,Z 参数设置。

EKF仅将IMU数据用于状态预测。 在EKF推导中，IMU数据不作为观测值使用。 使用Matlab符号工具箱导出了协方差预测、状态更新和协方差更新的代数方程，该工具箱可在这里找到：Matlab Symbolic Derivation (opens new window)。

# 运行单个EKF实例

The default behaviour is to run a single instance of the EKF. 在这种情况下，在EKF收到数据之前执行传感器选择和故障切换。 这为防止有限数量的传感器故障，如数据丢失等，提供了保护。 但不能防止传感器提供的不准确数据超过EKF和控制循环的补偿能力。

运行单个EKF实例的参数设置为：

# 运行多个EKF实例

根据IMU和磁强计的数量以及自动驾驶仪的CPU能力，EKF可以运行多个实例。 这提供了一系列更广泛的传感器错误的保护，每个EKF实例使用不同的传感器组合实现了这一点。 通过比较每个EKF实例的内部一致性，EKF选择器能够确定具有最佳数据一致性的EKF和传感器组合。 这样可以检测和隔离IMU偏差、饱和或数据卡住等故障。

EKF实例总数是由 EKF2_MULTI_IMU 和 EKF2_MULTI_MAG 所选择的IMU数量和磁强计数量的乘积，由以下公式给出：

N_instances = MAX(EKF2_MULTI_IMU , 1) x MAX(EKF2_MULTI_MAG , 1)

例如，一个带有 2 个IMU和 2 个磁强计的自动化驾驶仪可以在 EKF2_MULTI_IMU = 2 和 EKF2_MULTI_MAG = 2 的情况下运行，总共 4 个EKF实例，其中每个实例使用以下传感器组合：

  • 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

可处理的IMU或磁强计传感器的最大数量为每种传感器有4个，因此理论上最大有 4 x 4 = 16 个EKF实例。 实际上，这种做法受到现有计算资源的限制。 在开发这一功能的过程中，使用基于STM32F7的硬件的CPU进行测试，结果显示 4 个EKF实例具有可接受的处理负载和内存利用率裕度。

:::警告 应在飞行前进行地面测试以检查 CPU 和内存使用。 :::

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.

多EKF实例的设置由以下参数控制：

  • SENS_IMU_MODE: Set to 0 if running multiple EKF instances with IMU sensor diversity, ie EKF2_MULTI_IMU > 1.

    当设置为 1 (单个EKF 操作默认值)时，传感器模块选择EKF使用的IMU 数据。 这种保护可防止来自传感器的数据丢失，但并不能防止不良的传感器数据。 当设置为 0 时，传感器模块不进行选择。

  • SENS_MAG_MODE: Set to 0 if running multiple EKF instances with magnetometer sensor diversity, ie EKF2_MULTI_MAG > 1.

    当设置为 1 (单个EKF 操作默认值)时，传感器模块选择EKF使用的磁强计数据。 这种保护可防止来自传感器的数据丢失，但并不能防止不良的传感器数据。 当设置为 0 时，传感器模块不进行选择。

  • EKF2_MULTI_IMU: 此参数指定了多个EKF实例使用的 IMU 传感器数量。 如果 EKF2_MULTI_IMU <= 1, 那么只会使用第一个IMU 传感器。 当 SENS_IMU_MODE = 1, 这将是传感器模块选择的传感器。 如果 EKF2_MULTI_IMU >= 2 ，则一个单独的EKF实例将运行于指定数量的IMU传感器，最多不超过 4 个或目前的IMU数量。

  • EKF2_MULTI_MAG: 此参数指定了多个EKF实例使用的磁强计传感器数量。 如果 EKF2_MULTI_MAG <= 1 ，则只会使用第一个磁强计传感器。 当 SENS_MAG_MODE = 1 ，这将是传感器模块选择的传感器。 如果 EKF2_MULTI_MAG >= 2 ，则一个单独的EKF实例将运行于指定数量的磁强计传感器，最多不超过 4 个或目前的磁强计数量。

::: 注释 不支持多个EKF实例的飞行日志的录制和 EKF2 replay 。 若要启用 EKF 重播记录，你必须设置参数以启用 single EKF instance 。 :::

# 它使用什么传感器测量值？

EKF 具有不同的操作模式，以允许不同的传感器测量组合。 滤波器在启动时会检查传感器的最小可行组合，并且在完成初始倾斜，偏航和高度对准之后，进入提供旋转，垂直速度，垂直位置，IMU 增量角度偏差和 IMU 增量速度偏差估计的模式。

此模式需要 IMU 数据，一个偏航源（磁力计或外部视觉）和一个高度数据源。 所有EKF操作模式都需要这个最小数据集。 在此基础上可以使用其它传感器数据来估计额外的状态变量。

# IMU

  • 三轴机体固连惯性测量单元，以最小100Hz的频率获取增量角度和增量速度数据 。 注意：在 EKF 使用它们之前，应该使用圆锥校正算法校正 IMU 增量角度数据。

# 磁力计

需要以最小 5Hz 的速率的三轴机体固连磁力计数据（或外部视觉系统姿势数据）。 磁力计数据可以用于两种方式：

  • 使用倾角估计和磁偏角将磁力计测量值转换为偏航角。 然后将该偏航角用作 EKF 的观测值。 该方法精度较低并且不允许学习机体坐标系场偏移，但是它对于磁场异常和大的初置陀螺偏差更有鲁棒性。 它是启动期间和在地面时的默认方法。
  • XYZ 磁力计读数用作单独的观测值。 该方法更精确并且允许学习机体坐标系场偏移，但是它假设地球磁场环境只会缓慢变化，并且当存在显着的外部磁场异常时表现较差。

用于选择这些模式的逻辑由 EKF2_MAG_TYPE 参数设置。

该选项可以在没有磁力计的情况下运行。 或者使用 yaw from a dual antenna GPS 来替换磁力计，或者使用IMU测量数据和GPS速度数据从飞行器运动中估计偏航 estimate yaw from vehicle movement。

# 高度

高度数据源 - 来自 GPS，气压计，测距仪或外部视觉设备，需要最小频率为 5Hz。

:::注释 高度数据的主要来源由 EKF2_HGT_MODE 参数控制。 :::

如果不存在这些测量值，EKF 将无法启动。 当检测到这些测量值时，EKF 将初始化状态并完成倾角和偏航对准。 当倾角和偏航对齐完成后，EKF 可以转换到其它操作模式，从而可以使用其它传感器数据：

# 静态气压位置误差校正

气压表示的海拔高度因机体风的相对速度和方向造成的空气动力扰动而发生误差。 This is known in aeronautics as static pressure position error. 使用ECL/EKF2估计器库的EKF2模块提供了补偿这些误差的方法，只要风速状态估计是激活的。

对于固定翼式模式运行平台， 风速状态估计需要启用 Airspeage 或 Synthetic Sideslip 聚合。

对于多旋翼飞行器，可以启用并调整 Drag Specific Forces 的融合，以提供所需风速状态估计。

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.

A good tuning is obtained as follows:

  1. Fly once in Position mode repeatedly forwards/backwards/left/right/up/down between rest and maximum speed (best results are obtained when this testing is conducted in still conditions).
  2. Extract the .ulg log file using, for example, QGroundControl: Analyze > Log Download (opens new window) :::note The same log file can be used to tune the multirotor wind estimator. :::
  3. Use the log with the baro_static_pressure_compensation_tuning.py (opens new window) Python script to obtain the optimal set of parameters.

Tuning parameters:

# Barometer bias compensation

A barometer at a constant altitude is subject to drift in its measurements due to changes in the ambient pressure environment or variations of the sensor temperature. To compensate for this measurement error, EKF2 estimates the bias using GNSS height (if available) a "non drifting" reference. No tuning is required.

# GPS

# 位置和速度测量

GPS measurements will be used for position and velocity if the following conditions are met:

  • 通过设置 EKF2_AID_MASK 参数启用 GPS 的使用。
  • GPS 信号质量检查已通过。 这些检查由 EKF2_GPS_CHECK 和 EKF2_REQ_* 参数控制。
  • 通过设置 EKF2_HGT_MODE 参数，EKF 可以直接使用 GPS 高度。

# 偏航角测量

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 theweight fields.

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:

  • 验证第二接收器的数据是否存在。 This will be logged as vehicle_gps_position_1 and can also be checked when connected via the nsh console using the command listener vehicle_gps_position -i 1. GPS_2_CONFIG 参数需要被正确设置。
  • 检查每个接收器的 s_variance_m_s ， eph 和 epv 数据，并决定可以使用哪些精确度指标。 如果两个接收器都输出给传感器 s_variance_m_s 和 eph 数据，并且 GPS 垂直位置不直接用于导航，则建议设置 SENS_GPS_MASK 为 3。 在只有 eph 数据且两个接收器都不输出 s_variance_m_s 数据时，设置 SENS_GPS_MASK 为 2。 只有在 GPS 被选定为主高度数据源，并具有 EKF2_HGT_MODE 参数，并且两个接收器都输出有意义的 epv 数据的情况下，第 2 位才会被设置。
  • 混合接收器数据的输出被记录为 ekf_gps_position，并且当通过 nsh 终端连接时，可以通过命令 listener ekf_gps_position 进行检查。
  • 如果接收器以不同的频率输出, 混合输出的频率将是较低的接收器的频率。 在可能的情况下，接收器应配置为同样的输出频率。

# 全球导航卫星系统性能要求

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 米/秒 根据所报告的全球导航卫星系统高度计算出的漂移率（在固定时）。
hspd < 0.1 (EKF2_REQ_HDRIFT) 0.01 米/秒 所报告的全球导航卫星系统横向速度的筛选星等。
vspd < 0.2 (EKF2_REQ_VDRIFT) 0.02 米/秒 所报告的全球导航卫星系统垂直速度的滤波量级。

注解

The hpos_drift_rate, vpos_drift_rate and 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.

# Range Finder Obstruction Detection

The EKF can detect whether the rangefinder path-to-ground is obstructed (perhaps by a payload) using a kinematic consistency check between the vertical velocity estimate and the numerical derivative of the range finder data. If the range finder is statistically inconsistent with EKF2, the sensor is rejected for the rest of the flight unless the statistical test passes again for at least 1 second at a vertical speed of 0.5m/s or more.

The check is only enabled when the rangefinder is not used as the primary height source, and is only active while the vehicle is not moving horizontally (as it assumes a static ground height).

For effective obstruction detection, the range finder noise parameter needs to be tightly tuned using flight data. The kinematic consistency gate parameter can then be adjusted to obtain the desired fault detection sensitivity.

Tuning parameters:

# 空速

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 accelerometer measurements) along the X and Y body axes is controlled by the EKF2_BCOEF_X, EKF2_BCOEF_Y and EKF2_MCOEF parameters which set the ballistic coefficients for flight in the X and Y directions, and the momentum drag produced by the propellers, respectively. The amount of specific force observation noise is set by the EKF2_DRAG_NOISE parameter.

A good tuning is obtained as follows:

  1. Fly once in Position mode repeatedly forwards/backwards/left/right/up/down between rest and maximum speed (best results are obtained when this testing is conducted in still conditions).
  2. Extract the .ulg log file using, for example, QGroundControl: Analyze > Log Download (opens new window) :::note The same .ulg log file can also be used to tune the static pressure position error coefficients. :::
  3. Use the log with the mc_wind_estimator_tuning.py (opens new window) Python script to obtain the optimal set of parameters.

# 光流

Optical flow data will be used if the following conditions are met:

  • 有效的测距仪数据可用。
  • 在 EKF2_AID_MASK 参数中的第 1 位为真。
  • 光流传感器返回的质量度量值大于 EKF2_OF_QMIN 参数设置的最低要求。

# 外部视觉系统

Position, velocity or orientation measurements from an external vision system, e.g. Vicon, can be used:

  • 如果 EKF2_AID_MASK 参数中的第 3 位为真，则将使用外部视觉系统的水平位置数据。
  • 如果EKF2_HGT_MODE参数设置为3，则将使用外部视觉系统垂直位置数据。
  • 如果 EKF2_AID_MASK 参数中的第 8 位设置为真，将使用外部视觉系统速度数据。
  • 如果 EKF2_AID_MASK 参数中的第 4 位为真，则外部视觉系统姿态数据将用于偏航估计。
  • 如果 EKF2_AID_MASK 参数中的第 6 位为真，则外部视觉参考帧偏移将被估计并用于旋转外部视觉系统数据。

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.

EKF_AID_MASK 值 设置位 描述
321 GPS + EV_VEL + ROTATE_EV Heading w.r.t. North (Recommended)
73 GPS + EV_POS + ROTATE_EV Heading w.r.t. North (Not recommended, use EV_VEL instead)
24 EV_POS + EV_YAW Heading w.r.t. external vision frame
72 EV_POS + ROTATE_EV Heading w.r.t. North
272 EV_VEL + EV_YAW Heading w.r.t. external vision frame
320 EV_VEL + ROTATE_EV Heading w.r.t. North
280 EV_POS + EV_VEL + EV_YAW Heading w.r.t. external vision frame
328 EV_POS + EV_VEL + ROTATE_EV Heading w.r.t. North

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.

# 我如何启用 'ecl' 库中的 EKF ？

Set the SYS_MC_EST_GROUP parameter to 2 to use the ecl EKF.

# ecl EKF 和其它估计器相比的优点和缺点是什么？

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 attitude_estimator_q + local_position_estimator have been made and the best choice of estimator will depend on the application and tuning.

# 缺点

  • ecl EKF 是一种复杂的算法，需要很好地理解扩展卡尔曼滤波器理论及其应用于导航中的问题才能成功调参。 因此，不知道怎么修改，用户就很难得到好结果。
  • ecl EKF 使用更多 RAM 和闪存空间。
  • ecl EKF 使用更多的日志空间。

# 优点

  • ecl EKF 能够以数学上一致的方式融合来自具有不同时间延迟和数据速率的传感器的数据，一旦正确设置时间延迟参数，就可以提高动态操作期间的准确性。
  • ecl EKF 能够融合各种不同的传感器类型。
  • 当 ecl EKF 检测并报告传感器数据中统计上显着的不一致性，将帮助诊断传感器错误。
  • 对于固定翼飞机的操作，ecl EKF 可以使用或不使用空速传感器估计风速，并且能够将估计的风速与空速测量和侧滑假设结合使用，以延长 GPS 在飞行中丢失时的航位推算时间。
  • ecl EKF估计3轴加速度计偏差，这提高了尾座式无人机和其它机体在飞行阶段之间经历大的姿态变化时的精度。
  • 联邦结构（组合姿态和位置/速度估计）意味着姿态估计受益于所有传感器测量。 如果调参正确，这应该提供改善态度估计的潜力。

# 如何检查 EKF 性能？

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 <log_file>.pdf.

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.

# 输出数据

# 状态

Refer to states[32] in estimator_status (opens new window). The index map for states[32] 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[28] in estimator_status (opens new window). The index map for covariances[28] 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

# 观测新息和新息方差

The observation estimator_innovations, estimator_innovation_variances, and 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:

  • 磁力计 XYZ (gauss, gauss^2) : mag_field[3]
  • 偏航角度 (rad, rad^2) : heading
  • 真实空速 (m/s, (m/s)^2) : airspeed
  • 合成侧滑 (rad, rad^2) : beta
  • 光流 XY (rad/sec, (rad/s)^2) : flow
  • 距地高度 (m, m^2) : hagl
  • 阻力比力 ((m/s)^2): drag
  • 速度和位置新息：每个传感器

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[2] 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[2] 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[2] 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[2] 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[2] 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[2] 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[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
float32    aux_vvel # vertical auxiliary 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[3] in the ekf2_innovations message.

The index map is as follows:

  • [0] 角度跟踪误差量级 (rad)
  • [1] 速度跟踪误差量级（m/s）。 速度跟踪时间常量可以使用 EKF2_TAU_VEL 参数进行调整。 减小此参数可减少稳态误差，但会增加 NED 速度输出上的观测噪声量。
  • [2] 位置跟踪误差量级 (m)。 位置跟踪时间常量可以使用 EKF2_TAU_POS 参数进行调整。 减小此参数可减少稳态误差，但会增加 NED 位置输出上的观测噪声量。

# 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:

  • 数据丢失。 一个例子是测距仪无法提供返回数据。
  • 新息，即状态预测和传感器观测之间的差异过度。 这种情况的一个例子是过度振动导致大的垂直位置误差，导致气压计高度测量被拒绝。

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: 最大磁强计新息组分与新息测试极限的比率
  • vel_test_ratio: 最大速度新息组分与新息测试极限的比率
  • pos_test_ratio: 最大水平位置新息组分与新息测试极限的比率
  • hgt_test_ratio: 垂直位置新息与新息测试极限的比率
  • tas_test_ratio: 真空速新息与新息测试极限的比率
  • hagl_test_ratio: 距地高度新息与新息测试极限的比率

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. 这些检查由 EKF2_GPS_CHECK 和 EKF2_REQ_* 参数控制。 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:

  • 如果新息方差小于观测方差（这需要一个不可能的负值状态方差）或协方差更新将为任何一个状态产生负值方差，那么：
  • 状态方差（协方差矩阵中的对角线）被限制为非负。
  • 状态方差应用数值上限。
  • 协方差矩阵强制对称。

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.

# 如果高度估计值发散了怎么办?

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

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:

  • 将主要的高度传感器的新息门槛的值加倍。 如果使用气压高度，则设置 EKF2_BARO_GATE。
  • 初始化时将 EKF2_ACC_NOISE 的值增加到 0.5。 如果仍然出现发散，则进一步增加 0.1，但不要超过 1.0。

Note that the effect of these changes will make the EKF more sensitive to errors in GPS vertical velocity and barometric pressure.

# 如果位置估计发散了应该怎么办?

The most common causes of position divergence are:

  • 高振动级别。
    • 通过改进无人机的机械隔离来解决。
    • 增加 EKF2_ACC_NOISE 和 EKF2_GYR_NOISE 的值会有所帮助，但确实会使 EKF 更容易受到 GPS 故障的影响。
  • 过大的陀螺仪偏差偏移。
    • 通过重新校准陀螺仪来修复。 检查过度温度灵敏度(> 3 deg/sec 偏差在从冷机开始热启动时发生变化，如果受隔热影响以减缓温度变化的速度，则替换传感器。
  • 不好的偏航对齐
    • 检查磁力计校准和对齐。
    • 检查显示的航向 QGC 是否在 15 度以内
  • GPS 精度差
    • 检查是否有干扰
    • 改善隔离和屏蔽
    • 检查飞行位置是否有 GPS 信号障碍物和反射器（附近的高层建筑）
  • GPS 数据丢失

Determining which of these is the primary cause requires a methodical approach to analysis of the EKF log data:

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:

Position, Velocity, Height and Magnetometer Test Ratios

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.

Vibration metrics - successful

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.

GPS glitch - test ratios

This is accompanied with rise in the GPS receivers reported velocity accuracy which indicates that it was likely a GPS error.

GPS Glitch - reported receiver accuracy

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 Glitch - velocity innovations

# 确定 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[5] 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.

GPS Data Loss - in SITL

# 气压计地面效应补偿

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.

Barometer ground effect

You can enable ground effect compensation to fix this problem:

  • 从绘图中估算出气压计在起飞或着陆期间的跌落程度。 在上面的绘图中，人们可以看到降落过程中大约6米的气压计下沉。
  • 然后将参数 EKF2_GND_EFF_DZ 设置为该值，并添加 10% 的余量值。 因此，在这种情况下，6.6米的数值将是一个良好的起点。

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.

# 更多信息：

← 飞行终止配置 →