ArduPilot开源代码之EKF1

1. 源由

在Ardupilot上已经没有EKF1的源代码，但是EKF1是最为基础的引入概念。

从学习研究的角度，从这个点切入可以更好的理解概念，有一个比较好的基础。

既然是“小白”当然从这里入手。

2. EKF简介

更快处理器（如 STM32 F405/H743 等）的可用性使得能够实现更先进的数学算法来估计飞行器的姿态、速度和位置。开发了一种扩展卡尔曼滤波器（EKF）算法，该算法使用速率陀螺仪、加速度计、指南针、GPS、空速和气压测量来估计飞行器的位置、速度和角度方向。

EKF 相对于 DCM 和 惯性导航所使用的简单互补滤波算法的优势在于，通过融合所有可用的测量数据，它能够更好地拒绝具有显著误差的测量值，使得飞行器对单个传感器故障的敏感性降低。

EKF 算法的另一个特点是能够估计飞行器指南针读数的偏移量，并估计地球磁场，这适用于固定翼飞机、四轴飞行器以及小车应用。这使得它比当前的 DCM 和 INAV 算法对指南针校准误差的敏感性更低。

它还可以利用来自可选传感器（如光流传感器和激光测距仪）的测量数据来辅助导航。

整个算法可以归纳成两部分：算法步骤和算法参数。

注：其中EKF2/EKF3在 AP_NavEKF2 和 AP_NavEKF3 库中实现，基于最初的工作记录在这里：Inertial Navigation Estimation Library

3. 算法步骤

EKF（扩展卡尔曼滤波器）算法实现了对总共22个状态的估计。

• Step 1：IMU的角速率被积分以计算角位置。

• Step 2：IMU的加速度根据从机体坐标系（X，Y，Z）到地球北、东、向下坐标系的角位置转换，并校正重力。

• Step 3：加速度被积分以计算速度。

• Step 4：速度被积分以计算位置。

从1）到4）的过程被称为“状态预测”。‘状态’是我们试图估计的变量，如横滚、俯仰、偏航、高度、风速等。除了位置、速度和角度之外，滤波器还有其他假设变化缓慢的状态。这些状态包括陀螺仪偏差、Z轴加速度计偏差、风速、罗盘偏差和地球磁场。这些其他状态不会直接由“状态预测”步骤修改，但可以通过后面描述的测量进行修改。

• Step 5：估计的陀螺仪和加速度计噪声（EKF_GYRO_NOISE和EKF_ACC_NOISE）用于估计使用IMU数据计算的角度、速度和位置误差的增长。增大这些参数会导致滤波器的误差估计增长更快。如果没有使用其他测量（例如GPS）进行修正，这种误差估计将继续增长。这些估计的误差被记录在一个称为“状态协方差矩阵”的大矩阵中。

步骤1）到5）在每次获取新的IMU数据之后重复，直到从其他传感器获取新的测量数据为止。

如果我们有一个完美的初始估计、完美的IMU测量和完美的计算，那么我们可以在飞行过程中只重复1）到4）步骤，而不需要进行其他计算。然而，初始值误差、IMU测量误差和计算中的舍入误差意味着我们只能在几秒钟内进行，然后速度和位置误差就会变得太大。

扩展卡尔曼滤波器算法提供了一种将IMU、GPS、罗盘、空速计、气压计和其他传感器的数据结合或融合以计算位置、速度和角度方向更精确可靠的估计的方法。

以下示例描述了如何使用GPS水平位置测量，但同样的原理也适用于其他测量类型（气压高度、GPS速度等）。

• Step 6：当GPS测量到达时，滤波器计算从步骤4）预测的位置和GPS位置之间的差异。这个差异称为“创新”。

• Step 7：从步骤6）的“创新”，从步骤5）的“状态协方差矩阵”，以及由EKF_POSNE_NOISE指定的GPS测量误差，组合计算每个滤波器状态的修正。这被称为“状态修正”。

这是卡尔曼滤波器的聪明之处，因为它能够利用对不同误差和不同状态之间相关性的知识来校正除了被测量的状态之外的其他状态。例如，GPS位置测量能够校正位置、速度、角度和陀螺仪偏差中的误差。

