EKF2 估算系统

什么是 EKF2 估算系统？

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

• 态度（四元数）

• 速度（北、东、南）

• 位置（北、东、南）

• 陀螺仪偏置偏差（X、Y、Z）

• 陀螺仪比例系数（X、Y、Z）

• Z 加速偏置

• 地球磁场（北、东、南）

• 人体磁场（X、Y、Z）

• 风速（北、东）

它是基于滤波方程得出的 这里.

EKF2 的优势

• 它可以为每个 IMU 运行单独的 EKF2，使从 IMU 故障中恢复的可能性大大提高

• 如果出现故障，它可以在磁强计之间进行切换

• 它可以估算陀螺仪的比例因子，从而提高高速机动时的精度。

• 在移动过程中，它可以在启动时同时估算陀螺仪偏移和方位，并且不依赖 DCM 算法来确定初始方位。因此，当陀螺仪尚未校准时，它是在移动平台上飞行的理想选择。

• 它可以处理飞行中陀螺仪偏置的较大变化

• 它能更快地从不良传感器数据中恢复过来

• 它的输出稍稍平稳一些。

• 准确性稍高

• 计算能力略低

• 它在检查通过时开始使用 GPS，而不是等待载具电机启动。

它是如何做到这一点的呢？

1. 它不直接估算四元数的方向，而是估算一个误差旋转矢量，并根据惯性导航方程对四元数进行修正。当角度误差较大时，这种方法会更好，因为它可以避免在大角度变化时与四元数线性化相关的误差。

   • 有关这种方法的详情，请参阅 "姿态估计中的旋转矢量"，Mark E. Pittelkau，《制导、控制和动力学杂志》，2003 年。

2. 新的 EKF 在延迟时间范围内运行，因此在融合测量结果时，使用的是同一时间点的测量结果、滤波器状态和协方差矩阵。传统的 EKF 采用了一种更简单的方法，即使用延迟状态融合延迟测量，但协方差矩阵是当前时间的，这就降低了精度。

   • 然后，利用一个互补滤波器将延迟滤波状态预测到当前时间范围内，该滤波器可消除时间延迟，并滤除测量融合时出现的状态突变。

   • 这种方法受到澳大利亚国立大学的 Ali Khosravian 所做的输出预测器工作的启发。"多速率和多延迟矢量测量情况下的递归姿态估计》，A Khosravian、J Trumpf、R Mahony、T Hamel - American Control Conference，vol.- 2015。

   • 这比每次更新时使用缓冲 IMU 数据向前卷绕状态的计算成本要低得多，但精确度略低。传统的 EKF 通过在下一次测量前增量应用状态修正来平滑状态修正，从而降低了滤波器的稳定性。

3. 还对数学和代码进行了进一步优化，以提高速度。

4. 还增加了一种具有固定偏角的简单罗盘偏航角融合方法，可在地面上使用，或在磁场干扰阻碍使用更精确的三轴六态磁强计融合技术时使用。

使用 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. 设置参数后，您需要重新启动。

EKF2 日志数据

EKF2 的数据可在 NKF1 至 NKF5 日志数据包中找到。

数据包 NKF1 至 NKF4 包含每个 EKF 实例的信息，实例编号由字段 PI 的值给出。启用的实例数由 EK2_IMU_MASK 参数设置。数据包 NKF5 包含主要用于飞行控制的 EKF 实例的光流信息。

下面列出了可用的 EKF2 日志数据。还包括一些显示飞行数据示例的图表。这些数据是在 3DR Iris+ 四旋翼飞行器上使用 Pixhawk 记录的，参数变化如下，然后重新启动：

• EK2_ENABLE = 1（开启 EKF2）

• EK2_IMU_MASK = 3（指示 EKF2 运行两个实例，一个用于 IMU1 (MPU6000)，另一个用于 IMU2 (LDG20H + LSM303D)

• AHRS_EKF_TYPE = 2（告诉飞行控制系统使用 EKF2

• LOG_BITMASK = 131071（从启动开始记录 50Hz 数据）

*注：左键单击地块可查看全尺寸地块*。

滤波状态估计值

NKF1[0] （和 NKF1[1] 如果使用第二个 IMU）包含飞行控制系统使用的输出信号

• TimeUS - 时间戳（uSec）

• 左平移/横滚、前平移/俯仰 - 欧拉滚转角和俯仰角（度）

  

• 偏航/航向 - 欧拉偏航角（度）

  

• VN,VE - 东、北方向速度（米/秒）

  

• VD, dPD - 下行速度和位置导数（米/秒）

  

