EKF

Extended Kalman Filter (EKF)

EKF2 和 EKF3 的优缺点

1. EKF2被默认最稳定
2. EKF2能接收从Vicon系统或者ROS SLAM 中接受位置估计，EKF3目前是没有的，除非和EKF2一样。
3. EKF2仅仅估计加速度z轴补偿，EKF3估计三轴加速度补偿。
4. EKF3接收更新的传感器源，包括：Beacons、Wheel Encoders 以及Visual Odometry。
5. EKF2 估计gyro刻度因子但是EKF3没有估计。一般来说，刻度因子一般是1.0左右，这在转速非常快时候用处才体现出来。

选择用那个EKF

AHRS_EKF_USE 设为1 用EKF，设为0 用DCM(目前强制设置为1)

AHRS_EKF_TYPE 设为2 用EKF2， 设为3为用EKF3

EK2_ENABLE、EK3_ENABLE 分别启用EKF2和EKF3

EK2_IMU_MASK、EK3_IMU_MASK 为指定的imu使用其EKF core，设置为1 用第一个imu启动单一EKF；设置为2 用第二个imu开始一个单一EKF；设置为3 用第一个和第二个imu 开启两个EKF core。

使用上述参数，可以同时并行运行多达5个AHRS（DCMx1，EKF2x2，EKF3x2），但这可能会导致性能问题，因此如果并行运行EKF2和EKF3，请设置IMU_MASK以减少总数core。

EK2_ALT_SOURCE 0为用气压计。 1. 为用rangefinder 2. 为用GPS。在不平稳路上最好是不要用rangefinder的高度。

EK2_ALT_M_NSE 默认1 ，当增加后增大对加速度的依赖。

EK2_GPS_TYPE 控制用GPS的哪些参数。

EK2_YAW_M_NSE 默认值为0.5，更高的值信任罗盘，更低的值信任GPS。

扩展卡尔曼导航概述和调试

APM 扩展卡尔曼把陀螺仪速率、加速度计、罗盘、GPS、空速计、气压测量值用于估算飞行器的位置、速度和角度方向。

EKF算法实施库为：AP_NavEKF 库，资料见：https://github.com/priseborough/InertialNav

原理

EKF导出22个状态量的方程

如下：https://github.com/priseborough/InertialNav/blob/master/derivations/GenerateEquations22states.m

无数学简单描述EKF滤波的方法如下：

State Prediction

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

1. IMU角度率积分计算角度。

2. IMU加速度计从机体坐标系X、Y、Z转换为地球北东地坐标系，并且进行重力校正。

3. 加速度积分计算速度。

4. 速度积分计算位置。

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

   State Covariance Matrix

   每当我们获得新的IMU数据时，重复步骤1）至5），直到来自另一传感器的新测量可用。

   如果我们有完美的初始估计，完美的IMU测量和完美的计算，那么我们可以在整个飞行过程中不断重复1）到4）而无需其他计算。但是，初始值中的误差，IMU测量中的误差以及计算中的舍入误差意味着我们只能在速度和位置误差变得太大之前停留几秒钟。

   扩展卡尔曼滤波器算法为我们提供了一种融合来自IMU，GPS，罗盘，空速，气压计和其他传感器的数据的方法，以计算我们的位置，速度和角度方向的更准确和可靠的估计（状态估计）。

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

6. 当GPS测量数据后，滤波器通过4预测出位置和GPS的位置不同，这个不同叫做更新协方差。

   Innovation

7. 6中的更新、5中的状态协方差矩阵、GPS测量误差指定为EKF_POSNE_NOISE，这三个被用于校正每个滤波状态。

   State Correction

   EKF的有用之处在于，能够使用不同误差和不同状态之间的相关性来校正被测量的状态之外的状态。例如，GPS位置测量能够校正位置，速度，角度和陀螺仪偏差中的误差。

   校正量由状态误差与测量误差的假定比率控制。这意味着如果滤波器认为其自己的计算位置比GPS测量更准确，则GPS测量的校正将更小。如果它认为其自己的计算位置不如GPS测量精确，则GPS测量的校正将更大。GPS测量的假设精度由EKF_POSNE_NOISE参数控制。使EKF_POSNE_NOISE大导致过滤器认为GPS的位置是不准确的。

