PX4 -- EKF2

EKF2

参数

EKF2 中有一类 GATE 参数。当测量值在 VAR ± GATE 范围内才会更新值。

高度估计

四种高度控制方法：气压计，GPS，Range Finder，Vision。如果将 Range Finder 作为 Secondary Source （具体参考 Range Finder 篇），Primary Source 会在合适条件（一定高度，一定的速度下）切换为 Secondary Source 估计高度。气压计是默认的高度信息源。

Range Finder

在 EKF2 初始化时，会初始化 Terrain Estimator，估计地形高度（_terrain_vpos）。之后的高度估计会修正地形的影响。

滤波

一个量（高度）改变对所有的影响。参考下文，单变量更新对多变量的影响。

单变量更新

先放一张卡尔曼滤波公式：

如果都是单变量，且 $H_k=I$ 就演化为如下公式：

$$\begin{array}{rl}{x}_k^{\prime }& =x_k+K^{\prime }(z_k-x_k)\\ P_k^{\prime }& =P_k-K^{\prime }{P}_k\\ K^{\prime }& =P_k/(P_k+R_k)\end{array}$$

我们现在看一下 EKF2 中 terrain estimation 的更新过程

meas_hagl 对应上式中 $z_k$ ，是观测值。pred_hagl 对应上式中 $H_k{x}_k$，对应运动估计。obs_var 对应 $R_k$，terrain_var 对应 $P_k$。第 158 行对应式 3 ，计算卡尔曼增益。160 ，162 行分别计算式 1，2。

单变量更新对多变量的影响

151（计算观测后的方差）， 190 （计算卡尔曼增益） 对应公式3。 218 对应式 2 （更新方差），223 对应式 1 （更新状态）。