卡尔曼滤波器原理详细推导

去学点东西吧

已于 2023-10-09 11:00:35 修改

阅读量712

点赞数

分类专栏：算法控制理论文章标签：算法学习

于 2022-08-07 11:13:35 首次发布

本文链接：https://blog.csdn.net/weixin_43487974/article/details/125965145

版权

算法同时被 2 个专栏收录

6 篇文章 3 订阅

订阅专栏

控制理论

4 篇文章 6 订阅

订阅专栏

Kalman滤波器的设计可以分为三步：

Step1：计算卡尔曼增益（Kalman Gain）

$K_{k}=\frac{e_{k-1(EST)}}{e_{k-1(EST)}+e_{k(MEA))}}$

$e_{k-1(EST)}$ 为k-1时刻的估计误差， $e_{k(MEA))}$ 为k时刻的测量误差。

Step2：计算k时刻的状态变量估计值

$\hat{x}_{k}=\hat{x}_{k-1}+K_{k}(z_{k}-\hat{x}_{k-1})$

$z_{k}$ 为k时刻测量值。

Step3：更新k时刻的估计误差

$e_{k(EST)}=(1-K_{k})e_{k-1(EST)}$

具体原理及详细推导：

首先给出状态空间方程（离散形式）

$\left\{\begin{matrix} x_{k}=Ax_{k-1}+Bu_{k-1}+\omega_{k-1} \\ z_{k}=Hx_{k}+\upsilon _{k} \end{matrix}\right.$

其中 $\omega$ 为过程噪声， $\upsilon$ 为测量噪声，且满足正态分布

$P(\omega )\sim (0,Q)\; \; \; \; \; P(\upsilon )\sim (0,R)$

Q、R为协方差矩阵且

$Q=E[\omega \omega ^{T}]\; \; \; \; \; R=E[\upsilon \upsilon ^{T}]$

补充：

$Var(x)=E(x^{2})-E^{2}(x)\; \; \; \; \; Cov(x,y)=E(xy)-E(x)E(y)$

假设

$x_{k}=\begin{bmatrix} x_{1}\\ x_{2} \end{bmatrix}$ ，

则

$E[\omega \omega ^{T}]=E\begin{bmatrix} \omega_{1}\\\omega_{2} \end{bmatrix}\begin{bmatrix} \omega_{1}&\omega_{2} \end{bmatrix} =\begin{bmatrix} E[\omega_{1}^{2}] & E[\omega_{1}\omega_{2}] \\ E[\omega_{2}\omega_{1}] & E[\omega_{2}^{2}] \end{bmatrix} =\begin{bmatrix} \sigma _{\omega_{1}}^{2} & \sigma _{\omega_{1}}\sigma _{\omega_{2}} \\ \sigma _{\omega_{2}}\sigma _{\omega_{1}} & \sigma _{\omega_{2}}^{2} \end{bmatrix}$

$\sigma_{\omega _{1}}^{2}$ 和 $\sigma_{\omega _{1}}^{2}$ 分别表示方差 $Var(\omega _{1})$ 和 $Var(\omega _{2})$ ， $\sigma _{\omega _{1}}\sigma _{\omega _{2}}$ 和 $\sigma _{\omega _{2}}\sigma _{\omega _{1}}$ 分别表示协方差 $Cov(\omega _{1},\omega _{2})$ 和 $Cov(\omega _{2},\omega _{1})$ 。

所以协方差矩阵 $Q=E[\omega \omega ^{T}]$ 。

建模过程中 $\omega$ 和 $\upsilon$ 未知，无法准确得到 $x_{k}$ 和 $z_{k}$ ，为方便表示将上述状态方程去掉噪声直接计算出来的状态变量定义为先验估计值，即

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

另一方面由于测量结果 $z_{k}$ 已知，所以这里可以得到测量相关的估计值 $\hat{x}_{k(MEA)}$

$z_{k}=Hx_{k}\rightarrow \hat{x}_{k(MEA)}=H^{-1}z_{k}$

注：不管是上述计算得到的状态变量（先验估计值）还是测量相关的状态变量（测量估计值）都不准确。

因此为了得到一个相对准确的状态变量估计结果（后验估计值），可以使用数据融合（Data Fusion）的方法通过先验估计值和测量估计值对其进行表示

$\begin{aligned} \hat{x}_{k}=\hat{x}_{k}^{-}+G(\hat{x}_{k(MEA)}-\hat{x}_{k}^{-} )=\hat{x}_{k}^{-}+G(H^{-1}z_{k}-\hat{x}_{k}^{-} )\xrightarrow{G=K_{k}H}\hat{x}_{k}^{-}+K_{k}(z_{k}-H\hat{x}_{k}^{-})\end{aligned}$

其中 $G\in [0,1],K_{k}\in[0,H^{-1}]$ 。

目标：寻找卡尔曼增益 $K_{k}$ 使得 $\hat{x}_{k}\rightarrow x_{k}$

引入误差 $e_{k}$ 和先验误差 $e_{k}^{-}$

$e_{k}=x_{k}-\hat{x}_{k}$

$\begin{aligned} e_{k}^{-}&=x_{k}-\hat{x}_{k}^{-} \\&=Ax_{k-1}+Bu_{k-1}+\omega _{k-1}-A\hat{x}_{k-1}-Bu_{k-1} \\&=A(x_{k-1}-\hat{x}_{k-1})+\omega _{k-1} \\&=Ae_{k-1}+\omega _{k-1} \end{aligned}$

且满足

$\begin{aligned} &P(e)\sim (0,P) \\&P=E[ee^{T}]\end{aligned}$

所以误差协方差 $P_{k}$

$\begin{aligned} P_{k}= &E[e_{k}e_{k}^{T}]=E[(x_{k}-\hat{x}_{k})(x_{k}-\hat{x}_{k})^{T}] \\=&E[[(I-K_{k}H)e_{k}^{-}-K_{k}\upsilon _{k}] [(I-K_{k}H)e_{k}^{-}-K_{k}\upsilon _{k}]^{T}] \\=&E[[(I-K_{k}H)e_{k}^{-}-K_{k}\upsilon _{k}]][e_{k}^{-T}(I-K_{k}H)^{T}-\upsilon _{k}^{T}K_{k}^{T}]] \\=&E[(I-K_{k}H)e_{k}^{-}e_{k}^{-T}(I-K_{k}H)^{T}-(I-K_{k}H)e_{k}^{-}\upsilon _{k}^{T}K_{k}^{T}-K_{k}\upsilon _{k}e_{k}^{-T}(I-K_{k}H)^{T}+K_{k}\upsilon _{k}\upsilon _{k}^{T}K_{k}^{T}] \\=&(I-K_{k}H)E[e_{k}^{-}e_{k}^{-T}](I-K_{k}H)^{T}-(I-K_{k}H)E[e_{k}^{-}\upsilon _{k}^{T}]K_{k}^{T}-K_{k}^{T}E[\upsilon _{k}e_{k}^{-T}](I-K_{k}H)^{T}+K_{k}E[\upsilon _{k}\upsilon _{k}^{T}]K_{k}^{T} \\=&(I-K_{k}H)P_{k}^{-}(I-K_{k}H)^{T}+K_{k}RK_{k}^{T} \\=&(P_{k}^{-}-K_{k}HP_{k}^{-})(I-H^{T}K_{k}^{T})+K_{k}RK_{k}^{T} \\=&P_{k}^{-}-K_{k}HP_{k}^{-}-P_{k}^{-}H^{T}K_{k}^{T}+K_{k}HP_{k}^{-}H^{T}K_{k}^{T}+K_{k}RK_{k}^{T} \end{aligned}$

