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 H_k = I Hk​=I 就演化为如下公式：

x k ′ = x k + K ′ ( z k − x k ) P k ′ = P k − K ′ P k K ′ = P k / ( P k + R k ) \begin{aligned} x'_k &= x_k + K'(z_k - x_k) \\ P'_k &= P_k - K'P_k \\ K' &= P_k / (P_k + R_k) \end{aligned} xk′​Pk′​K′​=xk​+K′(zk​−xk​)=Pk​−K′Pk​=Pk​/(Pk​+Rk​)​

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

meas_hagl 对应上式中 z k z_k zk​ ，是观测值。pred_hagl 对应上式中 H k x k H_k x_k Hk​xk​，对应运动估计。obs_var 对应 R k R_k Rk​，terrain_var 对应 P k P_k Pk​。第 158 行对应式 3 ，计算卡尔曼增益。160 ，162 行分别计算式 1，2。

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

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