• PN,PE,PD - 北、东、下的位置（米）

  

• GX,GY,GZ - X、Y、Z 速率陀螺仪偏置（度/秒）

  

NKF2[0] （和 NKF2[1] 如果使用第二个 IMU）包含附加状态信息

• TimeUS - 时间戳（uSec）

• AZbias - Z 加速计偏置（厘米/秒/秒）

  

• GSX,GSY,GSZ - X、Y、Z 速率陀螺仪比例因子（%）。

  例如，0.5 的对数值相当于该传感器的比例因子为 1.005

  

• VWN,VWE - 北、东风速（米/秒）

• MN,ME,MD - 地球磁场北、东、南（毫高斯）

  

• MX,MY,MZ - 机身固定磁场 X、Y、Z（毫高斯）

  

• MI - EKF2 使用的磁强计索引

过滤器创新

NKF3[0] （和 NKF3[1] 如果使用第二个 IMU）包含滤波器创新值的信息。创新值是 EKF2 预测的测量值与传感器返回值之间的差值。创新值越小，表示传感器误差越小。由于 IMU 数据用于进行预测，因此 IMU 数据不好可能导致所有测量值的创新值都很大。

• TimeUS - 时间戳（uSec）

• IVN,IVE - 全球定位系统向北、向东的速度变化（米/秒）

  

• IVD - GPS 速度革新向下（米/秒）

  

• IPN,IPE - GPS 位置创新，北、东（米）

  

• IPD - 气压计位置创新向下（米）

  

• IMX,IMY,IMZ - 磁强计创新 X、Y、Z（mGauss）

  

• IYAW - 指南针偏航创新（度）

  

• IVT - 真实空速创新（米/秒）

过滤器健康和状态

NKF4[0] （和 NKF4[1] 如果使用第二个 IMU）包含创新方差检验比率的信息。数值小于 1 表示该测量值已通过检查，并被 EKF2 使用。如果数值大于 1，则表明该测量值的创新值过高，EKF2 将拒绝接受来自该传感器的数据。飞行中的值小于 0.3 是传感器数据质量良好的典型设置。

它们还包含与过滤器健康有关的其他信息

• TimeUS - 时间戳（uSec）

• SV - GPS 速度测试比率

• SP - GPS 定位测试比率

• SH - 气压测试比率

• SM - 磁强计测试比率

• SVT - 空速传感器 测试比率

  

• errRP - 估计姿态滚动/俯仰误差（rad）

  

• OFN - 因上次重置滤波状态而向北跳变的位置（米）

• OFE - 由于上次重置滤波状态而导致的位置东移（米）

• FS - 整数位屏蔽滤波器数值故障

• TS - 过滤器测量超时的整数位掩码

• SS - 过滤器解决方案状态的整数位掩码

• GPS - 滤波 GPS 质量检查的整数位掩码

• PI - 显示选择哪个 EKF2 实例进行飞行控制的索引

光流与测距仪融合

NKF5 包含用于飞行控制的 EK2 实例的光流融合信息

• TimeUS - 时间戳（uSec）

• normInnov - 光流创新方差检验比

• FIX,FIY - 光流 X 轴和 Y 轴创新值（mrad/s）

  

• AFI - 光流地形高度估算器创新（mrad/s）

• HAGL - 估计地面以上高度（米）

• meaRng - 测距仪测量的距离（米）

  

• 偏移量 - 相对于压力高度原点的估计地形偏移量

  

• RI - 测距仪创新（米）

  

• errHAGL - 地形高度偏移估计值的 1Σ 不确定性（米）

  

飞行稳定调整参数

对 EKF2 参数进行了调整，以便在精度和对传感器误差的鲁棒性之间取得折衷。

如果您有关于调整申报器的问题，请发帖询问 在论坛上 并在帖子标题中提及 EKF2 一词。

新 EKF 的参数以 EK2_ 开头，具体如下

EK2_ENABLE

用于打开或关闭 EKF 2。设置为 1 时开启，设置为 0 时关闭。打开 EKF2 仅使计算运行，并不意味着它将用于飞行控制。要将其用于飞行控制，请设置 AHRS_EKF_TYPE=2。更改 EK2_ENABLE 的值后需要重启或重新启动才能生效。

EK2_GPS_TYPE

这可以控制 GPS 测量值的使用：

• 0 = 使用 3D 速度示例；2D 位置示例

• 1 = 使用二维速度和二维位置

• 2 = 使用 2D 定位

• 3 = 不使用 GPS（如果有光流，则使用光流）。

ek2_velne_noise