校正的量由假设的状态误差与测量误差的比率控制。这意味着如果滤波器认为自己计算的位置比GPS测量更精确，那么来自GPS测量的校正将更小。如果它认为自己计算的位置比GPS测量更不精确，那么来自GPS测量的校正将更大。GPS测量的假设精度由EKF_POSNE_NOISE参数控制。增大EKF_POSNE_NOISE会导致滤波器认为GPS位置不太精确。

• Step 8：因为我们现在已经进行了一次测量，已更新的每个状态的不确定性量将减少。滤波器计算由于“状态修正”导致的不确定性减少，更新“状态协方差矩阵”，然后返回步骤1）。

4. 算法参数

4.1 EKF IMU

4.1.1 AHRS_EKF_USE*

这个参数设置为1表示启用滤波器的使用，设置为0表示使用传统算法。

注1：无论此参数如何设置，两种算法都在运行，并且只要启用了完整速率AHRS数据记录，所有EKF数据都将被记录。
注2：从Copter3.3版本开始，默认情况下启用了EKF，并且此参数不可用。飞机和小车用户仍然可以选择使用传统算法。

4.1.2 EKF_ABIAS_PNOISE

这个噪声参数控制垂直加速度计偏置状态误差估计的增长。增大这个参数会加快加速度计偏置的估计速度，但同时会增加噪音。

注：采样频率增加，噪音自然增加。频率降低相当于时间域平均，增加频率把更容易看出高频噪音。

4.1.3 EKF_ACC_PNOISE

这个噪声参数控制由加速度计测量误差（不包括偏置）引起的估计误差的增长。增大这个参数会使滤波器对加速度计测量的信任减少，对其他测量的信任增加。

4.1.4 EKF_GBIAS_PNOISE

这个噪声参数控制陀螺偏置状态误差估计的增长。增大这个参数会加快陀螺偏置的估计速度，但同时会增加噪音。

4.1.5 EKF_GYRO_PNOISE

这个噪声参数控制由陀螺测量误差（不包括偏置）引起的估计误差的增长。增大这个参数会使滤波器对陀螺测量的信任减少，对其他测量的信任增加。

4.1.6 EKF_EAS_GATE

这个参数缩放了用于空速测量创新一致性检查的阈值。

• 减小它会增加拒绝好测量的可能性；
• 增加它会增加接受坏测量的可能性；

注：它以标准偏差单位进行缩放。例如，设置为3意味着大于3倍假设标准偏差的差异将导致测量被拒绝。

4.1.7 EKF_EAS_NOISE

这是空速测量中的噪声的RMS(Root Mean Square)值。增大它会减少对这些测量的权重。

4.1.8 EKF_FALLBACK

这个参数控制传感器数据不一致是否会导致返回到DCM。

• 如果设置为0，则检测到传感器不一致不会导致返回;
• 如果设置为1，则数据中的大不一致将导致如果可用，则返回到DCM;

4.2 EKF GPS

4.2.1 EKF_GLITCH_ACCEL

这个参数控制在GPS测量的水平加速度值与滤波器预测值之间的最大差值（以cm/s^2为单位），在拒绝GPS位置测量之前。

• 如果这个值设置得太低，有效的GPS数据将经常被丢弃，导致位置精度降低。
• 如果这个参数设置得太高，GPS故障可能会导致位置的大幅快速变化。

4.2.2 EKF_GLITCH_RAD

这个参数控制在GPS测量的水平位置值与滤波器预测值之间的最大差值（以米为单位），在长期故障保护逻辑激活并对GPS测量施加偏移量以补偿之前。

• 小于此参数的位置跳跃将被暂时忽略，但如果它们持续存在，将被接受，并且滤波器将移动到新位置。
• 大于此值的位置步骤在初始阶段也会被忽略，但如果持续存在，GPS位置测量将通过步骤量进行校正，然后使用。

这样可以防止位置的大步变化。这种校正以恒定速率衰减回零，以便新的GPS位置逐渐实现。在北和东方向上的这种校正值可以通过绘制EKF4.OFN和EKF4.OFE闪存日志数据来检查。

