卡尔曼滤波算法详细推导

首先给大家推荐一下我老师大神的人工智能教学网站。教学不仅零基础,通俗易懂,而且非常风趣幽默,还时不时有内涵黄段子!点这里可以跳转到网站

一、预备知识

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
Q

    :均值为0,协方差矩阵为,且服从正态分布的过程噪声

H

    :测量矩阵

v_k
R

    :均值为0,协方差矩阵为,且服从正态分布的测量噪声

{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

所以预测误差协方差矩阵可以由上一次算出的估计误差协方差矩阵及状态转移矩阵和过程激励噪声的协方差矩阵算得

4、总结

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

P_0
x_0
P_0
P'_1
P'_1
K_1
x_1
K_1
P_1

1步,首先,已知,然后由算出,再由算出,有了这些参数后,结合观测值就能估计出,再利用更新

P_1
P'_2
P'_2
K_2
x_2
K_2
P_2

2步,然后下次更新过程为由算出,再由算出,有了这些参数后,结合观测值就能估计出,再利用更新

……

P_{n-1}
P'_n
P'_n
K_n
x_n
K_n
P_n

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、《矩阵分析与应用》 张贤达 著

点这里可以跳转到人工智能网站

发表评论