文章目录
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 ′ =