这为 GPS 接收机报告的速度精度设置了一个下限，用于设置水平速度观测噪声。如果所使用的接收机型号不能提供速度精度估计值，则将使用该参数值。增加参数值会降低 GPS 水平速度测量值的权重。单位为米/秒

EK2_VELD_NOISE

这将为 GPS 接收机报告的速度精度设定一个下限，用于设置实际速度观测噪声。如果所使用的接收机型号不能提供速度精度估计值，则将使用该参数值。增加该值会降低 GPS 垂直速度测量值的权重。单位为米/秒。

EK2_VEL_GATE

该值设定了应用于 GPS 速度测量创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被拒绝。增加标准偏差数则更有可能接受不良测量结果。

ek2_posne_noise

该参数设置 GPS 水平位置观测噪声。增大它可以降低 GPS 水平位置测量的权重。单位为米

EK2_POS_GATE

该值用于设置应用于 GPS 定位测量创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被拒绝。增加标准偏差数则更有可能接受不良测量结果。

EK2_GLITCH_RAD

它控制着滤波器预测值与 GPS 测量值之间的最大径向位置不确定性，然后滤波器的位置和速度状态才会重置为 GPS 位置。该值越大，滤波器就越能忽略较大的 GPS 误差，但也意味着 IMU 和罗盘等非 GPS 误差会在滤波器强制返回 GPS 位置之前造成较大的位置误差。它的单位是米。

EK2_GPS_DELAY

这是 GPS 测量值落后于惯性测量值的毫秒数。滤波器可补偿的最大延迟为 250 毫秒。

EK2_ALT_SOURCE

该参数控制 EKF 使用哪个高度传感器。如果所选选项无法使用，则默认将巴罗高度作为主要高度源。设置 0 将始终使用气压高度。设置 1 使用测距仪，仅在与光流导航（EK2_GPS_TYPE = 3）结合使用时可用。设置 2 使用 GPS。当使用 Baro 以外的高度源时，Baro 高度和 EKF 高度估计值之间的偏移量会不断更新。如果需要在滤波器运行时切换到 Baro 高度，则会根据所学偏移对 Baro 高度进行校正，以防止高度估计值出现骤降。

EK2_ALT_NOISE

这是高度测量中噪声的有效值。增加该值会降低气压测量的权重，使滤波器对气压测量误差的反应更慢，但对 GPS 和加速度计误差更敏感。单位为米。

EK2_HGT_I_GATE

该值设置了应用于高度测量创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被拒绝。增大该值则更有可能接受糟糕的测量结果。

EK2_HGT_DELAY

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

EK2_MAG_NOISE

这是磁强计测量中噪声的有效值。增加它可以降低这些测量值的权重。单位为 mGauss。

EK2_MAG_CAL

这将决定滤波器何时使用三轴磁力计融合模型，以估算地球和人体固定磁场状态。该模型只适合在外部磁场环境稳定时使用。

• EKF_MAG_CAL = 0 可以在空中进行校准，是平面用户的默认设置。

• EKF_MAG_CAL = 1 可在操纵时进行校准。

• EKF_MAG_CAL = 2 可以防止在任何飞行条件下进行磁力计校准，建议在外部磁场变化时使用，并且是漫游车的默认设置。

• EKF_MAG_CAL = 3 可在第一次空中场和偏航复位完成后进行校准，是旋翼飞行器的默认设置。

• EKF_MAG_CAL = 4 可始终进行校准。

EK2_MAG_GATE

该参数设置应用于磁强计测量创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被剔除。增大它则更有可能接受坏的测量结果。

EK2_EAS_NOISE

这是飞机使用的等效空速测量值的噪声均方根值。增加该值会降低空速测量值的权重，使风速估计值的噪声降低，收敛速度减慢。在没有 GPS 测量数据的情况下进行盲区定位时，增大该值也会增加导航误差。它的单位是米/秒。

EK2_EAS_GATE

该值设定了应用于空速测量创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被拒绝。增大该值则更有可能接受糟糕的测量结果。

EK2_RNG_NOISE

这是测距仪测量中噪声的有效值。增加该值会降低测量的权重。单位为米。

EK2_RNG_GATE

该参数设置了应用于测距仪创新一致性检查的标准偏差数。减少该值，好的测量结果就更有可能被拒绝。增大该值则会使坏的测量结果更有可能被接受。

EK2_MAX_FLOW

该参数设置滤波器可接受的最大光流速。单位为拉德/秒。

EK2_FLOW_NOISE

这是光流测量中噪声和误差的均方根值。增加该值可降低这些测量值的权重。单位为拉德/秒。

