卡尔曼滤波算法详细推导

2023-05-16

一、预备知识

1、协方差矩阵

$X$ 是一个 $n$ 维列向量， $u_i$ 是 $x_i$ 的期望，协方差矩阵为

$P=E[(X-E[X])(X-E[X])^T]$

$=\begin{bmatrix} E[(x_1-u_1)(x_1-u_1)]& E[(x_1-u_1)(x_2-u_2)]& ...& E[(x_1-u_1)(x_n-u_n)]&\\ E[(x_2-u_2)(x_1-u_1)]& E[(x_2-u_2)(x_2-u_2)]& ...& E[(x_2-u_2)(x_n-u_n)]\\ ...& ...& ...& ...&\\ E[(x_n-u_n)(x_1-u_1)]& E[(x_n-u_n)(x_2-u_2)]& ...& E[(x_n-u_n)(x_n-u_n)]& \end{bmatrix}$

可以看出

协方差矩阵都是对称矩阵且是半正定的

协方差矩阵的迹 $tr(P)$ 是 $X$ 的均方误差

2、用到的两个矩阵微分公式

公式一：

$\frac{\partial tr(AB)}{\partial A}=B^T$

公式二：若 $B$ 是对称矩阵，则下式成立

$\frac{\partial tr(ABA^T)}{\partial A}=2AB$

tr表示矩阵的迹，具体推导过程参考相关矩阵分析教程

二、系统模型与变量说明

1、系统离散型状态方程如下

由k-1时刻到k时刻，系统状态预测方程

$X_k=AX_{k-1}+Bu_k+w_k$

系统状态观测方程

$Z_k=HX_k+v_k$

2、变量说明如下

$A$ ：状态转移矩阵

$u_k$ ：系统输入向量

$B$ ：输入增益矩阵

$w_k$ ：均值为0，协方差矩阵为 $Q$ ，且服从正态分布的过程噪声

$H$ ：测量矩阵

$v_k$ ：均值为0，协方差矩阵为 $R$ ，且服从正态分布的测量噪声

初始状态以及每一时刻的噪声 ${X_0, w_1,...,w_k,v_1,...v_k}$ 都认为是互相独立的，实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。

三、卡尔曼滤波器

卡尔曼估计实际由两个过程组成：预测与校正，在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测。在校正阶段，滤波器利用对当前状态的观测值修正在预测阶段获得的预测值，以获得一个更接进真实值的新估计值。

1、变量说明

$x_k$ ：真实值

$\hat{x}_k$ ：卡尔曼估计值

$P_k$ ：卡尔曼估计误差协方差矩阵

${\hat{x_k}}'$ ：预测值

${P_k}'$ ：预测误差协方差矩阵

$K_k$ ：卡尔曼增益

$\hat{z}_k$ ：测量余量

2、卡尔曼滤波器计算过程

预测：

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

${P}'_k=AP_{k-1}A^T+Q$

校正：

$\hat{z}_k=z_k-H\hat{x}'_k$

$K_k={P}'_kH^T(H{P}'_kH^T+R)^{-1}$

$\hat{x}_k=\hat{x}'_k+K_k\hat{z}_k$

更新协方差估计：

$P_k=(I-K_kH){P}'_k$

观察以上六个式子，我们使用过程中关键要明白 ${P}'_k$ ， $K_k$ 的算法原理，及 $P_k$ 的更新算法

3、卡尔曼滤波算法详细推导

从协方差矩阵开始说起，真实值与预测值之间的误差为

${e}'_k=x_k-\hat{x}'_k$

预测误差协方差矩阵为 ${P}'_k=E[{e}'_k{{e}'_k}^T]=E[(x_k-\hat{x}'_k)(x_k-\hat{x}'_k)^T]$

真实值与估计值之间的误差为

$e_k=x_k-\hat{x}_k=x_k-(\hat{x}'_k+K_k(Hx_k+v_k-H\hat{x}'_k))$

