卡尔曼滤波算法详细推导

一、预备知识

1、协方差矩阵

    X是一个n维列向量,u_ix_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}'_kK_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、总结

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

1步,首先P_0x_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、《矩阵分析与应用》 张贤达 著

 

 

 

 

 

 

 

 

 

 

你可能感兴趣的:(算法推导,滤波算法,卡尔曼滤波)