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

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

相关推荐

  • floyed「建议收藏」

    floyed「建议收藏」#include<iostream>#include<cstdio>#include<cstdlib>#include<cstring>#include<algorithm>usingnamespacestd;constintmaxn=405;constintinf=4000005;intd1[maxn][m…

  • 消除“Permission is only granted to system apps”错误[通俗易懂]

    消除“Permission is only granted to system apps”错误

  • 网站防攻击

    网站防攻击网站如何防御DDos攻击和CC攻击?网站如何防御DDoS攻击和CC攻击?如果网站所在的服务器不是高仿服务器,一旦遭遇DDos攻击或大量的CC攻击,那么自己的网站很有可能会处于瘫痪状态,甚至会引起服务器所在的整个机房服务器受影响,自己的服务器则很有可能陷入“黑洞”或者机房管理员暂时封闭IP,给网站拥有者带来严重的影响。一、网站为什么会遭遇DDos攻击或CC攻击?网站之所以会遭受DDo…

  • Oracle insert all语句介绍

    Oracle insert all语句介绍Oracle中insert语句的高级用法,INSERTALL语句介绍:1、无条件insertall全部插入CREATETABLEt1(product_idNUMBER,product_nameVARCHAR2(80),MONTHNUMBER);INSERTINTOt1VALUES(111,’苹果’,1);INSERTINTOt1VALUES(222,’橘…

  • 嵌套事务回滚策略_内部事务回滚会导致外部事务回滚

    嵌套事务回滚策略_内部事务回滚会导致外部事务回滚嵌套事务回滚策略

  • linux工具箱_docker ps命令

    linux工具箱_docker ps命令1.gdb调试利器GDB是一个由GNU开源组织发布的、UNIX/LINUX操作系统下的、基于命令行的、功能强大的程序调试工具。对于一名Linux下工作的c++程序员,gdb是必不可少的工具;1.1.启动gdb对C/C++程序的调试,需要在编译前就加上-g选项:$g++-ghello.cpp-ohello调试可执行文件:$gdbprogram也就是你的执行文件,一般在当前目录下。调试core文件(core是程序非法执行后coredump后产生的文件):$gdb$gdb

发表回复

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

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