卡尔曼滤波(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
)
本文，我们给出卡尔曼滤波算法的具体实现流程，让算法能够运转起来！
“状态”是物体固有的变化属性，故状态值是无法直接获得的，只能进行估计。与之相对的，经过工具测量，我们获得的数据为“量测”，故量测值是可以直接通过测量获得的。那么，
| 变量 | 说明 |
|---|---|
| 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,k+1,⋯时刻已知 |
| X ^ ( k + 1 / k ) \hat{X}(k+1/k) X^(k+1/k) | 在 k + 1 k+1 k+1时刻通过状态预测方程获得，在 k + 1 , k + 2 , ⋯ k+1,k+2,\cdots k+1,k+2,⋯时刻已知 |
| Z ( k ) Z(k) Z(k) | 在 k k k时刻直接测量获得，在 k , k + 1 , ⋯ k,k+1,\cdots k,k+1,⋯时刻已知 |
首先，对于过程噪声的协方差矩阵
Q
(
k
)
Q(k)
Q(k)和量测噪声的协方差矩阵
R
(
k
)
R(k)
R(k)，二者是无法获得的，通常作为先验参数进行设置。
其次，状态协方差矩阵
P
(
j
/
k
)
P(j/k)
P(j/k)和量测协方差
S
(
j
/
k
)
S(j/k)
S(j/k)具有“统计意义”，即依据定义，二者是通过“均值”得到的。因此，对于少量样本数据，二者也是无法直接获得的，通常通过先验参数和递推公式进行计算。那么，
| 变量 | 说明 |
|---|---|
| P ( 0 / 0 ) , Q ( k ) , R ( k ) P(0/0),Q(k),R(k) P(0/0),Q(k),R(k) | 先验参数，人为设定 |
| P ( k / k − 1 ) , k ≥ 1 P(k/k-1), k \geq 1 P(k/k−1),k≥1 | 在 k k k时刻通过 P ( k − 1 / k − 1 ) P(k-1/k-1) P(k−1/k−1)计算获得，在 k , k + 1 , ⋯ k,k+1,\cdots k,k+1,⋯时刻已知 |
| P ( k / k ) , k ≥ 1 P(k/k), k \geq 1 P(k/k),k≥1 | 在 k k k时刻通过 P ( k / k − 1 ) P(k/k-1) P(k/k−1)计算获得，在 k , k + 1 , ⋯ k,k+1,\cdots k,k+1,⋯时刻已知 |
| S ( k / k − 1 ) , k ≥ 1 S(k/k-1), k \geq 1 S(k/k−1),k≥1 | 在 k k k时刻通过 P ( k / k ) P(k/k) P(k/k)计算获得，在 k , k + 1 , ⋯ k,k+1,\cdots k,k+1,⋯时刻已知 |
| 步骤序号 | 公式 | 说明 |
|---|---|---|
| 0 | P ( 0 / 0 ) = P 0 P(0/0)=P_0 P(0/0)=P0, X ( 0 / 0 ) = X 0 X(0/0)=X_0 X(0/0)=X0, Q ( k ) = Q 0 Q(k)=Q_0 Q(k)=Q0, R ( k ) = R 0 R(k)=R_0 R(k)=R0, G ( k ) = G 0 G(k)=G_0 G(k)=G0 | 变量初始化，设定初始值，设定先验参数 |
| 1 | P ( k + 1 / k ) = Φ ( k + 1 ) P ( k / k ) Φ T ( k + 1 ) + G ( k ) Q ( k ) G T ( k ) P(k+1/k) = \Phi(k+1)P(k/k)\Phi^T(k+1) + G(k)Q(k)G^T(k) P(k+1/k)=Φ(k+1)P(k/k)ΦT(k+1)+G(k)Q(k)GT(k) | 计算状态预测协方差矩阵 |
| 2 | S ( k + 1 / k ) = H ( k + 1 ) P ( k + 1 / k ) H T ( k + 1 ) + R ( k + 1 ) S(k+1/k) = H(k+1)P(k+1/k)H^T(k+1) + R(k+1) S(k+1/k)=H(k+1)P(k+1/k)HT(k+1)+R(k+1) | 计算量测预测协方差矩阵 |
| 3 | K ( k + 1 ) = P ( k + 1 / k ) H T ( k + 1 ) S − 1 ( k + 1 / k ) K(k+1)=P(k+1/k)H^T(k+1)S^{-1}(k+1/k) K(k+1)=P(k+1/k)HT(k+1)S−1(k+1/k) | 计算卡尔曼增益 |
| 4 | X ^ ( k + 1 / k ) = Φ ( k ) X ^ ( k / k ) + Γ ( k ) u ( k ) \hat{X}(k+1/k) = \Phi(k)\hat{X}(k/k) + \Gamma(k)u(k) X^(k+1/k)=Φ(k)X^(k/k)+Γ(k)u(k)，或者， X ^ ( k + 1 / k ) = Φ ( k ) X ^ ( k / k ) \hat{X}(k+1/k) = \Phi(k)\hat{X}(k/k) X^(k+1/k)=Φ(k)X^(k/k) | 获得 k + 1 k+1 k+1时刻状态预测值 |
| 5 |
Z
~
(
k
+
1
/
k
)
=
Z
(
k
+
1
)
−
Z
^
(
k
+
1
/
k
)
=
Z
(
k
+
1
)
−
H
(
k
+
1
)
X
^
(
k
+
1
/
k
)
Z~(k+1/k)=Z(k+1)−Z^(k+1/k)=Z(k+1)−H(k+1)X^(k+1/k) | 获得 k + 1 k+1 k+1时刻量测预测误差 |
| 6 | 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 + 1 k+1 k+1时刻状态滤波值（目标） |
| 7 | P ( k + 1 / k + 1 ) = P ( k + 1 / k ) − K ( k + 1 ) H ( k + 1 ) P ( k + 1 / k ) P(k+1/k+1) = P(k+1/k)-K(k+1)H(k+1)P(k+1/k) P(k+1/k+1)=P(k+1/k)−K(k+1)H(k+1)P(k+1/k) | 获得 k + 1 k+1 k+1时刻状态滤波协方差矩阵（用于下一时刻状态预测协方差矩阵的计算） |
| 8 | k = k + 1 k=k+1 k=k+1，跳转到步骤1 | 开始下一时刻的循环 |
以上列举了卡尔曼滤波算法的实现流程。