$=(I-K_kH)(x_k-\hat{x}'_k)-K_kv_k$

卡尔曼估计误差协方差矩阵为

$P_k=E[e_ke_k^T]$

将 $e_k$ 代入得到

$P_k=E[[(I-K_kH)(x_k-\hat{x}'_k)-K_kv_k][(I-K_kH)(x_k-\hat{x}'_k)-K_kv_k]^T]$

$=(I-K_kH)E[(x_k-\hat{x}'_k)(x_k-\hat{x}'_k)^T](I-K_kH)^T+K_kE[v_k{v}^T_k]K^T$

其中 $E[v_kv_k^T]=R$ ，并将预测误差协方差矩阵代入，得到

$P_k=(I-K_kH){P}'_k(I-K_kH)^T+K_kRK_k^T$

卡尔曼滤波本质是最小均方差估计，而均方差是 $P_k$ 的迹，将上式展开并求迹

$tr(P_k)=tr({P}'_k)-2tr(K_kH{P}'_k)+tr(K_k(H{P}'_kH^T+R)K_k^T)$

最优估计 $K_k$ 使 $tr(P_k)$ 最小，所以上式两边对 $K_k$ 求导

$\frac{\partial tr(P_k)}{\partial K_k} = -\frac{\partial tr(2K_kH{P}'_k)}{\partial K_k}+\frac{\partial tr(K_k(H{P}'_kH^T+R)K_k^T)}{\partial K_k}$

套用第一节中提到的那两个矩阵微分公式，得到

$\frac{\partial tr(P_k)}{\partial K_k}=-2(H{P}'_k)^T+2K_k(H{P}'_kH^T+R)$

令上式等于0，得到

$K_k=P_k'H^T(HP_k'H^T+R)^{-1}$

到此，我们就知道了卡尔曼增益是怎么算出来的了，但是又有问题， $P'_k$ 是怎么算的呢？

$P'_k=E[(x_k-\hat{x}'_k)(x_k-\hat{x}'_k)^T]$

$=E[(Ax_{k-1}+Bu_k+w_k-A\hat{x}_{k-1}-Bu_k)(Ax_{k-1}+Bu_k+w_k-A\hat{x}_{k-1}-Bu_k)^T]$

$=E[(A(x_{k-1}-\hat{x}_{k-1})+w_k)(A(x_{k-1}-\hat{x}_{k-1})+w_k)^T]$

$=E[(Ae_{k-1})(Ae_{k-1})^T]+E[w_kw_k^T]$

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

(注意其中展开过程用到了 $E[w_k]=0$ )

所以预测误差协方差矩阵 $P'_k$ 可以由上一次算出的估计误差协方差矩阵 $P_{k-1}$ 及状态转移矩阵 $A$ 和过程激励噪声的协方差矩阵 $Q$ 算得

四、实例

在惯性器件+GPS融合定位中，卡尔曼滤波是比较常用的。这里简化举例，简化为一维的，假设小车沿一条直线行驶。里面有加速度计来计算位置，每100ms计算一次，并有GPS定位，1S更新一次。套到上述公式中，是这样操作的，在GPS没有更新的时候，只能作预测，用这几个公式：

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

${P}'_k=AP_{k-1}A^T+Q$

$P_k=(I-K_kH){P}'_k$

由于我们简化为一维，公式可以写成这样

$\hat{x}'_k = \hat{x}_{k-1}+u$

${p}'_k={p}_{k-1}+q$

$p_k = (1-k_k){p}'_{k}$

$u$ 是由加表每个计算周期的积分得到的位置增量， $q$ 就代表加表误差， $k$ 是卡尔曼增益，GPS没有更新时， $k$ 不会变，初始时设置 $k = 0$ 。可以看出随着计算次数的增加，衡量预测误差的 ${p}'$ 会越来越大。这和实际情况也是一致，积分次数越多，累积误差会越来越大。 $q$ 越大， ${p}'$ 发散得越快，传感器精度，影响预测精度。

更新了10次，1秒时间到了，GPS有更新，然后就到了校正过程，套用：

$\hat{z}_k=z_k-H\hat{x}'_k$

$K_k={P}'_kH^T(H{P}'_kH^T+R)^{-1}$

$\hat{x}_k=\hat{x}'_k+K_k\hat{z}_k$

$P_k=(I-K_kH){P}'_k$

假设位置可以直接观测，即 $H=[1]$ , 简化后

$\hat{z}_k = {z}_k - \hat{x}'_k$

${k}_k = \frac{p'_k}{p'_k + r}$

$\hat{x}_k = \hat{x}'_k + k_k\hat{z}_k = (1-k_k)\hat{x}'_{k} + k_kz_k$

由这三个公式就得到了卡尔曼估计值 $\hat{x}_k$ 。r是常量，描述GPS的测量误差。由 $k$ 的计算公式，可以看出，预测误差 ${p}'$ 越大， $k$ 越接近1，再看第三个公式，k越接近1， $\hat{x}$ 越接近测量值 $z$ ，这个公式和互补滤波的公式很像，但是这个权重k是动态变化的，这也是卡尔曼滤波的高明之处。总结就是，单凭传感器预测位置，计算次数越多，误差 $p'$ 就会越大，卡尔曼增益 $k$ 就越大，测量值权重就越大，校正时，最终估计值就会越接近测量值。校正完了之后，GPS 就又要等1秒才能更新，这1秒内，是不是只能更相信加表了？这是肯定的，

更新卡尔曼估计误差 $p$

$p_k = (1-k_k){p}'_{k}$

假如 $k$ 接近1，那么 $p$ 接近0，下次预测计算时，

${p}'_k={p}_{k-1}+q$

$p'$ 就接近 $q$ 了，说明累积的误差清零了。校正完了之后，可以更相信预测值，随着预测更新次数增加。预测误差会更大，导致校正时，卡尔曼增益会更大，更加相信观测值，周而复始。

五、总结

总结卡尔曼滤波的更新过程为

1步，首先 $P_0$ ， $x_0$ 已知，然后由 $P_0$ 算出 $P'_1$ ，再由 $P'_1$ 算出 $K_1$ ，有了这些参数后，结合观测值就能估计出 $x_1$ ，再利用 $K_1$ 更新 $P_1$

2步，然后下次更新过程为由 $P_1$ 算出 $P'_2$ ，再由 $P'_2$ 算出 $K_2$ ，有了这些参数后，结合观测值就能估计出 $x_2$ ，再利用 $K_2$ 更新 $P_2$

......

n步，由 $P_{n-1}$ 算出 $P'_n$ ，再由 $P'_n$ 算出 $K_n$ ，有了这些参数后，结合观测值就能估计出 $x_n$ ，再利用 $K_n$ 更新 $P_n$

这就是卡尔曼滤波器递推过程。

至于 $P_k$ 的算法，

$P_k=P'_k-K_kHP'_k-P'_kH^TK_k^T+K_k(HP'_kH^T+R)K_k^T$

将 $K_k$ 代入上式右边最后一项中， $K_k^T$ 保持原样

$P_k=P'_k-K_kHP'_k-P'_kH^TK_k^T+P'_kH^T(HP'_kH^T+R)^{-1}(HP'_kH^T+R)K_k^T$

$=P'_k-K_kHP'_k$

$=(I-K_kH)P'_k$

（转载请声明出处谢谢合作）

reference：

1、https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2

2、《矩阵分析与应用》张贤达著

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)