4.2.3 EKF_GPS_TYPE

这个参数控制使用GPS速度测量：

• 0 = 使用3D速度；
• 1 = 使用2D速度；
• 2 = 不使用速度；

4.3 EKF Height

4.3.1 EKF_ALT_NOISE

这是高度测量中的噪声的RMS值。如果增加这个参数，滤波器会认为气压计的噪声更大，并在其测量中放置更少的权重。

• 如果这个参数设置得太小，滤波器会不断对气压计测量中的噪声做出反应，导致飞行器在悬停期间不断进行快速的姿态和位置变化。
• 如果这个参数设置得太大，惯性传感器的误差会导致滤波器位置漂移较快，因为没有足够的GPS校正。这会导致在悬停期间飞行器位置过多的漂移。

4.3.2 EKF_ALT_SOURCE

这个参数控制在光流导航期间用于确定高度的测量来源。

• 设置为0，使用气压计；
• 设置为1，使用测距仪；

4.3.3 EKF_GND_GRADIENT

这个参数控制在融合测距仪数据时假设的地形梯度百分比，影响估计地形高度对测量变化的响应速度。在不平坦地形操作时，可以增加此值以使地形估计更快速变化。

4.3.4 EKF_HGT_GATE

此参数用于调整高度测量创新一致性检查的阈值。降低此参数会使得有效测量更有可能被拒绝。增加此参数会使得错误测量更有可能被接受。

4.4 EKF Flow

4.4.1 EKF_FLOW_DELAY

这是光流速率测量滞后于IMU测量的毫秒数。

4.4.2 EKF_FLOW_GATE

这个参数控制在EKF开始拒绝光流测量之前，测量的光流率与预测率之间的最大差值。

• 减小此参数会增加拒绝有效光流率测量的可能性;
• 增加此参数会增加接受无效光流率测量的可能性;

注：它以标准偏差单位进行缩放。例如，设置为3意味着大于3倍假设标准偏差的差异将导致测量被拒绝。

4.4.3 EKF_FLOW_NOISE

这个参数允许光流速率测量误差和噪声。它表示预期的角速率误差的RMS值。

• 如果设置得太大，位置将会漂移更多；
• 如果设置得太小，EKF输出的位置和速度将变得嘈杂，并且在机动过程中有可能EKF会开始拒绝光流测量。

4.4.4 EKF_MAX_FLOW

此参数控制EKF接受的最大光流速率（单位为弧度/秒）。这有助于拒绝在数据传输过程中被破坏或当流量传感器无法跟上飞行器运动时的测量。

4.5 EKF Mag

4.5.1 EKF_MAGB_PNOISE

此噪声控制机身磁场状态误差估计的增长。增加该值会使罗盘偏移估计更快，但噪声更大。

4.5.2 EKF_MAGE_PNOISE

此噪声控制地磁场状态误差估计的增长。增加该值会使地磁场偏差估计更快，但噪声更大。

4.5.3 EKF_MAG_CAL

EKF能够在飞行中学习磁力计偏移。此参数控制何时启用学习：

• EKF_MAG_CAL = 0：当速度和高度表明飞行器在空中时启用学习
• EKF_MAG_CAL = 1：当飞行器在机动时启用学习
• EKF_MAG_CAL = 2：禁用学习
• EKF_MAG_CAL = 3：当飞行器解锁时启用学习

4.5.4 EKF_MAG_GATE

此参数用于调整磁力计测量创新一致性检查的阈值。

• 降低此参数会使得有效测量更有可能被拒绝。
• 增加此参数会使得错误测量更有可能被接受。

以标准偏差为单位进行缩放。例如，设置为3意味着差异大于假定标准偏差的3倍将导致测量被拒绝。

4.5.5 EKF_MAG_NOISE

这是磁力计测量噪声的均方根值，单位为1/1000。为了减少数值舍入误差，磁力计读数在被滤波器使用之前会缩放为1/1000。

