ArduPilot开源代码之EKF2

1. 源由

EKF2是在代码AP_NavEKF2中实现。

为此，研究下相关代码实现，其算法流程应该与《ArduPilot开源代码之EKF1》类似。

2. EKF2简介

AP_NavEKF2库中的一个24状态扩展卡尔曼滤波器，它估计以下状态：

• Attitude (Quaternions)
• Velocity (North,East,Down)
• Position (North,East,Down)
• Gyro bias offsets (X,Y,Z)
• Gyro scale factors (X,Y,Z)
• Z accel bias
• Earth magnetic field (North,East,Down)
• Body magnetic field (X,Y,Z)
• Wind Velocity (North,East)

3. 特性&优点

• 可以为每个IMU运行一个单独的EKF2，使得从IMU故障中恢复的可能性大大增加。
• 可以在磁力计出现故障时进行切换。
• 可以估计陀螺仪的缩放因子，这可以在高速机动时提高精度。
• 可以在启动时同时估计陀螺仪偏移和方向，而不依赖于DCM算法进行初始定位。这使得它非常适合在陀螺仪未校准的情况下从移动平台起飞。
• 可以在飞行中处理较大的陀螺仪偏置变化。
• 能够更快地从错误的传感器数据中恢复。
• 提供了稍微更平滑的输出。
• 稍微更准确。
• 使用的计算能力稍微少一些。
• 在通过检查后开始使用GPS，而不是等待飞行器电机启动。

4. 优化要点

1. 不直接尝试估计四元数的方向，而是估计一个误差旋转向量并将校正应用于惯性导航方程中的四元数。当存在较大的角度误差时，这种方法更好，因为它避免了在大角度变化时线性化四元数所带来的误差。

“Rotation Vector in Attitude Estimation”, Mark E. Pittelkau, Journal of Guidance, Control, and Dynamics, 2003

2. EKF2在延迟的时间轴上运行，因此当融合测量值时，它们使用的是来自同一时间点的测量值、滤波器状态和协方差矩阵。传统的EKF使用一种更简单的方法，其中延迟的测量值与延迟的状态融合，但协方差矩阵是当前时间的，这会降低准确性。

延迟的滤波器状态会通过一个互补滤波器预测到当前的时间轴上，该滤波器不仅消除了时间延迟，还过滤掉了在融合测量值时发生的状态突然变化。
“Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements,” A Khosravian, J Trumpf, R Mahony, T Hamel - American Control Conference, vol.-, 2015

3. 优化了数学和代码以提高速度。

在计算上比在每次更新时使用缓冲的IMU数据向前推进状态要高效得多，但精度稍差一些。传统的EKF通过在到下一个测量值的时间内逐步应用状态校正来平滑状态校正，这降低了滤波器的稳定性。

4. 增加了一种带有固定磁偏角的简单罗盘偏航角融合方法，该方法在地面或磁干扰阻止使用更准确的三轴六状态磁力计融合技术时使用。

5. 配置EKF2

1. 设置 EK2_ENABLE = 1 来启用新的 EKF。此时 EKF2 将并行运行并进行记录等操作，但它不会被控制回路使用。
2. 在控制回路中使用它，请将 AHRS_EKF_TYPE 设置为 2。
3. 多个 IMU 的使用由 EK2_IMU_MASK 参数控制：

• 仅使用 IMU1，请将 EK2_IMU_MASK 设置为 1（这是默认值）
• 仅使用 IMU2，请将 EK2_IMU_MASK 设置为 2
• 要使用 IMU1 和 IMU2 运行双 EKF2，请将 EK2_IMU_MASK 设置为 3

4. 设置参数后，需要重启。

6. 算法参数

6.1 EK2 IMU

6.1.1 EK2_ENABLE

此参数用于开启和关闭 EK2 2。

• 设置为 1 以开启
• 设置为 0 以关闭

注：开启 EK22 仅意味着计算将运行，但并不意味着它将用于飞行控制。要将其用于飞行控制，请设置 AHRS_EK2_TYPE=2。更改 EK2_ENABLE 参数的值后，需要进行重启或重新启动以使其生效。

6.1.2 EK2_GYRO_PNOISE

此控制干扰噪声控制因陀螺仪测量误差（不包括偏差）引起的估计误差的增长。增加它会使滤波器对陀螺仪测量的信任度降低，而对其他测量的信任度增加。单位为弧度/秒。

6.1.3 EK2_ACC_PNOISE

此控制干扰噪声控制因加速度计测量误差（不包括偏差）引起的估计误差的增长。增加它会使滤波器对加速度计测量的信任度降低，而对其他测量的信任度增加。单位为米/秒²。

6.1.4 EK2_GBIAS_PNOISE

此状态过程噪声控制因陀螺仪角增量偏差状态误差估计的增长。增加它会使陀螺仪偏差估计更快但更不稳定。单位为弧度/秒。

6.1.5 EK22_GSCL_PNOISE

此状态过程噪声控制陀螺仪刻度因子学习的速率。增加它会使陀螺仪刻度因子估计更快但更不稳定。

6.1.6 EK2_ABIAS_PNOISE

此状态过程噪声控制垂直加速度计增量速度偏差状态误差估计的增长。增加它会使加速度计偏差估计更快但更不稳定。单位为米/秒²。

6.2 EK2 GPS

6.2.1 EK2_GPS_TYPE

此参数控制 GPS 测量的使用方式：

• 0 = 使用 3D 速度和 2D 位置
• 1 = 使用 2D 速度和 2D 位置
• 2 = 使用 2D 位置
• 3 = 不使用 GPS（如果可用，将使用光流）

6.2.2 EK2_VELNE_NOISE

