一、协方差矩阵公式推导
已知 P k − = E [ e k − e k − T ] , e k − = x k − x ^ k − P_{k}^{-}=E\left[e_{k}^{-} e_{k}^{-T}\right], e_{k}^{-}=x_{k}-\hat{x}_{k}^{-} Pk−=E[ek−ek−T],ek−=xk−x^k−, 有:
e
k
−
=
x
k
−
x
^
k
−
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
−
A
x
^
k
−
1
+
B
u
k
−
1
=
A
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
−
1
=
A
e
k
−
1
+
w
k
−
1
\begin{aligned} &e_{k}^{-}=x_{k}-\hat{x}_{k}^{-} \\ &=A x_{k-1}+B u_{k-1}+w_{k-1}-A \hat{x}_{k-1}+B u_{k-1} \\ &=A\left(x_{k-1}-\hat{x}_{k-1}\right)+w_{k-1} \\ &=A e_{k-1}+w_{k-1} \end{aligned}
ek−=xk−x^k−=Axk−1+Buk−1+wk−1−Ax^k−1+Buk−1=A(xk−1−x^k−1)+wk−1=Aek−1+wk−1
P
k
−
=
E
[
e
k
−
e
k
−
T
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
A
e
k
−
1
+
w
k
−
1
)
T
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
e
k
−
1
T
A
T
+
w
k
−
1
T
)
]
=
E
[
A
e
k
−
1
e
k
−
1
T
A
T
+
A
e
k
−
1
w
k
−
1
T
+
w
k
−
1
e
k
−
1
T
A
T
+
w
k
−
1
w
k
−
1
T
]
=
E
[
A
e
k
−
1
e
k
−
1
T
A
T
]
+
E
[
A
e
k
−
1
w
k
−
1
T
]
+
E
[
w
k
−
1
e
k
−
1
T
A
T
]
+
E
[
w
k
−
1
w
k
−
1
T
]
\begin{aligned} P_{k}^{-} &=E\left[e_{k}^{-} e_{k}^{-T}\right] \\ &=E\left[\left(A e_{k-1}+w_{k-1}\right)\left(A e_{k-1}+w_{k-1}\right)^{T}\right] \\ &=E\left[\left(A e_{k-1}+w_{k-1}\right)\left(e_{k-1}^{T} A^{T}+w_{k-1}^{T}\right)\right] \\ &=E\left[A e_{k-1} e_{k-1}^{T} A^{T}+A e_{k-1} w_{k-1}^{T}+w_{k-1} e_{k-1}^{T} A^{T}+w_{k-1} w_{k-1}^{T}\right] \\ &=E\left[A e_{k-1} e_{k-1}^{T} A^{T}\right]+E\left[A e_{k-1} w_{k-1}^{T}\right]+E\left[w_{k-1} e_{k-1}^{T} A^{T}\right]+E\left[w_{k-1} w_{k-1}^{T}\right] \end{aligned}
Pk−=E[ek−ek−T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=E[(Aek−1+wk−1)(ek−1TAT+wk−1T)]=E[Aek−1ek−1TAT+Aek−1wk−1T+wk−1ek−1TAT+wk−1wk−1T]=E[Aek−1ek−1TAT]+E[Aek−1wk−1T]+E[wk−1ek−1TAT]+E[wk−1wk−1T]
由已知条件可知， e k − 1 e_{k-1} ek−1 是第 k − 1 k − 1 k−1 次的估计误差，而 w k − 1 w_{k-1} wk−1 是计算第 k k k个真实值的噪声，所以 e k − 1 e_{k-1} ek−1 和 w k − 1 w_{k-1} wk−1相互独立。根据上述结论可得先验误差协方差矩阵：
P
k
−
=
E
[
A
e
k
−
1
e
k
−
1
T
A
T
]
+
0
+
0
+
E
[
w
k
−
1
w
k
−
1
T
]
=
E
[
A
e
k
−
1
e
k
−
1
T
A
T
]
+
E
[
w
k
−
1
w
k
−
1
T
]
=
A
E
[
e
k
−
1
e
k
−
1
T
]
A
T
+
E
[
w
k
−
1
w
k
−
1
T
]
=
A
P
k
−
1
A
T
+
Q
\begin{aligned} P_{k}^{-} &=E\left[A e_{k-1} e_{k-1}^{T} A^{T}\right]+0+0+E\left[w_{k-1} w_{k-1}^{T}\right] \\ &=E\left[A e_{k-1} e_{k-1}^{T} A^{T}\right]+E\left[w_{k-1} w_{k-1}^{T}\right] \\ &=A E\left[e_{k-1} e_{k-1}^{T}\right] A^{T}+E\left[w_{k-1} w_{k-1}^{T}\right] \\ &=A P_{k-1} A^{T}+Q \end{aligned}
Pk−=E[Aek−1ek−1TAT]+0+0+E[wk−1wk−1T]=E[Aek−1ek−1TAT]+E[wk−1wk−1T]=AE[ek−1ek−1T]AT+E[wk−1wk−1T]=APk−1AT+Q
上期博客已经推导出
K
k
K_{k}
Kk 的表达式，将其代入到
P
k
P_{k}
Pk 表达式中，可得：
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
=
(
I
−
K
k
H
)
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
=
(
I
−
K
k
H
)
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
H
P
k
−
H
+
R
(
H
P
k
−
H
T
+
R
)
K
k
T
=
(
I
−
K
k
H
)
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
K
k
T
=
(
I
−
K
k
H
)
P
k
−
\begin{aligned} P_{k} &=P_{k}^{-}-K_{k} H P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+K_{k} H P_{k}^{-} H^{T} K_{k}^{T}+K_{k} R K_{k}^{T} \\ &=\left(I-K_{k} H\right) P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+K_{k}\left(H P_{k}^{-} H^{T}+R\right) K_{k}^{T} \\ &=\left(I-K_{k} H\right) P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H+R}\left(H P_{k}^{-} H^{T}+R\right) K_{k}^{T} \\ &=\left(I-K_{k} H\right) P_{k}^{-}-P_{k}^{-} H^{T} K_{k}^{T}+P_{k}^{-} H^{T} K_{k}^{T} \\ &=\left(I-K_{k} H\right) P_{k}^{-} \end{aligned}
Pk=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT=(I−KkH)Pk−−Pk−HTKkT+Kk(HPk−HT+R)KkT=(I−KkH)Pk−−Pk−HTKkT+HPk−H+RPk−HT(HPk−HT+R)KkT=(I−KkH)Pk−−Pk−HTKkT+Pk−HTKkT=(I−KkH)Pk−
二、五朵金花
- 先验估计
x ^ k − = A x ^ k − 1 + B u k − 1 \hat{x}_{k}^-=A \hat{x}_{k-1}+B u_{k-1} x^k−=Ax^k−1+Buk−1 - 先验误差协方差矩阵
P k − = A P k − 1 A T + Q P_{k}^{-}=A P_{k-1} A^{T}+Q Pk−=APk−1AT+Q - 卡尔曼增益
K k = P k − H T H P k − H T + R K_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R} Kk=HPk−HT+RPk−HT - 后验估计
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right) x^k=x^k−+Kk(zk−Hx^k−) - 更新误差协方差矩阵
P k = ( I − K k H ) P k − P_{k}=\left(I-K_{k} H\right) P_{k}^{-} Pk=(I−KkH)Pk−
注意到，在滤波增益计算公式3中涉及矩阵求逆问题，但由于 ( H P k − H T + R ) (H P_{k}^{-} H^{T}+R) (HPk−HT+R)是对称正定的,对其求逆可采用所谓的“变量循环重新编号法”或三角分解法，有利于减少计算量或提高数值稳定性，具体可参见计算方法之类书籍,此处不再详述。
三、Kalman滤波流程图
参考文献
卡尔曼滤波与组合导航原理【西北工业大学 严恭敏】
【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)
【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式
1894
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