8. 由于已经在测量，每个状态的不确定数是减少的。滤波器计算的不确定性减少是由于状态校正，更新状态协方差矩阵并且返回步骤1。

可调参数

AHRS_EKF_USE 目前默认为1，即启用EKF。

EKF_ABIAS_PNOISE 此参数控制垂直加速度偏置状态误差估计的增长。增加此参数让加速度计偏置估计更快且更嘈杂。

EKF_ACC_PNOISE 此噪声控制由于加速度测量误差引起的估计误差的增长。增加此参数让滤波器信任别的传感器多一点。

EKF_ALT_NOISE 气压计噪声，越大气压计噪声越大、越小气压计噪声越小。

EKF_ALT_SOURCE 此参数确定在光流导航间使用哪个高度测量源。设置为0表示使用气压计或1表示使用测距仪。如果设置为1，则车辆将尝试相对于地形保持恒定的高度，这是光流导航期间的默认行为。警告：EK2_ALT_SOURCE = 1仅适用于平面上的低空和低速操作，不适用于上行和下飞行。要使用较低海拔的测距仪和气压计进行上行和下行，请设置EK2_ALT_SOURCE = 0并使用RNG_USE_HGT参数。

EKF_EAS_GATE 此参数用于调整阈值用于空速测量更新不一致性检查。

EKF_EAS_NOISE 这是罗盘测量噪声的均方根值。 可以看EKF3的log数据来确定此值。

EKF_FALLBACK 此参数控制传感器不一致导致的回退至DCM。设置为1则当传感器不一致性很大则回退到DCM。

EKF_FLOW_DELAY 这是光流率测量的毫秒数落后IMU测量。

EKF_FLOW_GATE 增加此值会导致坏的光流测量速率被接受，减少此值会让可用的光流数据被拒绝。

EKF_FLOW_NOISE 光流噪声。

EKF_GBIAS_PNOISE 陀螺仪零偏值，越大陀螺仪，陀螺仪零偏估计更快更多噪声。

EKF_GLITCH_ACCEL 此参数控制水平加速度最大数量的差异(cm/s^2)和滤波器预测值之间用GPS测量值，在GPS定位测量以前被拒绝。如果此值设置得太低，那么有效的GPS数据将定期丢弃，定位精度将会降低。如果此参数设置得太高，然后GPS故障可能会导致大位置的快速变化。

EKF_GLITCH_RAD 预测的位置和GPS测量的位置之间最大的不同。

EKF_GND_GRADIENT 地形梯度。

EKF_GPS_TYPE 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity

EKF_GYRO_PNOISE 陀螺仪噪声，越大越不信任陀螺仪。

EKF_HGT_GATE 此参数调整阈值高度测量误差，低值正确值被忽略，高值错误值被接受。

EKF_MAGB_PNOISE 机体磁场状态误差估计。

EKF_MAG_CAL EKF允许学习磁力计补偿。0-4分别代表在不同的状态下学习。

EKF_MAG_GATE 磁力计GATE，增加坏的测量被接受，减少好的测量被去掉

EKF_MAG_NOISE 磁力计测量误差，增大的话更加相信陀螺仪的Z轴变化，减小的话非常容易受磁力计的影响。

EKF_MAX_FLOW 允许光流输出最大数据。

EKF_POS_DELAY 是GPS定位测量的毫秒数落后惯性测量。

EKF_POSNE_NOISE GPS噪声

EKF_POS_GATE 此参数调整位置阈值。

EKF_RNG_GATE rangefinder的阈值。

EKF_VELD_NOISE 垂直GPS速度测量速度测量误差。

EKF_VEL_DELAY GPS速度测量落后于惯性导航测量。

EKF_VEL_GATE GPS速度阈值

EKF_WIND_PNOISE

