卡尔曼滤波（Kalman Filtering）——（5）协方差矩阵与Kalman滤波流程图_综合

协方差矩阵

一、协方差矩阵公式推导
二、五朵金花
三、Kalman滤波流程图
参考文献

一、协方差矩阵公式推导

已知 $Pk?=E[ek?ek?T],ek?=xk?x^k?P_{k}^{-}=E\left[e_{k}^{-} e_{k}^{-T}\right], e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}$ , 有:

$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\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}$
$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]\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}$

由已知条件可知， $e_{k-1}$ ? 是第 $k ? 1$ 次的估计误差，而 $w_{k-1}$ ?是计算第 $k$ 个真实值的噪声，所以 $e_{k-1}$ 和 $w_{k-1}$ ?相互独立。根据上述结论可得先验误差协方差矩阵：

$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\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}$
上期博客已经推导出 $K_{k}$ 的表达式，将其代入到 $P_{k}$ 表达式中，可得：
$Pk=Pk??KkHPk??Pk?HTKkT+KkHPk?HTKkT+KkRKkT=(I?KkH)Pk??Pk?HTKkT+Kk(HPk?HT+R)KkT=(I?KkH)Pk??Pk?HTKkT+Pk?HTHPk?H+R(HPk?HT+R)KkT=(I?KkH)Pk??Pk?HTKkT+Pk?HTKkT=(I?KkH)Pk?\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}$

二、五朵金花

先验估计
$x^k?=Ax^k?1+Buk?1\hat{x}_{k}^-=A \hat{x}_{k-1}+B u_{k-1}$
先验误差协方差矩阵
$P_{k}^{-}=A P_{k-1} A^{T}+Q$
卡尔曼增益
$Kk=Pk?HTHPk?HT+RK_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R}$
后验估计
$x^k=x^k?+Kk(zk?Hx^k?)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)$
更新误差协方差矩阵
$Pk=(I?KkH)Pk?P_{k}=\left(I-K_{k} H\right) P_{k}^{-}$

注意到，在滤波增益计算公式3中涉及矩阵求逆问题，但由于 $H P_{k}^{-} H^{T}+R)$ 是对称正定的,对其求逆可采用所谓的“变量循环重新编号法”或三角分解法，有利于减少计算量或提高数值稳定性，具体可参见计算方法之类书籍,此处不再详述。

三、Kalman滤波流程图

参考文献

卡尔曼滤波与组合导航原理【西北工业大学严恭敏】
【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)
【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式