原文：http://ardupilot.org/dev/docs/extended-kalman-filter.html#extended-kalman-filter-navigation-overview-and-tuning
本文介绍了由直升机和飞机使用的扩展卡尔曼滤波器（EKF）算法，用于基于速率陀螺仪，加速度计，罗盘（磁力计），GPS，空速和大气压力测量来估计车辆位置，速度和角度方向。 它包括算法的概述和有关可用调谐参数的信息。
像Pixhawk和PX4这样更快的处理器的可用性使得能够实现更先进的数学算法来估计飞行器的方向，速度和位置。 已经开发了扩展卡尔曼滤波器（EKF）算法，其使用速率陀螺仪，加速度计，罗盘，GPS，空速和大气压力测量来估计飞行器的位置，速度和角度定向。 该算法在AP_NavEKF库中实现，并基于这里记录的初始工作：https://github.com/priseborough/InertialNav
通过设置AHRS_EKF_USE = 1，PX4和Pixhawk用户可以使用此算法而不是传统的补充过滤器。
重要信息：除非执行了Accel和罗盘校准，否则不要设置AHRS_EKF_USE = 1。 否则可能会由于传感器数据不良而导致飞行路线不规则。
EKF优于DCM和Copters Inertial Nav所使用的更简单的互补滤波器算法的优点在于，通过融合所有可用的测量，它能够更好地拒绝具有显着误差的测量，使得vehicle不太容易受到影响单个传感器的故障的影响。
EKF算法的另一个特征是能够估计vehicle比较读数中的偏移量，并估计平面，直升机和流动站应用中地球的磁场。 这使得它比目前的DCM和INAV算法对罗盘校准误差不那么敏感。
它还可以使用可选的传感器（如光学流量和激光测距仪）进行测量，以辅助导航。
实现的EKF（扩展卡尔曼滤波）算法估计总共22个状态，其中使用以下方程导出的基础方程：https://github.com/priseborough/InertialNav/blob/master/derivations/GenerateEquations22states.m
以下是非常简化的非数学描述过滤器的工作原理：
1.积分IMU角速度来计算角位置
2.使用从主体X，Y，Z到地球北，东和下轴的角位置来转换IMU加速度，并对重力进行校正
3.积分加速度来计算速度
4.Velocity被积分以计算位置
这个从1）到4的过程被称为’状态预测’。“状态”是我们试图估计的变量，如滚动，俯仰偏航，高度，风速等。除了假定缓慢变化的位置，速度和角度之外，滤波器还有其他状态。 这些包括陀螺偏差，Z加速度计偏差，风速，罗盘偏差和地球磁场。 这些其他状态不是通过“状态预测”步骤直接修改，而是可以通过稍后描述的测量进行修改。
5.估计的陀螺仪和加速度计噪声（EKF_GYRO_NOISE和EKF_ACC_NOISE）用于估计使用IMU数据计算的角度，速度和位置误差的增长。 使这些参数变大会导致滤波器误差估计增长更快。 如果没有使用其他测量（例如GPS）进行更正，则该误差估计将继续增长。 这些估计误差被放在称为“状态协方差矩阵”的大矩阵中。
每次获得新的IMU数据时都会重复步骤1）至5），直到来自另一个传感器的新测量值可用。
如果我们有一个完美的初步估计，完美的IMU测量和完美的计算，那么我们可以继续重复1）到4）在整个飞行，没有其他计算需要。 然而，初始值中的错误，IMU测量中的错误和我们计算中的舍入误差意味着我们只能在速度和位置误差过大之前几秒钟。
扩展卡尔曼滤波算法为我们提供了一种组合或融合来自IMU，GPS，罗盘，空速，气压计和其他传感器的数据的方法，以计算对我们的位置，速度和角度方向的更准确可靠的估计。
以下示例描述了GPS水平位置测量的使用方法，但主要适用于其他测量类型（气压高度，GPS速度等）
6.当GPS测量到达时，滤波器计算从4）到GPS位置之间的差值。 这种差异被称为“Innovation”。
7.来自6）的’Innovation’，5）中的’状态协方差矩阵’和由EKF_POSNE_NOISE指定的GPS测量误差被组合以计算每个过滤状态的校正。 这被称为“State Correction”。
这是卡尔曼滤波器的聪明部分，因为它能够使用不同误差和不同状态之间的相关性的知识来校正除被测量之外的状态。 例如，GPS位置测量能够校正位置，速度，角度和陀螺仪偏差中的误差。
校正量由假定的状态误差与测量误差的比值来控制。 这意味着如果滤波器认为自己的计算位置比GPS测量更准确，那么GPS测量的校正将会更小。 如果它认为自己的计算位置不如GPS测量精度，那么GPS测量的校正将会更大。 GPS测量的假定精度由EKF_POSNE_NOISE参数控制。 使EKF_POSNE_NOISE更大，导致过滤器认为GPS位置不太准确。
8.因为我们现在已经进行了一次测量，所以每个状态中不确定性的量都被减少了。 滤波器计算由于“状态校正”引起的不确定性的降低，更新“状态协方差矩阵”并返回步骤1）
应将此值设置为1以使能过滤器，或将其设置为0以使用传统算法。 请注意，无论此参数如何，两种算法都在运行，如果启用了全速率AHRS数据记录，则所有EKF数据都将被记录。
从Copter3.3起，EKF默认启用，此参数不可用。 Plane和Rover用户仍然可以选择使用传统算法。
这种噪声控制了垂直加速度计偏差状态误差估计的增长。 增加它使加速度计偏差估计更快，更嘈杂。
这种噪声控制由于除偏差之外的加速度计测量误差引起的估计误差的增长。 增加它使得笛卡尔信任加速度计测量值较少和其他测量更多。
这是高度测量中噪声的RMS值。 如果增加此参数，滤波器将认为气压计更加嘈杂，并且对其测量值的权重较小。
如果此参数设置太小，则过滤器将不断响应气压计测量中的噪声，这将导致过滤器高度嘈杂。 在copter中，这将导致copter在高空保持期间上下摆动。
如果这个参数设置得太高，那么高度会越来越多，并且会更容易受到GPS垂直速度毛刺的影响。
有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
该参数控制在光流导航期间使用哪个测量源来确定高度。 设置为0以使用气压计或1使用测距仪。 如果设置为1，车辆将尝试相对于地形保持恒定的高度，这是光流导航期间的默认行为。
该参数缩放了用于空速测量创新一致性检查的阈值。 减少它使得更好的测量将被拒绝。 增加它使得更可能接受坏的测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
这是罗盘测量中噪声的RMS值。 增加它减少了这些测量的权重。 有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。 有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
此参数控制传感器数据的不一致是否会导致DCM的后备。 如果设置为0，则检测不一致的传感器不能造成后退。 如果设置为1，则数据的大不一致会导致DCM的后备（如果可用）。
这是光流速度测量滞后于IMU测量的毫秒数。
该参数控制测量的光流速与EKF开始拒绝测量之前的预测速率之间的最大差值。 减少此参数使得有效的光流量测量更可能被拒绝。 增加此参数使得更可能接受无效的光流量测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
该参数允许光流量测量误差和噪声。 它表示以rad / sec表示的预期RMS误差。 如果设置太大，位置会漂移更多。 如果设置太小，EKF的位置和速度输出将变得嘈杂，并且存在EKF可能会在操纵期间开始拒绝光流测量的风险。
该噪声控制了陀螺偏置状态误差估计的增长。 增加它使速率陀螺仪偏差估计更快，更嘈杂。
该参数控制在GPS位置测量被拒绝之前，由滤波器预测的值与由GPS测量的值之间的水平加速度（cm / s ^ 2）的最大差值。 如果该值设置得太低，则有效的GPS数据将被定期丢弃，并且位置精度会降低。 如果此参数设置得太高，则GPS毛刺会导致位置的快速变化。
该参数控制在长期毛刺保护逻辑被激活之前由滤波器预测的值与由GPS测量的值之间的水平位置（以m为单位）的差异的最大值，并且将偏移应用于GPS测量以补偿。 位置跳转小于此参数将被暂时忽略，但如果它们持续将被接受，并且过滤器将移动到新位置。 位置步长大于此值，最初也将被忽略，但如果它们持续存在，GPS位置测量将在使用前按步数量进行修正。 这防止位置发生大的阶跃变化。 这种校正以恒定的速率衰减回零，使得新的GPS位置将逐渐实现。 可以通过绘制EKF4.OFN和EKF4.OFE闪存数据来检查北向和东方向的校正值。
该参数控制在定影测距数据时假定的地形梯度的百分比，并影响估计地形高度对测量变化的响应。 当在不平坦的地形上进行操作时，可以增加这一点，以使地形估计能够更快地变化。
该参数控制GPS速度测量的使用：0 =使用3D速度，1 =使用2D速度，2 =不使用速度
该噪声控制由于不包括偏差的陀螺测量误差引起的估计误差的增长。 增加它使滤波器相信陀螺仪测量更少，其他测量更多。
此参数缩放用于高度测量创新一致性检查的阈值。 减少它使得更好的测量将被拒绝。 增加它使得更可能接受坏的测量。
这种噪声控制了体磁场状态误差估计的增长。 增加它使罗盘偏移估计更快，更嘈杂。
这种噪声控制了地磁场状态误差估计的增长。 增加它使地球磁场偏差估计更快，更嘈杂。
EKF能够在飞行中学习磁力计偏移。 此参数控制学习活动时间：
EKF_MAG_CAL = 0: Learning is enabled when speed and height indicate the vehicle is airborne
EKF_MAG_CAL = 1: Learning is enabled when the vehicle is manoeuvring
EKF_MAG_CAL = 2: Learning is disabled
EKF_MAG_CAL = 3: Learning is enabled when the vehicle is armed
该参数缩放用于磁力计测量创新一致性检查的阈值。 减少它使得更好的测量将被拒绝。 增加它使得更可能接受坏的测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
这是磁力计测量值/ 1000中噪声的RMS值。磁化计读数在滤波器使用之前缩放1/1000，以减少数值舍入误差的影响。 增加该噪声参数可减少磁力计测量的加权。 这将使滤波器的偏航度受到磁力计误差的影响较小，但受Z陀螺漂移影响较大。 有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
该参数控制由EKF作为有效测量值接受的最大光流量（rad / sec）。 这有助于拒绝在数据传输期间或当流量传感器不能跟上车辆的运动时损坏的测量。
这是GPS位置测量滞后于惯性测量的毫秒数。
这是GPS水平位置测量中噪声的RMS值。 如果增加此参数，滤波器将认为GPS更加嘈杂，并且对水平GPS速度测量值的加权较小。
如果此参数设置为较小，则过滤器将不断响应GPS位置的噪声，这可能导致连续和快速的小姿态，以及在升降机期间copter中的位置变化。
如果此参数设置为较大，则惯性传感器错误将导致滤波器位置缓慢移动，因为惯性计算中的误差不能被GPS校正。 这可能导致在loiter期间对于copter过度漂移。
有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
该参数用于缩放用于GPS位置测量创新一致性检查的阈值。 减少它使得更好的测量将被拒绝。 增加它使得更可能接受坏的测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
该参数控制测量的地面距离与EKF开始拒绝测量前的预测范围之间的最大差值。 减少此参数使得有可能拒绝有效的测距仪测量。 增加此参数使得更可能接受无效测距仪测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
这是垂直GPS速度测量中的噪声的RMS值，单位为m / s。 如果增加此参数，滤波器将认为GPS更嘈杂，并且对垂直GPS速度测量的权重较小。
如果此参数设置太小，则滤波器将不断响应GPS测量中的噪声，这将导致滤波器高度嘈杂。 在直升机中，这将导致直升机上下摆动。
如果此参数设置得太高，则滤波器将不会充分利用GPS速度信息，并且将更容易受到气压计高度毛刺的影响。
有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
这是北方和东部GPS速度测量的噪声的RMS值，单位为m / s。 如果增加此参数，滤波器将认为GPS更加嘈杂，并且对水平GPS速度测量值的加权较小。
如果此参数设置太小，则滤波器将不断响应GPS测量中的噪声，这将导致滤波器滚动和俯仰角度嘈杂。 如果你有车外面清晰的天空，远离建筑物和其他大物体，那么任务刨床中的HUD应该是稳定的。 如果它显着移动，那么过滤器设置的GPS噪声很可能太高了。 这也将导致连续和快速的小角度和位置变化在copter 悬停时。
如果此参数设置得太高，则滤波器将不会充分利用GPS速度信息，将会在更多位置上移动，并且会更容易受到GPS位置故障的影响。
有关使用日志数据帮助设置此参数的更多信息，请参阅有关解释EKF3日志数据的部分。
这是GPS速度测量滞后于惯性测量的毫秒数。
该参数缩放用于GPS速度测量创新一致性检查的阈值。 减少它使得更好的测量将被拒绝。 增加它使得更可能接受坏的测量。 它以标准偏差为单位。 例如，设置为3意味着大于3 x的假定标准差的差异将导致测量被拒绝。
这种噪声控制了风态误差估计的增长。 增加它使风力估计更快，更嘈杂。
增加这个参数会增加风态在改变高度时适应的速度，但会使风速估计更加嘈杂。
如果没有对闪存日志中的过滤器记录的数据进行某些分析，则无法正确调整导航过滤器。 要记录此数据，重要的是启用AHRS数据记录。 EKF数据包含在EKF1，EKF2，EKF3和EKF4日志消息中。 本节介绍各种EKF日志数据的含义，并显示使用Mission Planner DataFlash日志查看功能绘制数据获得的示例。
TimeMS - time in msec from startup
Roll - Roll angle (deg)
Pitch - Pitch angle (deg)
Yaw - Yaw angle (deg)
VN,VE,VD - North,East,Down velocities (m/s)
PN,PE,PD - North,East,Down positions (m) relative to where the vehicle was armed
GX,GY,GZ - X,Y,Z Gyro biases (deg/min)
下图显示了具有Pixhawk控制器的平面的陀螺偏压。 可以看出陀螺仪偏差在开始时变化，并且随着传感器加热并达到其工作温度，可以稳定新值。 我们的控制器使用的便宜的MEMS惯性传感器可以随温度显着偏差。
TimeMS - 从启动以毫秒为单位的时间。
Ratio(比率) - IMU1和IMU2数据混合中使用的IMU1加速度计数据的加权百分比。 如果您的硬件（例如Pixhawk）提供了两个IMU，那么这通常会在50％的区域迅速波动，如图所示。
如果飞行部分的波动接近100或0％，则表示您可能会有混叠影响您的加速度计数据，您应该寻找解决方案来减少这种情况（例如，您的自动驾驶仪的振动隔离装置）。
AZ1bias - IMU1的Z加速度计偏差（cm / s：sup：2）
AZ2bias - IMU2的Z加速度计偏差（cm / s：sup：2）
VWN，VWE - 东北风速（m / s）。 正值意味着风正在沿着该轴的方向移动，例如正面的北风速度从南方吹来。
MN，ME，MD - 北，东，下土磁场强度（传感器单位）。 如果您快速飞行，或者在启用EKF_MAG_CAL的情况下飞行速度很慢，那么当飞行过程中“过滤”地球的磁场时，这些将会缓慢变化。
MX，MY，MZ-X，Y，Z体磁场偏差（传感器单位）。 如果您快速飞行，或者在启用EKF_MAG_CAL的情况下飞行速度很慢，那么当飞行过程中“过滤”地球的磁场时，这些将会缓慢变化。 它们具有与罗盘偏移相同的含义，但是是相反的符号（例如，在下图中，MX稳定在值+35，表示应使用COMPASS_OFS_X值-35。
此消息包含每个传感器（GPS，气压计，磁力计和空速）的innovations 。 innovations 是在应用校正之前使用IMU数据预测的值与由传感器测量的值之间的差异。
TimeMS - 从启动以毫秒为单位的时间
IVN，IVE，IVD - 北，东，北GPS速度测量（m / s）的创新。 这些是导航过滤器健康的重要指标。 如果您有上帝质量的IMU和GPS数据，它们将小到零，如下图所示：
当vehicle 无法操纵时，这些Innovations 的噪音水平可用于设置EKF_VELNE_NOISE和EKF_VELD_NOISE的值。 例如，在上图中，车辆非机动时的速度噪声对于北，东和向下的速度都在±0.3米/秒左右。 这意味着对于该示例，EKF_VELNE_NOISE和EKF_VELD_NOISE的良好起始值将为0.3m / s。
IPN，IPE - 北方，东部GPS位置测量（m）的Innovations 。 类似于速度Innovations ，它们应该是小的，以零为中心，如下例所示：
这些Innovations 的噪音水平可用于设置EKF_POSNE_NOISE的值。 在上图中，噪声位于+ -0.5m的带内，因此在该示例中，EKF_POSNE_NOISE的值的良好起始值将为0.5m。
IPD - 气压计高度测量（m）的Innovations 。 它们应该是小的并且以零为中心，如以下示例所示，尽管由于IMU误差，传感器滞后和气流变化对气压计读数的影响，突然高度变化或机动动作时，大约2m的瞬变是常见的。
在上图中可以看出，在2分钟后有一个小的1m的负偏移被去除。 这是由于Z加速度计上的偏置误差，需要时间才能被滤波器学习并进行补偿。 在这个例子中，底层的传感器噪声低约为-0.5m，这表明EKF_ALT_NOISE在平面应用中的良好起始值将为0.15m。
注意：对于copter，经验表明，EKF_ALT_NOISE的值通常必须高于理论值以平滑高度响应。
IMX，IMY，IMZ - 磁力计X，Y，Z测量的创新。 在演习过程中，应以零为中心，不要超过+ - 50，如下图所示：
在上面的例子中，EKF_MAG_CAL设置为1，所以直升机快速了解了磁力计的偏差（罗盘偏移量）。 虽然磁力计的基础噪声相对较低（大多数情况下为5以下），但是由于轴间的比例因子差异，磁力计失准以及由导致较大误差的电力系统产生的变化磁场，存在其他误差。 通常这些在创新中会导致大约50的尖锐瞬变，如上图所示。 因此，EKF_MAG_NOISE的默认值设置为0.05（传感器单元的噪声为50）。
下图是从低速直升机飞行中获得的，磁力计校准不好，EKF_MAG_CAL = 0。当车辆改变方向时，创新会发生明显的变化。
IVT - 实际空速测量（m / s）的Innovation 。 如果空速传感器未安装或未使用（例如地面），则此值为零。 如果空速传感器被正确校准，它应该以零为中心，但是根据飞行状况如何变化，噪音水平会有所不同。 以下是来自具有良好校准的空速传感器的飞行示例，其在低湍流中的约7m / s的中等风力条件下：
从零开始1m / s的恒定偏移将表示稳定的1m / s空速误差。 如果空气速度传感器在大风日的初始化期间未被覆盖，则可能会导致稳定的空速误差，从而导致显着的压力偏移，不正常或自初始化以来经历了较大的温度变化。
该图也可用于设置EKF_EAS_NOISE的值。 对于上述示例，总噪声（包括阵风）约为1.4 m / s，因此对于EKF_EAS_NOISE来说，这是一个很好的起始值。
该消息包含显示每个传感器相对于由EKF_POS_GATE，EKF_VEL_GATE，EKF_HGT_GATE，EKF_MAG_GATE和EKF_EAS_GATE设置的错误门如何执行的曲线。 这些参数控制在过滤器不使用测量之前允许测量的不一致性。 当我们提到本节中测量的不一致性时，我们将讨论由滤波器预测的测量值与传感器测量值之间的差值。 检查不一致的测量对于GPS来说特别重要，因为GPS测量可以具有非常大的瞬态位置和速度误差，如果它们被滤波器使用会导致崩溃。 以下消息在EKF4中可用：
TimeMS - 从启动以毫秒为单位的时间
SV - 组合的GPS速度不一致与由EKF_VEL_GATE参数设置的极限的比率。 对于具有良好GPS数据的flight ，这可能偶尔会超过1/2，但不应超过1.如果该线路超过1，则表示过滤器停止使用该时段的GPS速度数据 飞行。 传感器数据不会发生。 下图显示了使用默认参数从具有9到10颗卫星的四旋翼飞行中获取的良好GPS条件下的SV。 如果这条线太高，并且使用良好的GPS超过1，那么应该增加EKF_VEL_GATE参数。
SP - GPS总位置不一致与EKF_POS_GATE参数设置的限制的比率。 对于具有良好GPS数据的航班，这可能偶尔会超过1/2，但不应超过1.如果该行超过1，则表示过滤器停止使用该时段的GPS位置数据 飞行。 传感器数据不会发生。 下图显示了在良好GPS状态下，使用默认参数从带有9到10颗卫星的四旋翼飞行中获取的SP。 如果这条线太高，并且使用良好的GPS超过1，那么应该增加EKF_POS_GATE参数。
SH - 气压计高度不一致与EKF_HGT_GATE参数设定的极限的比率。 这可能会偶尔超过1/2，但不应超过1.如果该行超过1，则表示过滤器停止使用飞行中该时段的气压计数据。 传感器数据不会发生。 下图显示了使用默认参数从空转速度高达16 m / s的四旋翼飞行中获取的SH。 如果该行太高并且超过1，那么应该增加EKF_HGT_GATE参数。 可能导致其高的因素包括影响气压计读数的自动驾驶仪的气流和由于传感器漂移或混叠引起的加速度计误差。
SMX，SMY，SMZ - 磁力计X，Y和Z的测量不一致度与EKF_MAG_GATE参数设置的极限的比例。 这可能偶尔会超过1/2，但不应超过1.如果该行超过1，则表示过滤器停止使用飞行中该段时间的磁力计数据的该组件。 传感器数据不会发生。 下图显示了使用默认参数从四旋翼飞行中获取的SMX，SMY和SMZ数据。 如果这条线太高，并且定期超过1，那么它表示罗盘校准或安装的问题。 建议首先调查罗盘错误的原因，然后再提高EKF_MAG_GATE参数。
下图显示从流动站日志中获取的SMX，SMY和SMZ，油门要求ThrOutR也绘制。
每次油门踏上时，每次超过1次的高峰值，油门操作中的大值可以清楚地看到。 在这个例子中，建议采取措施减少罗盘干扰的数量。
SVT - 空速测量不一致与EKF_EAS_GATE参数设定的极限的比率。 这可能会偶尔超过1/2，但很少会超过1.如果该行超过1，则表示过滤器在飞行中停止使用空速数据。 可能导致其高的因素包括空速校准误差，强阵风和湍流的存在以及风速的快速变化。 在过滤器估计风速之前，在飞行开始时，这是正常的。