EK2_FLOW_GATE

该参数设置了应用于光流创新一致性检查的标准偏差数。减少标准差，好的测量结果就更有可能被拒绝。增大该值则更有可能接受糟糕的测量结果。

EK2_FLOW_DELAY

这是光学流量测量落后于惯性测量的毫秒数。这是光流量平均周期结束后的时间，不包括流量传感器内 100 毫秒平均所造成的时间延迟。

ek2_gyro_pnoise

这种控制干扰噪声可控制陀螺测量误差（不包括偏差）导致的估计误差增长。增加它可以使滤波器减少对陀螺仪测量值的信任，增加对其他测量值的信任。单位为拉德/秒。

EK2_ACC_PNOISE

这种控制干扰噪声可控制加速度计测量误差（不包括偏差）导致的估计误差增长。增大该值会降低滤波器对加速度计测量值的信任度，增加对其他测量值的信任度。单位为米/秒/秒。

ek2_gbias_pnoise

这种状态过程噪声控制着陀螺三角角偏置状态误差估计值的增长。增加该值会使陀螺偏差估计速度更快、噪音更大。单位为拉德/秒。

ekf2_gscl_pnoise

该状态过程噪声控制陀螺仪比例因子的学习速度。增加该值会使陀螺比例因子的估计速度更快，噪音更大。

ek2_abias_pnoise

这种状态过程噪声控制着垂直加速度计三角速度偏置状态误差估计值的增长。增加该值会使加速度计偏置估计更快、噪声更大。单位为米/秒/秒。

EK2_MAG_PNOISE

这种状态过程噪声控制着磁场状态误差估计值的增长。增加它可以使磁场偏差估计更快、噪声更大。单位为高斯/秒。

ek2_wind_pnoise

这种状态过程噪声控制着风状态误差估计值的增长。增加该值会使风速估算更快、噪声更大。单位为米/秒/秒。

ek2_wind_pscale

该参数控制在上升或下降高度时，风状态的过程噪声增加的程度，以考虑风速和风向随高度的变化。增大该参数可提高风状态在高度变化时的适应速度，但会使风速估算值更加嘈杂。

EK2_GPS_CHECK

这是一个 1 字节位图，用于控制 GPS 飞行前检查的执行。设置为 0 可绕过所有检查。设置为 255 则执行所有检查。设置为 3 时只检查卫星数量和 HDoP。设置为 31 可进行最严格的检查，但在旋翼飞行器移动（例如从船上发射）时仍可通过检查。在下列位中设置 1 会导致执行相应的检查。

0: 接收机报告的卫星数量必须为 >= 6

1: 接收机报告的 HDoP 必须为 >=2.5

2： 接收机报告的速度精度必须小于 1.0 米/秒（如有）。

3： 接收机报告的水平位置精度必须小于 5.0 米（如有）。

4: EKF2 磁强计或罗盘创新一致性检查必须通过。如果这些检查未通过，则偏航估计不可靠

5： 接收机报告的水平位置漂移率必须小于 0.3 米/秒

6： 过滤后接收器报告的垂直速度必须小于 0.3 米/秒

7： 过滤后接收器报告的水平速度必须小于 0.3 米/秒。

注：EKF2 需要在 10 秒内连续通过所有选定的检查，才能设置其原点并开始使用 GPS。

注： 检查 2、3、5、6 和 7 所需的精度可通过 EK2_CHECK_SCALE 参数进行调整。

ek2_check_scale

这是一个百分比缩放器，用于在 EKF 使用之前检查 GPS 精确度的阈值。大于 100 的值会增加 EKF 可接受的 GPS 最大误差，小于 100 的值会减少 EKF 可接受的 GPS 最大误差。这将修改 EK2_GPS_CHECK 参数中第 2、3、5、6 和 7 位启用的检查。

EK2_IMU_MASK

这是一个 1 字节位图，用于控制 EKF2 将使用哪些 IMU。每个被选中的 IMU 都将启动一个单独的 EKF2 实例。

• 设为 1 只使用第一个 IMU（默认值）

• 设置为 2 时只使用第二个 IMU

• 设置为 3 可使用第一个和第二个 IMU。

如果内存和处理资源允许，可以使用额外的 IMU，最多可达 6 个。内存和处理资源可能不足以运行多个实例。如果出现这种情况，EKF2 将无法启动，并向 GCS 控制台发送以下信息。

NavEKF2: 不 足够 内存

如果不使用地形数据，可以通过设置 TERRAIN_ENABLE=0 并重启来释放一些额外内存。