增加此噪声参数会减少磁力计测量的权重。这会使滤波器的偏航受磁力计误差的影响较小，但受Z轴陀螺仪漂移的影响较大。

4.6 EKF Position

4.6.1 EKF_POS_DELAY

这是GPS位置测量滞后于惯性测量的毫秒数。

4.6.2 EKF_POSNE_NOISE

这是GPS水平位置测量中的均方根噪声值。如果增加此参数，滤波器会认为GPS更噪声，并会减少对水平GPS速度测量的权重。

• 如果此参数设置得太小，滤波器将不断对GPS位置中的噪声作出反应，这可能会导致四旋翼飞行器在悬停时不断快速小幅度的姿态和位置变化。
• 如果此参数设置得太大，惯性传感器误差将导致滤波器位置缓慢漂移，因为惯性计算中的误差没有被GPS充分校正。这可能会导致四旋翼飞行器在悬停时的位置过度漂移。

有关使用日志数据设置此参数的更多信息，请参见EKF3日志数据解释部分。

4.6.3 EKF_POS_GATE

此参数用于调整GPS位置测量创新一致性检查的阈值。

• 降低此参数会使得有效测量更有可能被拒绝。
• 增加此参数会使得错误测量更有可能被接受。

以标准偏差为单位进行缩放。例如，设置为3意味着差异大于假定标准偏差的3倍将导致测量被拒绝。

4.6.4 EKF_RNG_GATE

此参数控制测量到地面的距离与预测距离之间的最大差异，超过该差异EKF将开始拒绝测量。

• 降低此参数会使得有效的距离测量更有可能被拒绝。
• 增加此参数会使得错误的距离测量更有可能被接受。

以标准偏差为单位进行缩放。例如，设置为3意味着差异大于假定标准偏差的3倍将导致测量被拒绝。

4.7 EKF Velocity

4.7.1 EKF_VELD_NOISE

这是垂直GPS速度测量中的均方根噪声值，单位为m/s。如果增加此参数，滤波器会认为GPS更噪声，并会减少对垂直GPS速度测量的权重。

• 如果此参数设置得太小，滤波器将不断对GPS测量中的噪声作出反应，这会导致滤波器高度变得噪声。

在四旋翼飞行器中，这会导致飞行器上下抖动。

• 如果此参数设置得太高，滤波器将无法充分利用GPS速度信息，并且更容易受到气压高度异常的影响。

有关使用日志数据设置此参数的更多信息，请参见EKF3日志数据解释部分。

4.7.2 EKF_VELNE_NOISE

这是北部和东部GPS速度测量中的均方根噪声值，单位为m/s。如果增加此参数，滤波器会认为GPS更噪声，并会减少对水平GPS速度测量的权重。

• 如果此参数设置得太小，滤波器将不断对GPS测量中的噪声作出反应，这会导致滤波器的滚动和俯仰角度变得噪声。

如果飞行器在外面，视野开阔，远离建筑物和其他大物体，那么任务规划器中的HUD应该是稳定的。
如果它明显移动，那么可能是GPS噪声对于滤波器设置来说太高。这也会导致四旋翼飞行器在悬停时不断快速小幅度的角度和位置变化。

• 如果此参数设置得太高，滤波器将无法充分利用GPS速度信息，位置将会更漂移，并且更容易受到GPS位置异常的影响。

4.7.3 EKF_VEL_DELAY

这是GPS速度测量滞后于惯性测量的毫秒数。

4.7.4 EKF_VEL_GATE

此参数用于调整GPS速度测量创新一致性检查的阈值。

• 降低此参数会使得有效测量更有可能被拒绝。
• 增加此参数会使得错误测量更有可能被接受。

以标准偏差为单位进行缩放。例如，设置为3意味着差异大于假定标准偏差的3倍将导致测量被拒绝。

4.8 EKF Wind

4.8.1 EKF_WIND_PNOISE

此噪声控制风状态误差估计的增长。增加该值会使风速估计更快但噪声更大。

4.8.2 EKF_WIND_PSCALE

增加此参数会增加风状态在改变高度时的适应速度，但会使风速估计引入更噪声。

5. 参考资料

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