卡尔曼滤波算法详细推导「建议收藏」

卡尔曼滤波算法详细推导「建议收藏」一、预备知识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算得

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

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

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/130649.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • 数组的前缀和及查分数组

    数组的前缀和及查分数组1,前缀和主要适用场景是原始数组不会被修改的情况下,频繁查询某个区间的累加和。这里就不写前缀和的代码了,就是用一个数组记录下原有数组的前缀和。比如,prefix[i]就代表着nums[0…i-1]所有元素的累加和,如果我们想求区间nums[i…j]的累加和,只要计算prefix[j+1]-prefix[i]即可,而不需要遍历整个区间求和。(需要注意的是使用场景是频繁查询某个区间的累加和,而不需要对原始数组进行频繁修改)2,查分数组的主要适用场景是**频繁对原始数组的某个区间的元素进行增减。**比

  • 771服务器cpu性能排行,CPU114查询网

    771服务器cpu性能排行,CPU114查询网4485083264155WQ4201954736564128225WQ2201964606764128200WQ220197392991632155WQ420198392701632105WQ4201910358722448180WQ420191333158122465WQ2201914318681224105WQ22019183067912246…

  • java switch是什么意思_java switch语句详解[通俗易懂]

    java switch是什么意思_java switch语句详解[通俗易懂]switch语句的格式:switch(整型或字符型变量){case变量可能值1:分支一;break;case变量可能值2:分支二;break;case变量可能值3:分支三;break;…default:最后分支;}在switch的语法里,我们要学到4个关键字:switch、case、break、default。在 switch(变量) 这一行里,变量只能是整型…

  • IE阻止了此网站安装ActiveX控件

    IE阻止了此网站安装ActiveX控件解决方法:工具——Internet选项——安全受信任站点,然后点击站点,把地址(这个你自己确定该站点是不是可信任的)复制进去,然后点添加,再点关闭然后继续点可信任站点,再点下面的自定义级别,在弹出的对话框里面对所有关于activex的选项做如下操作1:如果之前是启用或者提示的,不修改2:如果之前是禁用的,改成启用或提示重新访问此站点,即可。

  • QStringList自定义排序

    QStringList自定义排序假如有一个QStringList如下:QStringListkeys=QStringList()<<“1″<<“9″<<“7″<<“5″<<“3”;这样既可实现排序:qSort(keys.begin(),keys.end(),[](constQString&s1,constQStr…

  • idea激活码2021.4.21【在线破解激活】

    idea激活码2021.4.21【在线破解激活】,https://javaforall.cn/100143.html。详细ieda激活码不妨到全栈程序员必看教程网一起来了解一下吧!

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号