系列文章目录
卡尔曼滤波(1):状态更新方程的推导
卡尔曼滤波(2):让算法运转起来
前言
在上一篇文章中,我们展示了卡尔曼滤波状态更新方程的推导思路,并得到状态更新方程:
X ^ ( k + 1 / k + 1 ) = X ^ ( k + 1 / k ) + K ( k + 1 ) Z ~ ( k + 1 / k ) \hat{X}(k+1/k+1) = \hat{X}(k+1/k) + K(k+1) \tilde{Z}(k+1/k) X^(k+1/k+1)=X^(k+1/k)+K(k+1)Z~(k+1/k)
其中,
K ( k + 1 ) = P ( k + 1 / k ) H T ( k + 1 ) S − 1 ( k + 1 / k ) S ( k + 1 / k ) = H ( k + 1 ) P T ( k + 1 / k ) H T ( k + 1 ) + R ( k + 1 ) \begin{aligned} K(k+1) &= P(k+1/k)H^T(k+1)S^{-1}(k+1/k) \\ S(k+1/k) &= H(k+1)P^T(k+1/k)H^T(k+1) + R(k+1) \end{aligned} K(k+1)S(k+1/k)=P(k+1/k)HT(k+1)S−1(k+1/k)=H(k+1)PT(k+1/k)HT(k+1)+R(k+1)
本文,我们给出卡尔曼滤波算法的具体实现流程,让算法能够运转起来!
一、明确变量是否已知
1.状态值与测量值
“状态”是物体固有的变化属性,故状态值是无法直接获得的,只能进行估计。与之相对的,经过工具测量,我们获得的数据为“量测”,故量测值是可以直接通过测量获得的。那么,
变量 | 说明 |
---|---|
X ( k ) X(k) X(k) | 未知 |
X ^ ( k / k ) \hat{X}(k/k) X^(k/k) | 在 k k k时刻通过状态更新方程获得,在 k , k + 1 , ⋯ k,k+1,\cdots k< |