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了。