PX4中的卡尔曼滤波——代码公式对照解释

这篇博客以PX4中速度和位置估计来分析卡尔曼滤波过程，将枯燥的公式和鲜活的代码联系起来。
版本：v1.9.0
源码位置：

1. ~src/lib/ecl/EKF/vel_pos_fusion.cpp
2. ~src/lib/ecl/EKF/ekf_helper.cpp
3. ~src/lib/ecl/EKF/control.cpp

能力有限，出现错误欢迎指正讨论！

1. 变量解释

bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available
	bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
	float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
	float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
	float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
	float innovation[6]; // local copy of innovations for  [VN,VE,VD,PN,PE,PD]

• fuse_map: 判断数据是否有效的标志位
• innov_check_pass_map[6]: 判断数据更新是否有效的标志位
• R[6]：六个变量观测的噪音
• Kfusion[24]：卡尔曼增益
• innovation[6]：六个状态变量

2. 代码和公式对应

2.1卡尔曼滤波公式

$$\begin{gathered} S=H{P}_{k+1|k}{H}^T+R \\ K=P_{k+1|k}{H}^T{S}^{-1} \\ {x}_{k+1|k+1}=x_{k+1|k}+K(Z-H\ast x_{k+1|k}) \\ {P}_{k+1|k+1}=[I-KH]\ast P_{k+1|k} \end{gathered}$$
其中$P$是协方差，$R$是传感器观测噪音，$Z$是传感器观测值。

2.2 对应代码

说明：这里矩阵$H=I$，因此公式会变得简单
这一小结代码在vel_pos_fusion.cpp

_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];

对应第一个公式，_vel_pos_innov_var就是$S$

Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];

对应第二个公式，Kfusion对应$K$

KHP[row][column] = Kfusion[row] * P[state_index][column];
P[row][column] = P[row][column] - KHP[row][column];

对应第四个公式

2.2 状态更新公式说明

这行代码在ekf_helper.cpp

_state.vel(i) = _state.vel(i) - K[i + 4] * innovation;

对应第三个公式

重点说下第三个，和公式对比好像是错误的，因为这个代码表示的公式应该是
$$x_{k+1|k+1}=x_{k+1|k}-K\ast x$$

重点是我们需要知道_state.vel这个变量怎么来的，在GPS数据融合预处理中，有这么一段代码
这行代码在control.cpp

_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);

这里的_gps_sample_delayed.vel(0)就是代码里面$Z$，因此这个公式对应的是
$$x=x_{k+1|k}-Z$$
将这个公式带入上式就可以得到
$$\begin{gathered} x_{k+1|k+1}=x_{k+1|k}-K\ast (x_{k+1|k}-Z) \\ =x_{k+1|k}+K(Z-H\ast x_{k+1|k}) \end{gathered}$$

3. 总结

PX4中卡尔曼滤波看起来很复杂，简而言之就是IMU做状态变量的更新，其他传感器纠正IMU估计出来的状态变量。复杂就复杂在不同传感器之间的处理，里面会出现各种情况（GPS可用但是气压计不可用，磁力计测得的数据不可信，等等…），因此要对这些情况做处理并且给出对应的协方差传播的公式和观测噪声矩阵。