此参数设置 GPS 接收器报告的速度精度的下限，用于设置水平速度观测噪声。

注：如果所使用的接收器型号未提供速度精度估计，则将使用此参数值。增加该参数值会降低 GPS 水平速度测量的权重。单位为米/秒。

6.2.3 EK2_VELD_NOISE

此参数设置 GPS 接收器报告的速度精度的下限，用于设置垂直速度观测噪声。

注：如果所使用的接收器型号未提供速度精度估计，则将使用此参数值。增加该参数值会降低 GPS 垂直速度测量的权重。单位为米/秒。

6.2.4 EK2_VEL_GATE

此参数设置应用于 GPS 速度测量创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.2.5 EK2_POSNE_NOISE

此参数设置 GPS 水平位置观测噪声。增加它会降低 GPS 水平位置测量的权重。单位为米。

6.2.6 EK2_POS_GATE

此参数设置应用于 GPS 位置测量创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.2.7 EK2_GLITCH_RAD

此参数控制滤波器预测值与 GPS 测量值之间的最大径向不确定性，当超过此值时，滤波器位置和速度状态将重置为 GPS。

增加此值允许滤波器忽略更大的 GPS 干扰，但也意味着非 GPS 错误（如 IMU 和罗盘）可能在滤波器强制返回 GPS 位置之前产生更大的位置误差。单位为米。

6.2.8 EK2_GPS_DELAY

此参数为 GPS 测量滞后于惯性测量的毫秒数。滤波器可以补偿的最大延迟为 250 毫秒。

6.3 EK2 Height

6.3.1 EK2_ALT_SOURCE

此参数控制 EK2 使用哪个高度传感器。如果所选选项不可用，将默认使用气压计作为主要高度源。

• 设置 0 将始终使用气压高度。
• 设置 1 使用测距仪，仅在与光流导航（EK2_GPS_TYPE = 3）结合使用时可用。
• 设置 2 使用 GPS。当使用气压计以外的高度源时，气压高度和 EK2 高度估计之间的偏移量会不断更新。

如果滤波器运行时需要切换到气压高度，则气压高度将根据学习到的偏移进行校正，以防止高度估计出现突然变化。

6.3.2 EK2_ALT_NOISE

此参数为高度测量的 RMS 噪声值。增加它会降低气压测量的权重，使滤波器对气压测量误差反应更慢，但对 GPS 和加速度计误差更敏感。单位为米。

6.3.3 EK2_HGT_I_GATE

此参数设置应用于高度测量创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.3.4 EK2_HGT_DELAY

此参数为高度测量滞后于惯性测量的毫秒数。滤波器可以补偿的最大延迟为 250 毫秒。

6.4 EK2 Flow

6.4.1 EK2_MAX_FLOW

此参数设置滤波器接受的最大光流速率的量值。单位为弧度/秒。

6.4.2 EK2_FLOW_NOISE

此参数为光流测量中的噪声和误差的 RMS 值。增加它会降低这些测量的权重。单位为弧度/秒。

6.4.3 EK2_FLOW_GATE

此参数设置应用于光流创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.4.4 EK2_FLOW_DELAY

此参数为光流测量滞后于惯性测量的毫秒数。这是从光流平均周期结束到滤波器更新之间的时间，不包括光流传感器内 100 毫秒的平均延迟时间。

6.5 EK2 Mag

6.5.1 EK2_MAG_NOISE

此参数为磁力计测量的 RMS 噪声值。增加它会降低这些测量的权重。单位为 mGauss。

6.5.2 EK2_MAG_CAL

此参数决定滤波器何时使用估算地球和机体固定磁场状态的三轴磁力计融合模型。此模型仅适用于外部磁场环境稳定的情况。

• EK2_MAG_CAL = 0 在飞行时启用校准，这是固定翼用户的默认设置。
• EK2_MAG_CAL = 1 在机动时启用校准。
• EK2_MAG_CAL = 2 无论飞行情况如何，均不进行磁力计校准，建议在外部磁场变化时使用，这是地面车辆的默认设置。
• EK2_MAG_CAL = 3 在完成首次空中场和偏航重置后启用校准，这是多旋翼的默认设置。
• EK2_MAG_CAL = 4 始终启用校准。

6.5.3 EK2_MAG_GATE

此参数设置应用于磁力计测量创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.5.4 EK2_MAG_PNOISE

此状态过程噪声控制磁场状态误差估计的增长。增加它会使磁场偏差估计更快但更不稳定。单位为高斯/秒。

6.6 EK2 Velocity

6.6.1 EK2_EAS_NOISE

此参数为飞机使用的等效空速测量的 RMS 噪声值。
增加它会降低空速测量的权重，使风速估计更不稳定且收敛速度更慢。
增加它还会在无 GPS 测量的情况下增加导航误差。单位为米/秒。

6.6.2 EK2_EAS_GATE

此参数设置应用于空速测量创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.7 EK2 Position

6.7.1 EK2_RNG_NOISE

此参数为测距仪测量的 RMS 噪声值。增加它会降低该测量的权重。单位为米。

6.7.2 EK2_RNG_GATE

此参数设置应用于测距仪创新一致性检查的标准差数量。

• 减少它会增加好测量被拒绝的可能性。
• 增加它会增加坏测量被接受的可能性。

6.8 EK2 Wind

6.8.1 EK2_WIND_PNOISE

此状态过程噪声控制风状态误差估计的增长。增加它会使风估计更快但更不稳定。单位为米/秒²。

6.8.2 EK2_WIND_PSCALE

此参数控制在高度变化时，风状态过程噪声的增加量，以考虑到随高度变化的风速和风向变化。增加此参数会使风状态在高度变化时适

7. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读