卡尔曼滤波 订阅
卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。 展开全文
卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。
信息
提出时间
1958
表达式
X(k)=A X(k-1)+B U(k)+W(k)
提出者
斯坦利·施密特
应用学科
天文,宇航,气象
中文名
卡尔曼滤波器,Kalman滤波,卡曼滤波
适用领域范围
控制、制导、导航、通讯等现代工程
外文名
KALMAN FILTER
卡尔曼滤波背景
斯坦利·施密特(Stanley Schmidt)首次实 现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。
收起全文
精华内容
下载资源
问答
  • 详解卡尔曼滤波原理

    万次阅读 多人点赞 2017-03-18 13:54:15
    详解卡尔曼滤波原理 在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者...

    详解卡尔曼滤波原理

      在网上看了不少与卡尔曼滤波相关的博客、论文,要么是只谈理论、缺乏感性,或者有感性认识,缺乏理论推导。能兼顾二者的少之又少,直到我看到了国外的一篇博文,真的惊艳到我了,不得不佩服作者这种细致入微的精神,翻译过来跟大家分享一下,原文链接:http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
      我不得不说说卡尔曼滤波,因为它能做到的事情简直让人惊叹!意外的是很少有软件工程师和科学家对对它有所了解,这让我感到沮丧,因为卡尔曼滤波是一个如此强大的工具,能够在不确定性中融合信息,与此同时,它提取精确信息的能力看起来不可思议。

    什么是卡尔曼滤波?

      你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波总是能指出真实发生的情况。
      在连续变化的系统中使用卡尔曼滤波是非常理想的,它具有占用内存小的优点(除了前一个状态量外,不需要保留其它历史数据),并且速度很快,很适合应用于实时问题和嵌入式系统。
      在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂,这个状况有点糟糕。实际上,如果以正确的方式看待它,卡尔曼滤波是非常简单和容易理解的,下面我将用漂亮的图片和色彩清晰的阐述它,你只需要懂一些基本的概率和矩阵的知识就可以了。

    我们能用卡尔曼滤波做什么?

      用玩具举例:你开发了一个可以在树林里到处跑的小机器人,这个机器人需要知道它所在的确切位置才能导航。

    这里写图片描述

      我们可以说机器人有一个状态 这里写图片描述,表示位置和速度:

    这里写图片描述

      注意这个状态只是关于这个系统基本属性的一堆数字,它可以是任何其它的东西。在这个例子中是位置和速度,它也可以是一个容器中液体的总量,汽车发动机的温度,用户手指在触摸板上的位置坐标,或者任何你需要跟踪的信号。
      这个机器人带有GPS,精度大约为10米,还算不错,但是,它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖,如果机器人走错了一步,就有可能掉下悬崖,所以只有GPS是不够的。

    这里写图片描述

      或许我们知道一些机器人如何运动的信息:例如,机器人知道发送给电机的指令,知道自己是否在朝一个方向移动并且没有人干预,在下一个状态,机器人很可能朝着相同的方向移动。当然,机器人对自己的运动是一无所知的:它可能受到风吹的影响,轮子方向偏了一点,或者遇到不平的地面而翻倒。所以,轮子转过的长度并不能精确表示机器人实际行走的距离,预测也不是很完美。
      GPS 传感器告诉了我们一些状态信息,我们的预测告诉了我们机器人会怎样运动,但都只是间接的,并且伴随着一些不确定和不准确性。但是,如果使用所有对我们可用的信息,我们能得到一个比任何依据自身估计更好的结果吗?回答当然是YES,这就是卡尔曼滤波的用处。

    卡尔曼滤波是如何看到你的问题的

      下面我们继续以只有位置和速度这两个状态的简单例子做解释。

    这里写图片描述

      我们并不知道实际的位置和速度,它们之间有很多种可能正确的组合,但其中一些的可能性要大于其它部分:

      卡尔曼滤波假设两个变量(位置和速度,在这个例子中)都是随机的,并且服从高斯分布。每个变量都有一个均值 μ,表示随机分布的中心(最可能的状态),以及方差 这里写图片描述,表示不确定性。

      在上图中,位置和速度是不相关的,这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣:位置和速度是相关的,观测特定位置的可能性取决于当前的速度:

      这种情况是有可能发生的,例如,我们基于旧的位置来估计新位置。如果速度过高,我们可能已经移动很远了。如果缓慢移动,则距离不会很远。跟踪这种关系是非常重要的,因为它带给我们更多的信息:其中一个测量值告诉了我们其它变量可能的值,这就是卡尔曼滤波的目的,尽可能地在包含不确定性的测量数据中提取更多信息!
      这种相关性用协方差矩阵来表示,简而言之,矩阵中的每个元素 这里写图片描述 表示第 i 个和第 j 个状态变量之间的相关度。(你可能已经猜到协方差矩阵是一个对称矩阵,这意味着可以任意交换 i 和 j)。协方差矩阵通常用“这里写图片描述”来表示,其中的元素则表示为“这里写图片描述 ”。

    使用矩阵来描述问题

      我们基于高斯分布来建立状态变量,所以在时刻 k 需要两个信息:最佳估计 这里写图片描述(即均值,其它地方常用 μ 表示),以及协方差矩阵 这里写图片描述

    这里写图片描述        (1)

      (当然,在这里我们只用到了位置和速度,实际上这个状态可以包含多个变量,代表任何你想表示的信息)。接下来,我们需要根据当前状态k-1 时刻)来预测下一状态k 时刻)。记住,我们并不知道对下一状态的所有预测中哪个是“真实”的,但我们的预测函数并不在乎。它对所有的可能性进行预测,并给出新的高斯分布。

      我们可以用矩阵 这里写图片描述 来表示这个预测过程:

      它将我们原始估计中的每个点都移动到了一个新的预测位置,如果原始估计是正确的话,这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示:

    这里写图片描述

      现在,我们有了一个预测矩阵来表示下一时刻的状态,但是,我们仍然不知道怎么更新协方差矩阵。此时,我们需要引入另一个公式,如果我们将分布中的每个点都乘以矩阵 A,那么它的协方差矩阵 这里写图片描述 会怎样变化呢?很简单,下面给出公式:

    这里写图片描述

      结合方程(4)和(3)得到:

    这里写图片描述

    外部控制量

      我们并没有捕捉到一切信息,可能存在外部因素会对系统进行控制,带来一些与系统自身状态没有相关性的改变。
      以火车的运动状态模型为例,火车司机可能会操纵油门,让火车加速。相同地,在我们机器人这个例子中,导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息,我们可以用一个向量这里写图片描述来表示,将它加到我们的预测方程中做修正。
      假设由于油门的设置或控制命令,我们知道了期望的加速度a,根据基本的运动学方程可以得到:

    这里写图片描述

      以矩阵的形式表示就是:

    这里写图片描述

      这里写图片描述称为控制矩阵,这里写图片描述称为控制向量(对于没有外部控制的简单系统来说,这部分可以忽略)。让我们再思考一下,如果我们的预测并不是100%准确的,该怎么办呢?

    外部干扰

      如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的,则不会出现什么问题。
      但是,如果存在未知的干扰呢?例如,假设我们跟踪一个四旋翼飞行器,它可能会受到风的干扰,如果我们跟踪一个轮式机器人,轮子可能会打滑,或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪,如果没有把这些外部干扰考虑在内,我们的预测就会出现偏差。
      在每次预测之后,我们可以添加一些新的不确定性来建立这种与“外界”(即我们没有跟踪的干扰)之间的不确定性模型:

      原始估计中的每个状态变量更新到新的状态后,仍然服从高斯分布。我们可以说这里写图片描述的每个状态变量移动到了一个新的服从高斯分布的区域,协方差为这里写图片描述。换句话说就是,我们将这些没有被跟踪的干扰当作协方差为这里写图片描述噪声来处理。

      这产生了具有不同协方差(但是具有相同的均值)的新的高斯分布。

      我们通过简单地添加这里写图片描述得到扩展的协方差,下面给出预测步骤的完整表达式:

    这里写图片描述

      由上式可知,新的最优估计是根据上一最优估计预测得到的,并加上已知外部控制量修正
      而新的不确定性上一不确定预测得到,并加上外部环境的干扰
      好了,我们对系统可能的动向有了一个模糊的估计,用这里写图片描述这里写图片描述来表示。如果再结合传感器的数据会怎样呢?

    用测量值来修正估计值

      我们可能会有多个传感器来测量系统当前的状态,哪个传感器具体测量的是哪个状态变量并不重要,也许一个是测量位置,一个是测量速度,每个传感器间接地告诉了我们一些状态信息。

      注意,传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样,我们用矩阵 这里写图片描述 来表示传感器的数据。

      我们可以计算出传感器读数的分布,用之前的表示方法如下式所示:

    这里写图片描述

      卡尔曼滤波的一大优点就是能处理传感器噪声,换句话说,我们的传感器或多或少都有点不可靠,并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

      从测量到的传感器数据中,我们大致能猜到系统当前处于什么状态。但是由于存在不确定性,某些状态可能比我们得到的读数更接近真实状态。

      我们将这种不确定性(例如:传感器噪声)用协方差这里写图片描述表示,该分布的均值就是我们读取到的传感器数据,称之为这里写图片描述
    现在我们有了两个高斯分布,一个是在预测值附近,一个是在传感器读数附近。

      我们必须在预测值粉红色)和传感器测量值绿色)之间找到最优解。
      那么,我们最有可能的状态是什么呢?对于任何可能的读数这里写图片描述,有两种情况:(1)传感器的测量值;(2)由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率,将这两个高斯分布相乘就可以了。

      剩下的就是重叠部分了,这个重叠部分的均值就是两个估计最可能的值,也就是给定的所有信息中的最优估计
      瞧!这个重叠的区域看起来像另一个高斯分布。

      如你所见,把两个具有不同均值和方差的高斯分布相乘,你会得到一个新的具有独立均值和方差的高斯分布!下面用公式讲解。

    融合高斯分布

      先以一维高斯分布来分析比较简单点,具有方差 这里写图片描述 和 μ 的高斯曲线可以用下式表示:

    这里写图片描述

      如果把两个服从高斯分布的函数相乘会得到什么呢?

    这里写图片描述

      将式(9)代入到式(10)中(注意重新归一化,使总概率为1)可以得到:

    这里写图片描述

      将式(11)中的两个式子相同的部分用 k 表示:

    这里写图片描述

      下面进一步将式(12)和(13)写成矩阵的形式,如果 Σ 表示高斯分布的协方差,这里写图片描述 表示每个维度的均值,则:

    这里写图片描述

      矩阵这里写图片描述称为卡尔曼增益,下面将会用到。放松!我们快要完成了!

    将所有公式整合起来

      我们有两个高斯分布,预测部分这里写图片描述,和测量部分这里写图片描述,将它们放到式(15)中算出它们之间的重叠部分:

    这里写图片描述

      由式(14)可得卡尔曼增益为:

    这里写图片描述

      将式(16)和式(17)的两边同时左乘矩阵的逆(注意这里写图片描述里面包含了 这里写图片描述 )将其约掉,再将式(16)的第二个等式两边同时右乘矩阵 这里写图片描述 的逆得到以下等式:

    这里写图片描述

      上式给出了完整的更新步骤方程。这里写图片描述就是新的最优估计,我们可以将它和这里写图片描述放到下一个预测更新方程中不断迭代。

    总结

      以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的话,你可以根据式(4)和(15)重新推导一下)
      我们可以用这些公式对任何线性系统建立精确的模型,对于非线性系统来说,我们使用扩展卡尔曼滤波,区别在于EKF多了一个把预测和测量部分进行线性化的过程。

    (ps: 第一次用Markdown,添加图片和公式心累啊,什么时候能直接拖拽就好了~~)

    附Markdown使用技巧:
    1. 改变文本字体、字号与颜色。参考链接:(http://blog.csdn.net/testcs_dn/article/details/45719357/)
    2. 在线公式编辑器,编辑好了右键“复制图片地址”,当作图片来添加。
     链接:(http://private.codecogs.com/latex/eqneditor.php)
    3. 段落首行缩进,按Shift+Space将输入法切换到全角状态,然后敲空格即可,一个空格代表一个汉字的间隔。
    4. 设置图片大小及居中显示。参考链接:(http://blog.csdn.net/soindy/article/details/50427079)
    5. 不懂百度。

    展开全文
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...

    更新日志:

    2020.02.13:修改了第三节推导中的公式错误

    2020.03.21:修改了2.1节中的部分表述和公式加粗,补充迹的求导公式

    1 简介

    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。

    EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用卡尔曼滤波框架对信号进行滤波,因此它是一种次优滤波。

     

    2 算法介绍

    2.1 泰勒级数展开

    泰勒级数展开是将一个在x=x_{0}处具有n阶导数的函数f(x),利用关于(x-x_{0})n次多项式逼近函数值的方法。

    若函数f(x)在包含x_{0}的某个闭区间[a,b]上具有n阶导数,且在开区间(a,b)上具有(n+1)阶导数,则对闭区间[a,b]上的任意一点x,都有:

    f(x)=\frac{f({{x}_{0}})}{0!}+\frac{f'({{x}_{0}})}{1!}(x-{{x}_{0}})+...+\frac{{{f}^{(n)}}({{x}_{0}})}{n!}{{(x-{{x}_{0}})}^{n}}+{{R}_{n}}(x)

    其中{{f}^{(n)}}({{x}_{0}})表示函数f(x)x=x_{0}处的n阶导数,等式右边成为泰勒展开式,剩余的{{R}_{n}}(x)是泰勒展开式的余项,是(x-x_{0})^{n}的高阶无穷小。

    (著名的欧拉公式{{e}^{ix}}=\cos x +i\sin x就是利用{{e}^{ix}}\cos x\sin x的泰勒展开式得来的!)

    当变量是多维向量时,一维的泰勒展开就需要做拓展,具体形式如下:

    f(\mathbf{x})=f({{\mathbf{x}}_{k}})+{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}(\mathbf{x}-{{\mathbf{x}}_{k}})+\frac{1}{2!}{{(\mathbf{x}-{{\mathbf{x}}_{k}})}^{T}}H({{\mathbf{x}}_{k}})(\mathbf{x}-{{\mathbf{x}}_{k}})+{{o}^{n}}

    其中,{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}={{\mathbf{J}}_{F}}表示雅克比矩阵,\mathbf{H}({{\mathbf{x}}_{k}})表示海塞矩阵,{{\mathbf{o}}^{n}}表示高阶无穷小。

    {[\nabla f({{\bf{x}}_k})]^T} = {{\bf{J}}_F} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_n}}}}\\ {\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_n}}}} \end{array}} \right]

    {\bf{H}}({{\bf{x}}_k}) = \left[ {\begin{array}{*{20}{c}} {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_1^2}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_n}}}}\\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_2^2}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_n^2}}} \end{array}} \right]

    这里,f({{\bf{x}}_k})m维,{{\bf{x}}_k}状态向量为n维,\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_m}\partial {x_n}}} = \frac{{\partial f{{({{\bf{x}}_k})}^T}}}{{\partial {x_m}}}\frac{{\partial f({{\bf{x}}_k})}}{{\partial {x_n}}}

    一般来说,EKF在对非线性函数做泰勒展开时,只取到一阶导和二阶导,而由于二阶导的计算复杂性,更多的实际应用只取到一阶导,同样也能有较好的结果。取一阶导时,状态转移方程和观测方程就近似为线性方程,高斯分布的变量经过线性变换之后仍然是高斯分布,这样就能够延用标准卡尔曼滤波的框架。

     

    2.1 EKF

    标准卡尔曼滤波KF的状态转移方程和观测方程为

    {{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}

    {{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}

    扩展卡尔曼滤波EKF的状态转移方程和观测方程为

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}          (1)

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}             (2)

    利用泰勒展开式对(1)式在上一次的估计值\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle处展开得

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)+{{\mathbf{s}}_{k}}          (3)

    再利用泰勒展开式对(2)式在本轮的状态预测值\mathbf{\theta }_{k}^{'}处展开得

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{v}}_{k}}            (4)

    其中,{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别表示函数f(\mathbf{\theta })h(\mathbf{\theta })\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle\mathbf{\theta }_{k}^{'}处的雅克比矩阵。

    (注:这里对泰勒展开式只保留到一阶导,二阶导数以上的都舍去,噪声假设均为加性高斯噪声)

     

    基于以上的公式,给出EKF的预测(Predict)和更新(Update)两个步骤:

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    其中的雅克比矩阵{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别为

    {{\mathbf{F}}_{k-1}}={{\left. \frac{\partial f}{\partial \mathbf{\theta }} \right|}_{\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle }}{{\mathbf{H}}_{k}}={{\left. \frac{\partial h}{\partial \mathbf{\theta }} \right|}_{\mathbf{\theta }_{k}^{'}}}

    雅可比矩阵的计算,在MATLAB中可以利用对自变量加上一个eps(极小数),然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。

    读者可能好奇?为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢?以下做一个简单的推导。

     

    3 推导

    先列出几个变量的表示、状态转移方程和观测方程:

    真实值{{\mathbf{\theta }}_{k}},预测值\mathbf{\theta }_{k}^{'},估计值\left\langle {{\mathbf{\theta }}_{k}} \right\rangle,观测值{{\mathbf{z}}_{k}},观测值的预测\mathbf{\hat{z}}_{k},估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},求期望的符号\left\langle \cdot \right\rangle

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}},     {{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}},     {{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})

    引入反馈:\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-{{{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)      (5)

     

    OK,可以开始推导了:

    由公式(3)(4)得到以下两个等式,标为式(6)(7)

    f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )={{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)

    h({{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)={{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{{'}} \right)

    计算估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},并把式子(4)(5)(7)代入,得到

    {{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right){{\left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right)}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]{{\left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]}^{T}} \right\rangle

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}}{\mathbf{R}}{\mathbf{K}}_{k}}^{T}

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\mathbf{\Sigma }_{k}^{'}{{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}}{\mathbf{R}}{\mathbf{K}}_{k}}^{T}

    其中\mathbf{\Sigma }_{k}^{'}表示真实值与与预测值之间的误差协方差矩阵。于是得到式(8)

    {{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}^{T}}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}

    因为{{\mathbf{\Sigma }}_{k}}的对角元即为真实值与估计值的误差的平方,矩阵的迹(用T[]表示)即为总误差的平方和,即

    T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'} \right]

    利用以下矩阵迹的求导公式(其中\mathbf{A}\mathbf{B}表示矩阵,\mathbf{a}表示列向量):

    Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})

    Tr(\mathbf{AB})=Tr(\mathbf{BA})

    \mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}

    要让估计值更接近于真实值,就要使上面的迹尽可能的小,因此要取得合适的卡尔曼增益{{\mathbf{K}}_{k}},使得迹得到最小,言外之意就是使得迹对{{\mathbf{K}}_{k}}的偏导为0,即

    \frac{dT\left[ {{\mathbf{\Sigma }}_{k}} \right]}{d{{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'} \right)}^{T}}=0

    这样就能算出合适的卡尔曼增益了,即

    {{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    代回式(8)得到

    {{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    接下来就差真实值与预测值之间的协方差矩阵\mathbf{\Sigma }_{k}^{'}的求值公式了

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle

    将以下两个等式代入到上式,

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )

    可以得到

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \left[f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]{{\left[ f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle

    {{\mathbf{\theta }}_{k}}\left\langle {{\mathbf{\theta }}_{k}} \right\rangle与观测噪声{{\mathbf{s}}_{k}}是独立的,求期望等于零;;\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle表示观测噪声的协方差矩阵,用{\mathbf{Q}}表示。于是得到

    \mathbf{\Sigma }_{_{k}}^{'}=\mathbf{F}_{k-1}\left\langle \left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right){{\left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{F}}_{k-1}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle \\ =\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    其中的协方差矩阵的转置矩阵就是它本身。OK,大功告成,以上就完成了全部公式的推导了。

     

    4 实际应用

    现在我们假设在海上有一艘正在行驶的船只,其相对于传感器的横纵坐标为(x;y;v_{x};v_{y})为隐藏状态,无法直接获得,而传感器可以测量得到船只相对于传感器的距离和角度(r;\theta),传感器采样的时间间隔为\Delta t,则:

    状态向量(x;y;v_{x};v_{y}),观测向量(r;\theta)

    状态转移方程和观测方程为:

    \left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{x_{k}}} \\ {{v}_{y_{k}}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right])+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}

    \left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right])+{{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{{{y}_{k}}}{{{x}_{k}}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}

    那么雅克比矩阵为

    {{\mathbf{F}}_{k-1}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]

    {{H}_{k}}=\left[ \begin{matrix} \frac{\partial {{r}_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \frac{\partial {{\theta }_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \end{matrix} \right]

    这里给定距离传感器的噪声均值为0,方差为10;角度传感器的噪声均值为0,方差为0.001(单位弧度);

    采样时间点为\small 100个;

    船只的初始状态为(1000;1500;5;-3),四个状态量的噪声的方差分别为(2;2;0.2;0.2)。仿真结果如下:

     

    从仿真结果可以看出,EKF在这种情形下的滤波效果还是不错的,但是在实际应用中,对于船只运动的状态转移噪声的均值\mathbf q和协方差矩阵\mathbf Q,以及传感器的观测噪声的均值\mathbf r和协方差矩阵\mathbf R,往往都是未知的,有很多情况都只有观测值而已,这样的情形下,就有必要利用观测值对噪声的统计量参数做出适当的估计(学习)。

     

    5 参数估计(参数学习)

    利用EM算法和极大后验概率估计(MAP),对未知的噪声参数做出估计,再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出,之后可能会补充,这里就先给出一种Adaptive-EKF算法的公式。

    {{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}},     {{\mathbf{s}}_{k}}\sim \mathcal{N}(\mathbf{q},\mathbf{Q})

    {{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}},     {{\mathbf{v}}_{k}}\sim \mathcal{N}(\mathbf{r},\mathbf{R})

    {{\mathbf{\varepsilon }}_{k}}={{\mathbf{z}}_{k}}-h(\mathbf{\theta }_{k}^{'})-{{\mathbf{r}}_{k}}

    (1)E-Step

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{G}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    (2)M-Step

    {{\mathbf{\hat{q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{q}}}_{k\text{-}1}}+\frac{1}{k}\left[ \left\langle {{\theta }_{k}} \right\rangle -f\left( \left\langle {{\theta }_{k-1}} \right\rangle \right) \right]

    {{\mathbf{\hat{Q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{Q}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{K}}_{k}}{{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}\mathbf{K}_{k}^{T}+{{\mathbf{\Sigma }}_{k}}-{{\mathbf{F}}_{k-1}}{{\mathbf{\Sigma }}_{k-1}}\mathbf{F}_{k-1}^{T} \right]

    {{\mathbf{\hat{r}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{r}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{z}}_{k}}-h\left( \left\langle \theta _{k}^{'} \right\rangle \right) \right]

    {{\mathbf{\hat{R}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{R}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}-{{\mathbf{H}}_{k}}\mathbf{\Sigma }_{k}^{'}\mathbf{H}_{k}^{T} \right]

    利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪,得到的效果如下图所示:

    相比于没有做参数估计的EKF滤波,可以看出,Adaptive-EKF在估计误差上要优于EKF滤波,而且,它并不需要指定状态转移噪声和观测噪声的参数,将更有利于在实际中的应用。

     

    6 总结

    EKF滤波通过泰勒展开公式,把非线性方程在局部线性化,使得高斯分布的变量在经过线性变换后仍然为高斯分布,这使得能继续把标准卡尔曼滤波KF的框架拿过来用,总体来说,EKF在函数的非线性不是很剧烈的情形下,能够具有很不错的滤波效果。但是EKF也有它的不足之处:其一,它必须求解非线性函数的Jacobi矩阵,对于模型复杂的系统,它比较复杂而且容易出错;其二,引入了线性化误差,对非线性强的系统,容易导致滤波结果下降。基于以上原因,为了提高滤波精度和效率,以满足特殊问题的需要,就必须寻找新的逼近方法,于是便有了粒子滤波PF和无迹卡尔曼滤波UKF,笔者将在接下来的博文中为读者解读。

     

    7 参考文献

    [1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37. 

    [2] https://www.cnblogs.com/gaoxiang12/p/5560360.html.

    [3] https://max.book118.com/html/2017/0502/103920556.shtm.


    原创性声明:本文属于作者原创性文章,小弟码字辛苦,转载还请注明出处。谢谢~ 

    如果有哪些地方表述的不够得体和清晰,有存在的任何问题,亦或者程序存在任何考虑不周和漏洞,欢迎评论和指正,谢谢各路大佬。

    需要代码和有需要相关技术支持的可咨询QQ:297461921

    展开全文
  • 卡尔曼滤波

    2018-12-08 17:42:02
    卡尔曼滤波在MATLAB的简单实现,适合初学者对卡尔曼滤波流程的分析。
  • 卡尔曼滤波详解

    万次阅读 多人点赞 2019-03-20 19:37:53
    主要讲解基本的卡尔曼滤波算法,有时候也说是离散或者线性卡尔曼滤波。 首先来看一个数学公式,这部分仅仅是给定一个思路,和最后实际算法无关。目前考虑到要估计当前系统的状态,而且有两个已知量,一个上一个状态...

    这篇主要介绍卡尔曼滤波公式详细推导,使用示例参考卡尔曼滤波示例

    Kalman Filter

    简单介绍

    主要讲解基本的卡尔曼滤波算法,有时候也说是离散或者线性卡尔曼滤波。

    首先来看一个数学公式,这部分仅仅是给定一个思路,和最后实际算法无关。目前考虑到要估计当前系统的状态,而且有两个已知量,一个上一个状态的估计值以及当前状态的测量值,这两个都有一定的噪声,需要做的就是把这两个结合起来,很简单的思路就是按照比例相加得到当前状态的估计值:
    X^k=KkZk+(1Kk)X^k1 \hat{X}_k = K_k \cdot Z_k + (1 - K_k) \cdot \hat{X}_{k-1}

    kk 表示离散的状态量,可以把它简单的理解为离散的时间间隔。k=1 表示1ms,k=2 表示2ms;

    X^k\hat{X}_k 是对当前状态的估计值,希望利用上面的公式对每一个 k 都能得到一个较为准确的 X 的值;

    ZkZ_k 是对当前状态的测量值,当然这个值并不是绝对准确的,会有一定的误差噪声(如果绝对准确,直接用就可以了,也就没必要搞这个卡尔曼滤波算法了);

    X^k1\hat{X}_{k-1} 是对上一状态的估计值,利用这个以及测量值对当前状态进行估计;

    KkK_k 是卡尔曼增益(kalman gain),在这里唯一未知的就是这个值,也是需要去求的值。当然可以直接设置值为0.5,但是这样比较暴力。最好的方式就是根据每一时刻的状态求一个当前状态最好的增益值,这样的话更好利用以前状态的估计值以及当前测量值来估计一个最优的当前值。

    后面卡尔曼滤波算法就是按照上面思路利用上一状态以及测量值去估计当前状态,只不过模型要更加复杂。

    基本模型

    卡尔曼滤波的状态方程,利用线性随机差分方程(Linear Stochastic Difference equation)利用上一个系统状态估计当前系统状态(这里假设上一状态与下一一状态有某种线性关系,比如恒温环境的温度,匀速运动的速度等,但是因为现实环境的复杂,这种线性关系不是完全平滑的,也就是会有一些扰动):
    xk=Axk1+Buk1+w x_k = Ax_{k-1} + Bu_{k-1}+w
    使用时一般忽略 uu 控制输入,得到:
    xk=Axk1+w(1.1) x_k = Ax_{k-1} + w \qquad {(1.1)}
    加上对于当前状态的测量方程(简单来说就是测量值和状态值的线性函数):
    zk=Hxk+v(1.2) z_k = Hx_k + v \qquad {(1.2)}

    k1k-1kk 分别表示上一状态和当前状态;

    xRnx \in R^n 表示要估计的状态;

    ARn×nA \in R^{n \times n} 表示上一状态到当前状态的转换矩阵;

    uRlu \in R^l 表示可选的控制输入,一般在实际使用中忽略;

    BRn×lB \in R^{n \times l} 表示控制输入到当前状态的转换矩阵;

    zRmz \in R^m 表示测量值;

    HRm×nH \in R^{m \times n} 表示当前状态到测量的转换矩阵;

    wRnw \in R^n 表示过程噪声,主要是从上一状态进入到当前状态时,会有许多外界因素的干扰;

    vRmv \in R^m 表示测量噪声,主要是任何测量仪器都会有一定的误差;

    在上面转换矩阵 AA BB HH ww vv 是随着状态变化的,在这里没有添加下标,假设是不变的。

    上面提到的过程噪声 ww 和测量噪声 vv 假设是相互独立的(之间没有关系,无相互影响),且是高斯白噪声,意思是这些噪声在离散的状态上是没有关系的(互相独立的, 每个时刻的噪声都是独立的)且 服从高斯分布:
    p(w)N(0,Q)(1.3)p(v)N(0,R)(1.4)Q=wwTR=vvTE(w)=0E(v)=0 \begin{aligned} p(w) & \sim N(0, Q) \qquad {(1.3)} \\ p(v) & \sim N(0, R) \qquad {(1.4)} \\ Q & = ww^T \\ R & = vv^T \\ E(w) & = 0 \\ E(v) & = 0 \end{aligned}

    QRn×nQ \in R^{n \times n} 表示过程噪声 ww 的协方差矩阵,表示 ww 向量元素之间的相关关系;

    RRm×mR \in R^{m \times m} 表示测量噪声 vv 的协方差矩阵,表示 vv 向量元素之间的相关关系;

    在上面协方差矩阵 QQ RR 是随着状态变化的,在这里假设是不变的。

    推导过程

    上面是给出了卡尔曼滤波的整体思路和基本模型,在这里一步步分解来看卡尔曼滤波的推导过程。首先确定了卡尔曼滤波的基本模型,那么下一步是怎样根据这个模型来不断求取后面的状态。

    首先定义几个变量:

    x^kˉ\bar{\hat{x}_k} 加了一个负号上标,表示仅仅利用过程先验知识求出的当前状态的先验状态估计(a priori state estimate),不考虑过程噪声的情况下;

    x^k\hat{x}_k 表示利用测量值 zkz_k 求出的当前状态的后验状态估计(a posteriori state estimate),也是最终求得的状态;

    需要注意的是,卡尔曼滤波的最终目标是求 x^k\hat{x}_k ,这是对当前状态的最优估计。

    根据上面的定义忽略过程噪声可以得到:
    x^kˉ=Ax^k1+Buk1 \bar{\hat{x}_k} = A\hat{x}_{k-1} + Bu_{k-1}
    通常都是忽略 uu 这个可选的控制输入(以后加入该变量的话也是非常方便的):
    x^kˉ=Ax^k1(2.1) \bar{\hat{x}_k} = A\hat{x}_{k-1} \qquad {(2.1)}
    然后同理忽略测量噪声可以得到:
    zkˉ=Hx^kˉ \begin{aligned} \bar{z_k} & = H\bar{\hat{x}_k} \end{aligned}

    zkˉ\bar{z_k} 表示忽略测量噪声根据当前先验状态得到的无噪声测量值。

    那么很简单的可以用实际得到的测量值减去无噪声由上一状态预测的测量值:
    zk˙=zkzkˉ=zkHx^kˉ \begin{aligned} \dot{z_k} & = z_k - \bar{z_k} \\ & = z_k - H\bar{\hat{x}_k} \end{aligned}

    zk˙\dot{z_k} 表示实际测量值(包含了上面提到的各种干扰即过程噪声以及测量噪声)减去无噪声测量值,一般被叫做measurement innovation 或者是 residual,表示预测的测量值和实际测量值的差异。实际得到是过程噪声和测量噪声对当前测量值的影响(当然也会影响当前状态),也就是包含了 wwvv 的情况。

    现在得到了一个无噪声的状态的估计 x^kˉ\bar{\hat{x}_k},以及一个噪声变量 zk˙\dot{z_k}。根据经验来说把这两个值按照一定的方式整合起来就可以得到后验状态估计。卡尔曼滤波中采用按照比例组合的方式(其实对于下面的公式在数学上有严格的证明过程,但是仅仅是使用卡尔曼滤波就不去深究了):
    x^k=x^kˉ+Kkzk˙=x^kˉ+Kk(zkHx^kˉ)(2.2) \begin{aligned} \hat{x}_k & = \bar{\hat{x}_k} + K_k\dot{z_k} \\ & = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k}) \qquad {(2.2)} \end{aligned}

    KkRn×mK_k \in R^{n \times m} 表示卡尔曼增益(gain)或者均化系数(blending factor)。

    到这里,有了上面的公式(2.2),为了得到 x^k\hat{x}_k,其他参数都是已知的,目前需要把每个状态的 KkK_k 求出来,这里定义先验和后验估计误差:
    ekˉxkx^kˉekxkx^k \bar{e_k} \equiv x_k - \bar{\hat{x}_k} \\ e_k \equiv x_k - \hat{x}_k
    对应的误差协方差矩阵为:
    Pkˉ=E[ekˉeˉkT]Pk=E[ekekT] \bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] \\ P_k = E[e_ke_{k}^T]
    然后为了使后验状态估计 x^k\hat{x}_k 最优,那么目标就是最小化后验状态估计的误差协方差矩阵 PkP_k 值。

    后验误差

    根据公式(1.1),(2.2),(1.2),(2.1)可以得到:
    ek=xkx^k=xk(x^kˉ+Kk(zkHx^kˉ))=xk(x^kˉ+Kk((Hxk+v)Hx^kˉ))=xkx^kˉKkHxkKkv+KkHx^kˉ=(IKkH)(xkx^kˉ)Kkv=(IKkH)ekˉKkv \begin{aligned} e_k & = x_k - \hat{x}_k \\ & = x_k - (\bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k})) \\ & = x_k - (\bar{\hat{x}_k} + K_k((Hx_k + v) - H\bar{\hat{x}_k})) \\ & = x_k - \bar{\hat{x}_k} - K_kHx_k - K_kv + K_kH\bar{\hat{x}_k} \\ & = (I - K_kH)(x_k-\bar{\hat{x}_k}) - K_kv \\ & = (I - K_kH)\bar{e_k} - K_kv \end{aligned}
    所以带入展开可得:
    ekekT=[(IKkH)ekˉKkv][(IKkH)ekˉKkv]T=(IKkH)ekˉekˉT(IKkH)TKkvekˉT(IKkH)T(IKkH)ekˉvTKkT+KkvvTKkT \begin{aligned} e_ke_{k}^T & = \left[ (I - K_kH)\bar{e_k} - K_kv \right]\left[ (I - K_kH)\bar{e_k} - K_kv \right]^T \\ & = (I - K_kH)\bar{e_k}\bar{e_k}^T(I - K_kH)^T - K_kv\bar{e_k}^T(I - K_kH)^T - (I - K_kH)\bar{e_k}v^TK_k^T + K_kvv^TK_k^T \end{aligned}
    ekˉ\bar{e_k}z1,,zk1z_{1} , \cdots , z_{k - 1} 的线性函数,和当前 zkz_k 无关,所以:

    E(ekˉvT)=0E(vekˉT)=0 E(\bar{e_k}v^T) = 0 \\ E(v\bar{e_k}^T) = 0

    带入整理得到:
    Pk=E[ekekT]=(IKkH)Pkˉ(IKkH)T+KkRKkT=PkˉKkHPkˉPkˉHTKkT+KkHPkˉHTKkT+KkRKkT \begin{aligned} P_k = E[e_ke_{k}^T] & = (I-K_kH)\bar{P_k}(I - K_kH)^T + K_kRK_k^T \\ & = \bar{P_k} - K_kH\bar{P_k} - \bar{P_k}H^TK_k^T + K_kH\bar{P_k}H^TK_k^T + K_kRK_k^T \end{aligned}
    合并 KkK_k 项目:
    Pk=PkˉPkˉHT(HPkˉHT+R)1HPkˉ+[KkPkˉHT(HPkˉHT+R)1](HPkˉHT+R)[KkPkˉHT(HPkˉHT+R)1]T \begin{aligned} P_k & = \bar{P_k} - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}H\bar{P_k} \\ & + \left[ K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}\right ](H\bar{P_k}H^T + R)\left[K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \right ]^T \end{aligned}

    具体合并的过程其实利用最基本的矩阵运算就可以解决,这里主要是矩阵运算可能不那么直观,如果把它想象成正常的以 KkK_k
    为未知数的一元二次方程,可能就很好理解了。首先我们目标是合并 KkK_k 项,先考虑二次项部分: KkHPkˉHTKkT+KkRKkT=Kk(HPkˉHT+R)KkT \begin{aligned} K_kH\bar{P_k}H^TK_k^T + K_kRK_k^T & =K_k(H\bar{P_k}H^T + R)K_k^T \end{aligned} 然后把剩余的两个一次项添加上面合并好的二次项里面,为了抵消上面中间项 HPkˉHT+RH\bar{P_k}H^T + R,所以在添加的时候会额外乘以一个 (HPkˉHT+R)1(H\bar{P_k}H^T + R)^{-1},然后直接就合并好了所有的 KkK_k 项: [KkPkˉHT(HPkˉHT+R)1](HPkˉHT+R)[KkPkˉHT(HPkˉHT+R)1]T \left[ K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}\right ](H\bar{P_k}H^T + R)\left[K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \right ]^T 当然这里还有个问题是上面合并项里面会多出来一下之前不存在的项,把上面合并好的公式展开然后减去最开始的 KkK_k
    项,得到了不包含 KkK_k 项的一些补偿项,这样把补偿项添加回去就完成了。

    上面的式子中前两项不包含 KkK_k 因子,所以如果要最小化 PkP_k,只需:
    KkPkˉHT(HPkˉHT+R)1=0 K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} = 0
    所以得到:
    Kk=PkˉHT(HPkˉHT+R)1(2.5) K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \qquad {(2.5)}
    然后可以得到:
    Pk=(IKkH)Pkˉ(2.4) P_k = (I-K_kH)\bar{P_k} \qquad {(2.4)}

    先验误差

    带入上面的公式(1.1),(2.1)可以得到:
    ekˉ=xkx^ˉk=(Axk1+w)(Ax^k1)=A(xk1x^k1)+w=Aek1+w \begin{aligned} \bar{e_k} & = x_k - \bar{\hat{x}}_k \\ & = (Ax_{k-1} + w) - (A\hat{x}_{k-1}) \\ & = A(x_{k-1} - \hat{x}_{k-1}) + w \\ & = Ae_{k-1} + w \end{aligned}
    所以带入展开可得:
    ekˉeˉkT=(Aek1+w)(Aek1+w)T=Aek1ek1TAT+wwT+wek1TAT+Aek1wT \begin{aligned} \bar{e_k}\bar{e}_{k}^T & = (Ae_{k-1} + w)(Ae_{k-1} + w)^T \\ & = Ae_{k-1}e_{k-1}^TA^T + ww^T + we_{k-1}^TA^T + Ae_{k-1}w^T \end{aligned}

    又因为:
    E(ek1wT)=0E(wek1T)=0 E(e_{k-1}w^T) = 0 \\ E(we_{k-1}^T) = 0

    然后可以得到:
    Pkˉ=E[ekˉeˉkT]=APk1AT+Q(2.3) \bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] = AP_{k-1}A^T + Q \qquad {(2.3)}

    基本分析

    从上面的公式可以看出来,如果过程误差 Pkˉ=0\bar{P_k} = 0 ,那么 Kk=0K_k = 0 ,可以 得到 x^k=x^kˉ=Ax^k1\hat{x}_k = \bar{\hat{x}_k} = A\hat{x}_{k-1} ,完全相信以前估计的值,这样就是为啥不能设置 P0=0P_0 = 0 。当然给 QQ 赋值可能能解决这个问题,而且不能设置太小,这样手动引入较大的过程误差,估计结果很不平滑,后面示例3也会讲解。对于RR 的变化对结果的作用后面的示例会介绍,同理 RR 设置太大也会导致这样的结果。在这种情况下,如果初始时设置的不正确,整个估计都是错误的。

    基本公式

    最后得到了上面的上面(2.1)-(2.5)五个方程式,是经典的卡尔曼滤波的五个基本公式,利用这五个公式就可以完成卡尔曼滤波的求解过程。根据迭代过程分成两个方程集合,一个是time update方程,一个是measurement update方程,如下:

    Time Update Measurement Update
    x^kˉ=Ax^k1\bar{\hat{x}_k} = A\hat{x}_{k-1} Kk=PkˉHT(HPkˉHT+R)1K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}
    Pkˉ=APk1AT+Q\bar{P_k} = AP_{k-1}A^T + Q x^k=x^kˉ+Kk(zkHx^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k})
    Pk=(IKkH)PkˉP_k = (I - K_kH)\bar{P_k}

    迭代过程

    得到上面的基本公式以后,就可以计算计算后面的状态,但是公式中有一些参数是未知的,需要定义,如下:

    uu 变量一般来说是被忽略的;

    AABBHH 上面提到对于每个状态 kk 都是相同的,同时大部分情况下都是单位矩阵(复杂的计算这里不考虑,由具体应用决定);

    RR 一般来说是相对简单可以求到的,就是仪器的噪声协方差;

    QQ 就很难直接的获得,也没有特定的方法,一般是系统过程比较稳定,也就是没有过程噪声;

    zz 所有状态的的测量值都是由仪器测量得到的是已知的;

    对于RRQQ 的取值来说不是特别重要,虽然可能这两个参数估计的不是很准确(不可能符合纯粹的高斯或者正太分布),但是实际情况中卡尔曼滤波算法也可以得到整的比较准确态估计,但是需要记住的是:

    The better you estimate the noise parameters, the better estimates you get.

    那么这样只需要传入基本的 x^0\hat{x}_0P0P_0 就可以了,一般来说需要进行指定。对于 x^0\hat{x}_0 来说不需要什么技巧,因为随着卡尔曼滤波算法的运行,xx 会逐渐的收敛。但是对于 P0P_0 ,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的 x^0\hat{x}_0 是系统最优的,从而使算法不能收敛。

    算法迭代过程如下图(其中 uu 控制输入一般是忽略的):

    Kalman Filter Operation

    参考

    1. Kalman Filter For Dummies
    2. Greg Welch, Gary Bishop, “An Introduction to the Kalman Filter”, University of North Carolina at Chapel Hill Department of Computer Science, 2001
    3. 一个应用实例详解卡尔曼滤波及其算法实现
    4. 卡尔曼滤波算法–核心公式推导导论
    展开全文
  • 卡尔曼滤波系列——(一)标准卡尔曼滤波

    万次阅读 多人点赞 2019-03-03 16:03:58
    卡尔曼滤波(Kalman Filter)是一种利用线性系统状态方程,利用对系统的观测数据,对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响,所以系统状态的估计过程也可看作是滤波过程。应用场景...

    更新日志:

    2020.03.20:修改了部分内容的表述方式,重做了实验并给出相应结果,补充了矩阵迹的求导公式;

    2020.09.24:修改了推导中第四个公式的符号错误,第k时刻预测值应由上一时刻的估计结果推出,而非真实值;

    1 简介

    卡尔曼滤波(Kalman Filter)是一种利用线性系统状态方程,利用对系统的观测数据,对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响,所以系统状态的估计过程也可看作是滤波过程。应用场景之一有利用传感器跟踪感兴趣目标的位置,传感器获取的目标距离、速度、方位角等观测值往往含有噪声。卡尔曼滤波利用目标的动态信息与观测结果相结合,抑制噪声的影响,从而获得一个关于目标位置更准确的估计,这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑)。

     

    2 算法介绍

    为了便于读者理解卡尔曼滤波的运作过程,这里先举个简单易懂的例子。

    设想现在咱养了一头猪,一周前,这只猪的体重是46±0.5kg,这里所用的±0.5,表示其实对这只猪一周前的重量并不是那么确定的,也就是说,46kg这个重量有0.5kg的误差。现在,一周过去了,咱想要知道它此刻有多重,又大概有多少的误差?

    为了确定此刻猪头的重量,一般咱采用的方法是拿个大体重秤去称量,假设现在体重秤给出的结果是49kg。可是这个时候旁边一个人告诉我们,这个体重秤不准,有±1kg的误差。按照人的第一反应,应该是会根据经验思考一下这个49kg到底准不准,按照经验,一周时间这猪头应该差不多长了2kg,所以根据经验判断应该是48kg,有±2kg的误差。怎么办?不管是经验的感性判断还是体重秤的理性判断,都有误差,这个时候想同时把这两种判断考虑到一起,给出一个比较可靠的估计值。之后,每周咱都得用这个不准的体重秤和经验去估计这头猪的重量,这就是卡尔曼滤波的过程。

     

    标准的卡尔曼滤波系统方程如下:

    {{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}

    {{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}

    上面的两个式子分别叫作状态转移方程和观测方程。其中\mathbf{A}叫作状态转移矩阵,对应到例子中就是现在与一周前体重的转移系数,取值为1;\mathbf{B}{{\mathbf{u}}_{k}}是系统模型的参数,在例子中可以理解成一周内猪的固定增长量,\mathbf{B}可以取值为1,{{\mathbf{u}}_{k}}取恒值为2,这样一周就是增长2kg;\mathbf{C}叫作观测矩阵,对应到例子中的体重秤就是1;{{\mathbf{s}}_{k}}是状态转移噪声向量或者叫过程噪声,对应于例子中的±2kg的经验判断误差;{{\mathbf{v}}_{k}}是观测噪声向量,对应于例子中的±1kg的体重秤测量误差;{{\mathbf{\theta }}_{k}}k时刻的真实状态向量;\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}k时刻的预测值,对应于例子中48kg的经验判断;{{\mathbf{z}}_{k}}k时刻的观测向量,对应于例子中体重秤称得的重量49kg。

    实际应用中,{{\mathbf{\theta }}_{k}}{{\mathbf{z}}_{k}}{{\mathbf{s}}_{k}}{{\mathbf{v}}_{k}}往往是列向量的形式,可以理解为同时对多个属性做滤波估计操作,例如同时对猪头此刻的体重、长度和高度做出估计(滤波)。状态转移噪声向量{{\mathbf{s}}_{k}}一般是服从多维高斯分布的,其均值为\mathbf{0}向量,协方差矩阵为{\mathbf{Q}};观测噪声向量{{\mathbf{v}}_{k}}一般也是服从多维高斯分布的,其均值为0向量,协方差矩阵是{\mathbf{R}}。最终滤波后得出的估计向量值用\left\langle {{\mathbf{\theta }}_{k}} \right\rangle表示。

    那么卡尔曼滤波是怎么实现的?它分为预测(Predict,或者叫传播Propagation)和更新(Update)两个步骤:

    Predict:

    \mathbf{\theta }_{k}^{'}=\mathbf{A}\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle+\mathbf{B}\mathbf{u}_{k-1}

    \mathbf{\Sigma }_{k}^{'}=\mathbf{A}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{A}}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-\mathbf{C\theta }_{k}^{'} \right)

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}

    为了说明更简洁,咱先把上面的六个公式标为(1)~(6),理一下这个算法的思路:

    首先矩阵\mathbf{A}\mathbf{C}\mathbf{B}{{\mathbf{u}}_{k}}{\mathbf{Q}}{\mathbf{R}}是已知的,假设现在我们已经有一组数据了,例如从第1周到第50周猪头的观测值{{\mathbf{z}}_{k}}(体重秤给出,k=1\tilde{\ }50)和估计值\left\langle {{\mathbf{\theta }}_{k}} \right\rangle(算法算出来的,k=1\tilde{\ }50),同时根据算法也会有估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}}(算法算出来的,k=1\tilde{\ }50)。接下来是51周(k=51),已经得到了此时猪头的重量观测值{{\mathbf{z}}_{51}},有了这些数据就可以走算法了:

    1. 第一步:根据公式(1)和公式(2)计算预测值\mathbf{\theta }_{k}^{'}以及预测值与真实值之间的协方差矩阵\mathbf{\Sigma }_{k}^{'};(数据准备)
    2. 第二步:根据公式(3)和公式(4)计算卡尔曼增益\mathbf{K}_{k}^{'},然后根据公式(5)估计此时猪头的重量\left\langle {{\mathbf{\theta }}_{k}} \right\rangle;(滤波估计)
    3. 第三步:根据公式(6)计算估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},用于下一次递推;(参数更新)

    以上三步算法中k=51,为了更清楚地对应到公式,这里没有更改。以上内容共出现了四个值不知道读者有没有看懵了,如果有,这一段要好好体会一下,它们分别是真实值、估计值、观测值、预测值。

    • 真实值就是目标属性的真实状态值,用{{\mathbf{\theta }}_{k}}表示,例如猪头此时此刻的重量,这个值咱从头到尾都不知道,所以才要用卡尔曼滤波方法去估计这一个值;
    • 估计值就是用来估计真实值的数值,用\left\langle {{\mathbf{\theta }}_{k}} \right\rangle表示,例如算法每递推一次,就会估计出一个猪头重量值;
    • 观测值就很明白了,就是状态值的一个映射,用{{\mathbf{z}}_{k}}表示,例如例子中体重秤称得的猪头重量;
    • 预测值就是根据上一次的估计值,根据线性系统的模型参数算出来下一步应该是什么样的状态,用\mathbf{\theta }_{k}^{'}表示,例如根据上周猪头的重量,对本周重量的一个经验判断。

    有了以上这些,读者就可以实现这一整个算法,主要就是上面的这六个公式,每进行一次递推,就要算一遍这些公式。那么最后的小问题就是初始值的设定了,因为有了这一步与上一步的关系,只要整个算法第一次递推之前的初始值确定,接下来整个程序就能正常的运行起来了。这里初始值就两个,一个是初始估计值\left\langle {{\mathbf{\theta }}_{1}} \right\rangle,可以直接取第一次的观测值{{\mathbf{z}}_{1}};另一个是初始的估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{1}},可以取主对角线上的值均为一个较小值(例如0.1这样的,根据实际数据而定)的对角阵,如果只对一维的数据做滤波,那就是一个较小值(例如0.1)。

     

    好的,大功告成,根据以上的内容,读者就可以自己写出完整的卡尔曼滤波算法了,希望我有把卡尔曼滤波的原理讲清楚了。接下来我想给出递推公式的推导过程,有兴趣的读者可以看一下,自己推看看。

     

    3 公式推导

    首先列出我们所有的符号为推导做准备:

    真实值{{\mathbf{\theta }}_{k}}、估计值\left\langle {{\mathbf{\theta }}_{k}} \right\rangle、观测值{{\mathbf{z}}_{k}}、预测值\mathbf{\theta }_{k}^{'}、估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}},求期望的符号\left\langle \cdot \right\rangle

    线性系统的状态差分方程:{{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}

    观测方程:{{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}

    引入卡尔曼增益以修正观测结果:\left\langle {{{\bf{\theta }}_k}} \right\rangle = {\bf{\theta }}_k^{'} + {{\bf{K}}_k}\left( {{{\bf{z}}_k} - {\bf{z}}_k^'} \right) = {\bf{\theta }}_k^{' }+ {{\bf{K}}_k}\left( {{{\bf{z}}_k} - {\bf{C\theta }}_k^'{\rm{ }}} \right)

    预测值\mathbf{\theta }_{k}^{'}=\mathbf{A}{\langle{\mathbf{\theta }}_{k-1}\rangle}+\mathbf{B}{{\mathbf{u}}_{k-1}}

    以上四个公式是给出的前提条件,分别用公式(1)~(4)表示。

     

    OK,可以开始推导了:

    计算估计值与真实值之间的误差协方差矩阵{{\mathbf{\Sigma }}_{k}}

    {{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right){{\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)}^{T}} \right\rangle

    然后把公式(2)代入到公式(3)中去掉{{\mathbf{z}}_{k}}再代入到上面的等式得到

    {{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)-{{\mathbf{K}}_{k}}{{\mathbf{v}}_{k}} \right]{{\left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)-{{\mathbf{K}}_{k}}{{\mathbf{v}}_{k}} \right]}^{T}} \right\rangle

    然后把里面的因式相乘,再分别求期望,又有{{\mathbf{\theta }}_{k}}\mathbf{\theta }_{k}^{'}与观测噪声{{\mathbf{v}}_{k}}是独立的,求期望等于零,于是得到

    {{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)}^{T}}+{{\mathbf{K}}_{k}}\left\langle {{\mathbf{v}}_{k}}{{\mathbf{v}}_{k}}^{T} \right\rangle \mathbf{K}_{k}^{T}

    上面的等式中\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle表示真实值与预测值的协方差矩阵,用\mathbf{\Sigma }_{k}^{'}表示;\left\langle {{\mathbf{v}}_{k}}{{\mathbf{v}}_{k}}^{T} \right\rangle表示观测噪声的协方差矩阵,用{\mathbf{R}}表示。于是得到

    {{\mathbf{\Sigma }}_{k}}&=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}{{\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)}^{T}}+{{\mathbf{K}}_{k}}\mathbf{RK}_{k}^{T} \\ &=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{C\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}

    因为{{\mathbf{\Sigma }}_{k}}的对角元即为真实值与估计值的误差的平方,矩阵的迹(用T[]表示)即为总误差的平方和,即

    T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{C\Sigma }_{k}^{'} \right]

    利用以下矩阵迹的求导公式(其中\mathbf{A}\mathbf{B}表示矩阵,\mathbf{a}表示列向量):

    Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})

    Tr(\mathbf{AB})=Tr(\mathbf{BA})

    \mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}

    要让估计值更接近于真实值,就要使上面的迹尽可能的小,因此要取得合适的卡尔曼增益{{\mathbf{K}}_{k}},使得迹得到最小,言外之意就是使得迹对{{\mathbf{K}}_{k}}的偏导为0,即

    \frac{\partial T\left[ {{\mathbf{\Sigma }}_{k}} \right]}{\partial {{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{C\Sigma }_{k}^{'} \right)}^{T}}=0\frac{\partial T\left[ {{\mathbf{\Sigma }}_{k}} \right]}{\partial {{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{C\Sigma }_{k}^{'} \right)}^{T}}=0

    这样就能算出合适的卡尔曼增益了,即

    {{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}{{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}

    再把这个式子代回{{\mathbf{\Sigma }}_{k}}的求值公式就可以得到

    {{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}{{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{C\Sigma }_{k}^{'}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}

    接下来就差真实值与预测值之间的协方差矩阵\mathbf{\Sigma }_{k}^{'}的求值公式了

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle

    然后把公式(1)和公式(4)代入得到

    \mathbf{\Sigma }_{_{k+1}}^{'}=\left\langle \left[ \mathbf{A}\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)+{{\mathbf{s}}_{k}} \right]{{\left[ \mathbf{A}\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle

    {{\mathbf{\theta }}_{k}}\left\langle {{\mathbf{\theta }}_{k}} \right\rangle与观测噪声{{\mathbf{s}}_{k}}是独立的,求期望等于零;\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle表示观测噪声的协方差矩阵,用{\mathbf{Q}}表示。于是得到

    \mathbf{\Sigma }_{_{k+1}}^{'}=\mathbf{A}\left\langle \left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right){{\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{A}}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle =\mathbf{A}{{\mathbf{\Sigma }}_{k}}{{\mathbf{A}}^{T}}+\mathbf{Q}

    其中的协方差矩阵的转置矩阵就是它本身。这样就完成了全部公式的推导了。

     

    4 实验结果

    为了更形象地给出卡尔曼滤波算法的效果,这里另外构造一个实际应用的例子:一辆车子在空旷场地上行驶,通过GPS测量车子实时相对于一个参考点的横纵坐标,之后通过该观测结果,估计该车辆的实时位置。

    状态向量\left [x,y,v_{x},v_{y} \right ]^{T},观测向量\left [{z_x},{z_y} \right ]^{T}

    状态转移方程和观测方程为:

    \left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{x_{k}}} \\ {{v}_{y_{k}}} \\ \end{matrix} \right]=A\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}

    \left[ \begin{matrix} {{z_x}_{k}} \\ {{z_y}_{k}} \end{matrix} \right]=C\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}=\begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0& 0 \end{bmatrix}\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}

    这里给定GPS横坐标测量的噪声均值为0,方差为2;纵坐标测量的噪声均值为0,方差为2(单位为m);

    采样时间点为\small 500个,采样的时间间隔\small \Delta t=0.01

    车辆的初始状态为(-20;20;10;10),四个状态量的噪声的方差分别为(0.01;0.01;0.05;0.05)。仿真结果如下:

     

    可以看到,相对于直接取用GPS的观测结果作为目标状态的估计而言,通过卡尔曼滤波后获得的状态估计要更加准确,状态波动小,较为稳定,更符合实际中车辆的行驶过程。本次实验中,通过卡尔曼滤波技术确定的定位均方误差约为0.5m,而直接用观测结果的定位均方误差为1.7m,可以看出卡尔曼滤波在定位精度上可以带来极大地提升。

    这里需要说明一点,为了使得读者能更好的把程序和算法的公式对应起来,代码的变量并没有设置成最节省内存的形式,实际应用中,滤波数据可能非常庞大,每一次递推过程都应该覆盖前一次的计算结果,这样才不会过多占用内存。

     

    5 总结

    数据滤波是去除噪声还原真实数据的一种数据技术,卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。

    卡尔曼滤波算法的主要思想在于同时利用了目标的实时状态信息和观测结果,相比于直接利用单一观测估计系统状态而言,卡尔曼滤波算法利用了更多的先验信息,每一个时刻都对系统状态做估计,并预测了下一个时刻的系统大致状态,之后根据观测结果,利用新息与卡尔曼增益相乘,修正预测量,从而完成对系统状态更准确的估计。其整个过程是递推进行的,每完成一次系统的状态估计,就计算一次估计值与真实值的误差协方差矩阵,用于下一次估计时的变量计算。

    矩阵{\mathbf{Q}}和矩阵{\mathbf{R}}表征的是状态噪声和观测噪声的协方差,而卡尔曼滤波算法的最终状态估计,说到底就是根据矩阵{\mathbf{Q}}和矩阵{\mathbf{R}}的值去权衡系统的估计偏向预测和偏向观测的程度。举个例子,如果矩阵{\mathbf{Q}}远大于矩阵{\mathbf{R}},也就是说状态噪声十分剧烈,相对的观测噪声很小,那么系统的估计就非常接近观测的结果;反之矩阵{\mathbf{Q}}远小于矩阵{\mathbf{R}},也就是说观测噪声相当大,相对的状态噪声很小,那么系统的状态估计就更多地偏向于根据上一个时刻的状态估计所预测的当前时刻系统状态。

    标准卡尔曼滤波方法是在线性模型下,噪声满足高斯分布时的一种最优估计手段,这同时带来了算法的短处:矩阵{\mathbf{Q}}和矩阵{\mathbf{R}}需要提前给定无法自适应,非线性模型无法求解,非高斯噪声算法不匹配等等,所以各国学者针对不同的应用背景,对标准尔曼滤波做了诸多扩展,笔者将会接下来的博文中讲解部分扩展形式,希望大家关注。

     

    6 参考文献

    [1] Kalman, Rudolf. (1959). A New Approach to Linear Filtering and Prediction Problems. J. Basic Engineering. 82D. 35-45. 

    [2] https://blog.csdn.net/u012554092/article/details/78290223.

    [3] https://blog.csdn.net/heyijia0327/article/details/17487467.


    原创性声明:本文属于作者原创性文章,小弟码字辛苦,转载还请注明出处。谢谢~ 

    代码下载请到本博文实验程序

    如果有哪些地方表述的不够得体和清晰,有存在的任何问题,亦或者程序存在任何考虑不周和漏洞,欢迎评论和指正,谢谢各路大佬。

    有需要相关技术支持的可咨询QQ:297461921

     

     

    展开全文
  • 扩展卡尔曼滤波_无迹卡尔曼滤波_扩展信息滤波_l粒子滤波算法.rar
  • matlab卡尔曼滤波和扩展卡尔曼滤波(KF&EKF)例程,可供学习参考使用,谢谢! 希望能帮到你。。。。。。。。。。。
  • 卡尔曼滤波讲解PPT,包含经典卡尔曼滤波、无迹卡尔曼滤波、扩展卡尔曼滤波、多模型卡尔曼滤波,包含理论推导、仿真图示;适合汇报、学习、教学用
  • 卡尔曼滤波应用及其matlab实现

    万次阅读 多人点赞 2018-02-11 20:32:18
    线性卡尔曼滤波 卡尔曼滤波在温度测量中的应用 X(k)=A*X(k-1)+T*W(k-1) Z(k)=H*X(k)+V(k) 房间温度在25摄氏度左右,测量误差为正负0.5摄氏度,方差0.25,R=0.25。Q=0.01,A=1,T=1,H=1。 假定快时刻...
  • 针对条件线性高斯状态空间模型, 提出cubature 卡尔曼滤波-卡尔曼滤波算法(CKF-KF), 分别应用CKF 和KF 估计模型中的非线性和线性状态. 该算法对非线性与线性状态均进行cubature 采样, 并将两种样本通过线性方程和量测...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 5,247
精华内容 2,098
关键字:

卡尔曼滤波