精华内容
下载资源
问答
  • 详解卡尔曼滤波原理

    万次阅读 多人点赞 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节中的部分表述和公式加粗,补充迹的求导公式

    2021.04.14:修改公式显示错误

    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}})+{​{o}^{n}}

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

    {[\nabla f({​{\bf{x}}})]^T} = {​{\bf{J}}({\bf x})} = \begin{bmatrix} \frac{\partial f({\bf x})_1}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_1}{\partial {\bf x}_n}\\ \vdots & \ddots & \vdots \\ \frac{\partial f({\bf x})_m}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_m}{\partial {\bf x}_n} \end{bmatrix}

    这里,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]

    利用以下矩阵迹的求导公式(其中\bf A\bf B表示矩阵,\bf 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{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}^{'}

    (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

    展开全文
  • 卡尔曼滤波

    2019-02-21 10:34:31
    关于卡尔曼滤波器的介绍,卡尔曼滤波的原理 关于卡尔曼滤波器的介绍,卡尔曼滤波的原理
  • 卡尔曼

    千次阅读 2010-06-04 14:12:00
     回顾整个过程:  1,觉得卡尔曼有现成的预测和更新公式,只要确定参数C、Q、R就可以使用,很简单,便最小二乘从直接测量的一天的数据算出C,再根据样本方差确定Q,由于Y是直接通过恒定的Y计算得来,...

    断断续续,大概弄了将近一个月的时间,今天总算是把卡尔曼滤波器用于流量矩阵估计的问题给解决了。实验结果很理想,和作者的实验结果差不多。

          回顾整个过程:

          1,觉得卡尔曼有现成的预测和更新公式,只要确定参数C、Q、R就可以使用,很简单,便用最小二乘从直接测量的一天的数据算出C,再根据样本方差确定Q,由于Y是直接通过恒定的Y计算得来,所以假设R=0。但是,通过这些参数,使用卡尔曼进行估计时,发现估计的值是发散的,相对误差几乎成指数增长,令人费解。

          2,反省,觉得还是由于对卡尔曼理解不够,所以导致不知道问题出在哪。于是开始着手对卡尔曼滤波的两个方程的理解,包括公式的推导。主要是其中的增益矩阵K的推导,soul的那篇论文里并没有给出推导公式,我又看了其参考文献的关于卡尔曼的介绍,也没有给出推导,后来反而是在网上瞎找时,偶然在维基百科上找到了关于卡尔曼滤波的完整介绍,包括各个公式所依据的理论和推导过程,知道原来卡尔曼的理论基础原来是马尔可夫过程和贝叶斯统计。通过这上面的介绍也消除了我以前在看卡尔曼的一个疑问,就是关于更新公式中的估计方差的公式不一致的问题,有的论文(流量估计方面的论文中关于卡尔曼滤波)提出Pe = (I-KA)Pp(I - KA)'   (1)而有的论文(专门单独介绍卡尔曼滤波的论文)为Pe = (I-KA)Pp   (2)。其实两种写法都对,当K取最优增益时(使估计误差最小),(1)可以通过一系列推导简写为(2) ,当然在实际应用时,K都是取的最优增益,所以便取(2)。

           3,通过2对卡尔曼滤波的理解,我觉得可能是由于参数的确定出了问题,因为soul提出用最大似然估计的EM算法确定C和Q,而我用的是最小二乘,而我们知道卡尔曼滤波在使用时是假设X(t)=CX(t-1)+W(t),即X为马尔可夫链式的高斯分布,所以明显地用最大似然估计得到的参数更适用于卡尔曼估计。今天的实验结论也证明了这一点。但是问题又来了,soul本人的论文中关于怎么样求参数这一问题却写的相当简略而且有点自相矛盾,而实际上,卡尔曼滤波的参数确定需要根据实际的问题来决定,这也是卡尔曼滤波应用的难点和关键点。soul的混乱与矛盾在于,论文的第一段说通过24小时直接测量到的X 算出C,第二段又笔锋一转:只测量X0,根据观测数据Y,通过EM算法确定参数C、Q的最大似然估计。这二段文字我读了很多遍,联系上下文揣摩了很多遍,觉得还是矛盾,后来我又在其参考文献里专门琢磨其EM算法,由于引入了SWITCH,导致公式符号复杂化,看起来很吃力,后来终于搞定。为了弄清楚EM算法,我又专门看了些专门介绍EM算法的论文,茹正亮的《EM算法在不完全参数估计中的应用》写的比较好,至少容易看懂,至此我明白了EM算法的原理,知道了soul论文中的矛盾不矛盾的唯一可能性:如果EM算法的的输入数据包括24小时直接测量到的X和Y,那么这个矛盾就不矛盾了。但我知道这是不可能的,因为EM算法在E步和M步递推时,必须要借助于隐藏数据X的概率密度,这样才能从上一下的参数确定出下一步的参数。

          4,在这一矛盾下,我选择了后者(只测量到X的一个初始态和24小时的Y,假设其余的X为未知的马尔可夫高斯分布),因为作者对于后者的介绍的多一些,于是我用代码直接编写的参数估计的EM最大似然估计算法,其具体的EM公式实在是很难看懂,运行后发现结果很不理想,因为中间根据上一步参数求出下一步参数时,需要用到卡尔曼平滑器(根据24小时的Y对X和P进行平滑处理),这样首先又需要应用卡尔曼滤波求出预测的估计的所有X,算法代码在运行时往往会发散。我昨天去图书馆借了几本关于卡尔曼的书,了解到原来是当P在小型机上计算时中间有一个矩阵减法,这样容易导致其不正定,进而导致迭代计算时会发散。由于初始参数的选取我是采用了过程1中提到的最小二乘算出的C和Q,所以便出现了1中出现的发散现象。总之EM算法对参数的初始值很敏感,这样,初值的选定成了问题。这一部分现在我还没有搞清楚。

          5,我决定走自己的路,选择矛盾的前者,即通过直接测量的24小时的X,通过最大似然估计求出C才Q(假设R=0,即Y的测量无误差),这一条路其实中间我尝试过,似然函数为矩阵C和Q的函数,由于无法用EM算法求解,所以只好直接求偏导算极值,但是遇到了函数矩阵的问题,即把C和Q这样的矩阵看作变量,如何求导?查阅矩阵分析的书,都是介绍矩阵函数的(矩阵的元素为函数),昨天在图书馆看到一本特殊矩阵的书,里面才有提到函数矩阵的概念,和一些性质(这时候我才知道这个叫函数矩阵)。至此我已经觉得彻底无路可走了,因为函数矩阵的问题我是无法搞定了。

          6,转念一想,既然写成矩阵无法求解,那我就再将矩阵的各个元素分别列出来,独立求解,昨天晚上推导了一会,发现了可行性,便编写了代码验证,由于算法比较粗糙,用MATLAB运行了1个多小时,未出结果,因为矩阵过大,MATLAB存不下。今天上午重新对算法改进,一行一行的求解C,这样终出求了出来。根据C再求出Q,将C和Q代入之前的卡尔曼,经过对一些错误的修正,终于取了满意的实验结果。

      整个过程中,一方面碰到矛盾、问题时好多次都绝望了,想过放弃,改做别的方法,中间我还做了一些实验,关于相关系数平稳性的实验,发现任何周期的相关系数都极不稳定,没有任何规律可言,而OD流的周期性通过我采用的一种独特的平滑技术才表现出天的周期性。这让我觉得卡尔曼在用于TM估计时是无力的,因为24小时估计出来的时空关系C(为常数)是粗糙的。我还据此产生的新的想法,略去卡尔曼的传输过程,直接通过Y求X,运用平滑技术的周期性,但广义逆的问题不好解决,这样做出来的东西是个什么东西,没有理论支持,就暂且搁下了。

      今天的实验结果让我看到了卡尔曼的强大,它的优秀自适应调整能力。到目前为止,我还是很惊异于它居然有这么好的实验结果。我的关于卡尔曼的改进还有没有可提升的空间?这一问题变得更容易被否定。目前周静静提出了一种UD分解的平方根卡尔曼滤波,对我的研究冲击很大。我也是因此而昨天去的图书馆,找到了卡尔曼发散的原因呵。

      先写到这里。路还很长,而且分岔口很多。

     

    本文来自CSDN博客,转载请标明出处:http://blog.csdn.net/vert_/archive/2009/11/08/4785179.aspx

    展开全文
  • 1) 标准卡尔曼滤波器2) 扩展卡尔曼滤波器3) 双卡尔曼滤波器4) 平方根卡尔曼滤波器 该软件包还包含每种过滤器类型的说明性示例,演示它们的实际应用。 在所有 4 种情况下,KF 函数都接受多维系统的输入噪声样本,并...
  • 卡尔曼滤波使用框架

    2017-11-23 17:07:48
    搭建了卡尔曼跟踪的基本框架,可在本框架下进一步开发
  • 卡尔曼滤波器

    2013-03-26 14:36:42
    卡尔曼滤波器
  • 7 对于一个运动模型,建立卡尔曼滤波模型,进行仿真,设已知初始时刻运动目标的真实位置和速度,并已知卡尔曼滤波使用的初始状态值,对该问题给出仿真;进一步分析该问题的稳态卡尔曼解,直接使用稳态卡尔曼滤波...
  • 不敏卡尔曼滤波器与卡尔曼滤波器对比
  • 介绍卡尔曼滤波的方法与卡尔曼滤波代码,对初学者有很大的帮助

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 19,383
精华内容 7,753
关键字:

卡尔曼怎么用