EKF_WIND_PSCALE

解释日志数据

EKF1

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)

EKF2

TimeMS - time in msec from startup.

Ratio - Weighting percentage of the IMU1 accelerometer data used in the blending of IMU1 and IMU2 data. If two IMU’s are available with your hardware (eg Pixhawk), then this will normally fluctuate rapidly in the 50% region as seen here.

AZ1bias - Z accelerometer bias for IMU1 (cm/s:sup:2)

AZ2bias - Z accelerometer bias for IMU2 (cm/s:sup:2)

VWN,VWE - North and East wind velocity (m/s). A positive value means the wind is moving in the direction of that axis, eg a positive North wind velocity is blowing from the South.

MX,MY,MZ - X, Y, Z body magnetic field biases (sensor units). If you are flying quickly, or are at low speed with EKF_MAG_CAL enabled, these will slowly change during flight as the filter ‘learns’ the earth’s magnetic field.

EKF3

此信息包含每个传感器的更新。更新为估计出来的值和传感器测量出来的值。

TimeMS - Time in msec from startup

IVN,IVE,IVD NED坐标系下 GPS速度测量更新（m/s）。当飞机没有移动时，这些由更新引起的噪声水平能用于设置EKF_VELNE_NOISE 、EKF_VELD_NOISE的值。要是波动范围在±0.3m/s时，则对应的值为0.3

IPN,IPE NED坐标下GPS位置测量更新（m）。他们应该靠近0才是最完美的。要是在±0.5m/s，则此值为0.5m

IPD 在气压高度测量的更新（m）。在不动时，此值最小变化为±0.15m，则此值应该为0.15m。经验表面，此值必须高，去平滑高度变化。EKF_ALT_NOISE

IMX,IMY,IMZ 磁力计测量值的X、Y、Z的更新值，在飞机飞动时候，不超过±50，最大最小不超过±50的话，则EKF_MAG_NOISE 设置为0.05。（差的磁力计校准也能从此处看出来）

IVT 空速测量误差。

EKF4

TimeMS - Time in msec from startup

SV 此参数用于确定EKF_VEL_GATE值，gps速度不一致，如果sv的线太高并且超过1 则应该增加EKF_VEL_GATE的值。

SP 用于确定EKF_POS_GAT 的值，gps位置不一致，若是sp超过1则EKF_POS_GATE 应该增加。

SH 气压计高度不匹配率限制。 EKF_HGT_GATE ，若是超过1则EKF_HGT_GATE值应该增加。

SMX,SMY,SMZ 磁力计XYZ 测量率不匹配EKF_MAG_GATE 若是大于1则代表罗盘校准有问题或者安装有问题，此时应该增加EKF_MAG_GATE参数。

SVT 空速计的测量更新。

EKF2 Estimation System

啥是EKF2系统？

24状态扩展卡尔曼滤波器在AP_NavEKF2库中估计如下的状态：

• 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)

EKF2的优势

1. 每一个IMU运行单独的EKF2，若是一个IMU故障则能从中恢复。
2. 若是磁力计故障则会切换磁力计。
3. 能估计陀螺仪尺度因子，在高速运动情况下能提高精度。
4. 让陀螺仪未校准时候也是不影响的，因为会估算出陀螺仪补偿和方向。
5. 能处理再飞行中的更大的陀螺仪零偏改变。
6. 能快速从坏的传感器数据中恢复。
7. 提供平滑的输出
8. 它使用较少的计算能力
9. 在没解锁时候就开始解锁马达。

如何获得的优势？

Using EKF2

1. EK2_ENABLE = 1 使能EKF2
2. 用EKF2控制进行控制，AHRS_EKF_TYPE = 2
3. 用多个传感器，由EK2_IMU_MASK 来控制。仅仅用IMU1，EK2_IMU_MASK = 1 ；仅仅用IMU2，EK2_IMU_MASK = 2；设置EK2_IMU_MASK = 3 双EKF2分别用IMU1和IMU2。

EKF2 日志数据

EKF2的数据，在NKF1到NKF9数据包中。

