卡尔曼滤波算法详细推导方法_卡尔曼滤波5个公式[通俗易懂]

卡尔曼滤波算法详细推导方法_卡尔曼滤波5个公式[通俗易懂]一、预备知识1、协方差矩阵是一个维列向量,是的期望,协方差矩阵为可以看出协方差矩阵都是对称矩阵且是半正定的协方差矩阵的迹是的均方误差2、用到的两个矩阵微分公式公式一:公式二:若是对称矩阵,则下式成立……_卡尔曼滤波推导

一、预备知识

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算得

四、实例

在惯性器件+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_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、《矩阵分析与应用》 张贤达 著

今天的文章卡尔曼滤波算法详细推导方法_卡尔曼滤波5个公式[通俗易懂]分享到此就结束了,感谢您的阅读。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/71952.html

(0)
编程小号编程小号

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注