其中 $e$ 和 $\upsilon$ 相互独立，因此 $E[e_{k}^{-}\upsilon _{k}^{T}]=E[\upsilon _{k}e_{k}^{-T}]=0$ 。

同样可以求得先验误差协方差 $P_{k}^{-}$

$\begin{aligned} P_{k}^{-}&=E[e_{k}^{-}e_{k}^{-T}] \\&=E[(Ae_{k-1}+\omega _{k-1})(e_{k-1}^{T}A^T+\omega _{k-1}^{T})] \\&=E[Ae_{k-1}e_{k-1}^{T}A^T+Ae_{k-1}\omega _{k-1}^{T}+\omega _{k-1}e_{k-1}^{T}A^T+\omega _{k-1}\omega _{k-1}^{T}] \\&=E[Ae_{k-1}e_{k-1}^{T}A^T]+E[Ae_{k-1}\omega _{k-1}^{T}]+E[\omega _{k-1}e_{k-1}^{T}A^T]+E[\omega _{k-1}\omega _{k-1}^{T}] \\&=AE[e_{k-1}e_{k-1}^{T}]A^T+E[\omega _{k-1}\omega _{k-1}^{T}] \\&=AP_{k-1}A^T+Q \end{aligned}$

其中 $e_{k-1}$ 和 $\omega_{k-1}$ 相互独立，因此 $E[Ae_{k-1}\omega _{k-1}^{T}]=E[\omega _{k-1}e_{k-1}^{T}A^T]=0$ 。

以二维状态变量为例

$P=E[ee^{T}]=\begin{bmatrix} \sigma _{e_{1}}^{2} & \sigma _{e_{1}}\sigma _{e_{2}} \\ \sigma _{e_{2}}\sigma _{e_{1}} & \sigma _{e_{2}}^{2} \end{bmatrix}$

$tr(P)=\sigma _{e_{1}}^{2}+\sigma _{e_{2}}^{2}$

所以要使误差（协方差P）最小，只需保证协方差矩阵的迹 $tr(P)$ 最小。

$tr(P_{k})=tr(P_{k}^{-})-2tr(K_{k}HP_{k}^{-})+tr(K_{k}HP_{k}^{-}H^{T}K_{k}^{T})+tr(K_{k}RK_{k}^{T})$ $\begin{aligned} \frac{\mathrm{d(tr(P_{k}))} }{\mathrm{d} K_{k}}&=0-2(HP_{k}^{-})^{T}+2K_{k}HP_{k}^{-}H^{T}+2K_{k}R=0 \\&\Rightarrow K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R} \end{aligned}$

总结

预测环节

（1）计算先验估计值 $\hat{x}_{k}^{-}$

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

（2）计算先验误差协方差 $P_{k}^{-}$

$P_{k}^{-}=AP_{k-1}A^T+Q$

校正环节

（1）计算卡尔曼增益 $K_{k}$

$K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R}$

（2）计算状态（后验）估计值 $\hat{x_{k}}$

$\hat{x}_{k}^{-}+K_{k}(z_{k}-H\hat{x}_{k}^{-})$

（3）更新误差协方差 $P_{k}$

$\begin{aligned} P_{k}&=P_{k}^{-}-K_{k}HP_{k}^{-}-P_{k}^{-}H^{T}K_{k}^{T}+K_{k}HP_{k}^{-}H^{T}K_{k}^{T}+K_{k}RK_{k}^{T} \\&=P_{k}^{-}-K_{k}HP_{k}^{-}-P_{k}^{-}H^{T}K_{k}^{T}+K_{k}(HP_{k}^{-}H^{T}+R)K_{k}^{T} \\&\overset{K_{k}}{=}P_{k}^{-}-K_{k}HP_{k}^{-} \\&=(I-K_{k}H)P_{k}^{-} \end{aligned}$