NKF1到NKF4包含第一个传感器EKF实例化信息，NKF6到NKF9包含第二个传感器实例化，若是EK2_IMU_MASK 设置为3。

EKF_ENABLE = 0 (关闭传统的EKF)

EK2_ENABLE = 1（打开EKF2）

EK2_IMU_MASK = 3（用2个EKF实例化，分别为IMU1和IMU2）

AHRS_EKF_TYPE = 2（高速飞控系统用EKF2）

LOG_BITMASK = 131071 （记录50hz数据）

滤波器状态估计

NKF1和NKF6分别包含imu1和imu2飞行控制系统输出。

TimeUS - time stamp (uSec)

Roll、Pitch、 Yaw 欧拉角（度）

VN,VE,VD 北东地速度（m/s） dPD 向下位置倒数（m/s）

PN,PE,PD 北东地位置（m）

GX,GY,GZ XYZ陀螺仪零偏速率（度/秒）

NKF2和NKF7分别包含imu1和imu2额外状态信息。

TimeUS - time stamp (uSec)

AZbias Z轴加速度偏差（cm/s/s）

GSX,GSY,GSZ - X,Y,Z 陀螺仪尺度因子速率（%） log中的0.5相当于1.005。

VWN,VWE 北东的风速。(m/s)

MN,ME,MD 北东地的磁场。(mGauss)

MX,MY,MZ -机体坐标系磁场。(mGauss)

MI 被用于EKF2的磁力计指数。

滤波器更新

NKF3和NKF8分别包含imu1和imu2滤波器更新

更新值为EKF2中预测的值和传感器中测得的数据之间的不同。很小的更新值暗示着很小的传感器误差。因为IMU数据被用于预测，坏的IMU数据能导致很大的测量更新值。

TimeUS - time stamp (uSec)

IVN,IVE,IVD - GPS速度更新，北东地 （m/s）

IPN,IPE - GPS位置更新北东（m）

IPD - 气压向下位置更新（m）

IMX,IMY,IMZ - 磁力计更新XYZ（mGauss）

IYAW - 罗盘yaw 更新（度）

IVT - 空速管更新（m/s）

滤波器健康和状态

NKF4和NKF9 包含更新协方差测试比率。一个值小于1则代表测量值通过检测并且被用于EKF2，当值大于1则代表数据很差，并不被用。在飞行中值小于0.3，则代表很好的传感器数据。

中肯地代表滤波器的健康状况。

TimeUS - time stamp (uSec)

SV - GPS velocity test ratio

SP - GPS position test ratio

SH - Barometer test ratio

SM - Magnetometer test ratio

SVT - Airspeed sensor Test ratio

errRP - 估计的姿态roll/pitch 误差（rad）

OFN - 位置跳跃北由于最近重置滤波器状态（m）

OFE - 位置跳跃东由于最近重置滤波器状态（m）

FS - 滤波器数值错误

TS - 滤波器测量超时

SS - 滤波器解决方案状态

GPS - gps品质检测

PI - 展示哪个EKF2被用于飞行控制

光流和定高雷达融合

NKF5 为光流融合数据

TimeUS - time stamp (uSec)

normInnov - 光流更新协方差测试率

FIX,FIY - optical flow X and Y axis innovations (mrad/s)

AFI - optical flow terrain height estimator innovation (mrad/s)

HAGL - estimated height above ground level (m)

meaRng - Range measured by the range finder (m)

offset - estimated terrain offset relative to the pressure height origin

RI - Range finder innovation (m)

errHAGL - 1-Sigma uncertainty in the terrain height offset estimate (m)

参数调整

EK2_ENABLE 为1使能EKF2

AHRS_EKF_TYPE 为2则用ekf2作为飞行算法。

EK2_GPS_TYPE 使用GPS类型

EK2_GPS_CHECK 设置为31为最严格的检查

EK2_IMU_MASK 3用两组imu

注意：如果没有使用地形数据，一些额外的内存可以通过设置TERRAIN_ENABLE=0释放并重新启动。