寻找APM中EKF的五大公式

EKF核心代码位置

AP_NavEKF2.cpp


进入该函数

进入该函数，然后可以看到关键部分，也即卡尔曼五个公式的地方。

下面介绍每个公式的具体位置

28状态值

首先要知道选用的状态值有哪些，28状态值（但参与EKF方差估计的只有24个，不计算四元数的协方差，FIX4中的EKF是24状态值，没有0~2）

公式一：x（k+1|k）=F*x(k|k)+Q

四元数更新↓

速度位置更新↓

调用的相关变量如下↓

总结

IMU的数据用于预测过程，
IMU得到的角度增量用于更新四元数
IMU得到的速度增量用于更新速度和位置
更新姿态角涉及到状态值【9-14，上一时刻的26~28】
更新速度涉及到旋转矩阵及状态值【15】，也即涉及到状态值【9-15，上一时刻的26~28】
更新位置涉及到速度，也即涉及到状态值【3-5，9-15，上一时刻的26~28】
由此发现状态值的更新是非线性的，需要通过求偏导得到状态转移矩阵，和噪声驱动矩阵，从而进行P的第一次更新。

公式二：P(k+1|k) = FP(k|k)Ft + GQGt

进入下面该函数↓

得到过程噪声 ，注意没有四元数，共24维。其中进行了状态更新的速度位置和误差角，即状态量的【0~8】初始化为0

其他几个值分别初始化为↓

nextP即为更新后的P值，这里为最后的输出值，代码中完整公式为↓

nextP的来源就是根据上述公式，结果较长，这里不粘贴，知道是通过雅克比求偏导得来的就行。
NextP求完，复制给P

公式三：K=P（k+1|k） * Ht * 1/S(k)

下面只以磁力计为例
首先得到创新数据，也即H*stateStruct — MagMea,如下↓

其中MagPred，来源于状态值stateStruct.quat、stateStruct.earth_magfield、stateStruct.body_magfield↓

初始化测量噪声↓

根据公式得到K↓

公式四：x(k+1|k+1) = x(k+1|k) + K * (innovX)

公式五：P(k+1|k+1) =(I - K*H) P(k+1|k)

自此，卡尔曼的五个公式完成了，也得到了新的stateStruct值。
然而，最终采用的估计值并非是EKF的输出值，而是有用新的IMU的与EKF的输出值进行融合，作为最终的估计值，存放在outputState中。
APM的EKF花了大量的代码来限制噪声协方差的大小，以及根据计算的方差来判断是否进行融合等，更精华的地方或许也在此。

飞控学习告一段落，接下来学习slam了。