精华内容
下载资源
问答
  • 无迹卡尔曼滤波算法

    2012-03-20 22:42:07
    无迹卡尔曼滤波matlab仿真, UKF,有图有真相,不容错过哦
  • UKF的matlab代码,用于无迹卡尔曼滤波算法的学习,希望大家多多交流,共同学习.
  • 平方根无迹卡尔曼滤波算法程序 360 平方根无迹卡尔曼滤波算法程序 欢迎下载
  • 运动物体的轨迹预测无迹卡尔曼滤波算法实现
  • 在传统无迹卡尔曼滤波(UKF) 中对其估计精度和计算效率起关键作用的是采样算法, 即构造具有权重的样本点. 研究表明, 带权样本点匹配随机变量的阶矩越高滤波的精度越高, 如多项式无迹卡尔曼滤波(PUKF), 但通常此类算法...
  • 无迹卡尔曼滤波算法在目标跟踪中的研究,非常不错的资源,写的浅显易懂
  • 原始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别? 原文:https://www.zhihu.com/question/22714163/answer/87629897 KF针对于线性高斯的情况,EKF针对于非...

    原始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别?

    原文:https://www.zhihu.com/question/22714163/answer/87629897

     

    KF针对于线性高斯的情况,EKF针对于非线性高斯,其是将非线性部分进行一阶泰勒展开,因此忽 略了高阶项,误差较大,UKF是将UT变换与KF结合的产物,它的基础理念是接近一个非线性函数的 概率分布非接近其本身更简单。后两种卡尔曼是针对同一问题的不同思路的解决方案,其实UKF的 能力已经跳出了非线性高斯的范围,其也可以解决非高斯问题,只不过在这方面PF能做的更好,运 算量也更大。

     

    1.KF是线性滤波器,不多解释。2.说到非线性卡尔曼滤波器,是由贝叶斯滤波理论得出统一的理论框架:非线性卡尔曼滤波器的状态估计可等效为多维向量积分的计算。积分形式可表示为非线性函数x高斯概率密度函数。但该积分的闭式解很难求解,所以得对高斯加权的多维非线性函数的积分进行近似。近似主要分两大类:非线性函数的近似和高斯概率密度函数的近似。

    非线性函数的近似有基于泰勒级数展开的ekf,基于差分的DDF和多项式卡尔曼(比如 雪夫多项式等)。

    概率密度函数的近似主要有无迹变换(ukf),容积卡尔曼,中心差分卡尔曼,或是把协方差矩阵分解构成基于平方根的卡尔曼滤波器等。所以卡尔曼滤波器都是对后验密度做假设的次优估计。而粒子滤波,高斯混合滤波器,自适应网格点群滤波器等是不对后验概率密度做任何假设的最优估计。不过计算量挺大的。像基于ukf+srcdkf的位置状态估计,在进行时间更新的时候,就需要更新1700多个数据。要是采用粒子滤波计算量就更大……


    图说卡尔曼滤波,一份通俗易懂的教程

    原文:https://zhuanlan.zhihu.com/p/39912633

    作者:Bzarg

    编译:Bot

    编者按:卡尔曼滤波(Kalman filter)是一种高效的自回归滤波器,它能在存在诸多不确定性情况的组合信息中估计动态系统的状态,是一种强大的、通用性极强的工具。它的提出者,鲁道夫.E.卡尔曼,在一次访问NASA埃姆斯研究中心时,发现这种方法能帮助解决阿波罗计划的轨道预测问题,后来NASA在阿波罗飞船的导航系统中确实也用到了这个滤波器。最终,飞船正确驶向月球,完成了人类历史上的第一次登月。

    本文是国外博主Bzarg在2015年写的一篇图解。虽然是几年前的文章,但是动态定位、自动导航、时间序列模型、卫星导航——卡尔曼滤波的应用范围依然非常广。那么,作为软件工程师和机器学习工程师,你真的了解卡尔曼滤波吗?

    什么是卡尔曼滤波?

    对于这个滤波器,我们几乎可以下这么一个定论:只要是存在不确定信息的动态系统,卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰,卡尔曼滤波通常也能很好的弄清楚究竟发生了什么,找出现象间不易察觉的相关性。

    因此卡尔曼滤波非常适合不断变化的系统,它的优点还有内存占用较小(只需保留前一个状态)、速度快,是实时问题和嵌入式系统的理想选择。

    如果你曾经Google过卡尔曼滤波的教程(如今有一点点改善),你会发现相关的算法教程非常可怕,而且也没具体说清楚是什么。事实上,卡尔曼滤波很简单,如果我们以正确的方式看它,理解是很水到渠成的事。

    本文会用大量清晰、美观的图片和颜色来解释这个概念,读者只需具备概率论和矩阵的一般基础知识。

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

    让我们举个例子:你造了一个可以在树林里四处溜达的小机器人,为了让它实现导航,机器人需要知道自己所处的位置。

    也就是说,机器人有一个包含位置信息和速度信息的状态 [公式] :

    注意,在这个例子中,状态是位置和速度,放进其他问题里,它也可以是水箱里的液体体积、汽车引擎温度、触摸板上指尖的位置,或者其他任何数据。

    我们的小机器人装有GPS传感器,定位精度10米。虽然一般来说这点精度够用了,但我们希望它的定位误差能再小点,毕竟树林里到处都是土坑和陡坡,如果机器人稍稍偏了那么几米,它就有可能滚落山坡。所以GPS提供的信息还不够充分。

    我们也可以预测机器人是怎么移动的:它会把指令发送给控制轮子的马达,如果这一刻它始终朝一个方向前进,没有遇到任何障碍物,那么下一刻它可能会继续坚持这个路线。但是机器人对自己的状态不是全知的:它可能会逆风行驶,轮子打滑,滚落颠簸地形……所以车轮转动次数并不能完全代表实际行驶距离,基于这个距离的预测也不完美。

    这个问题下,GPS为我们提供了一些关于状态的信息,但那是间接的、不准确的;我们的预测提供了关于机器人轨迹的信息,但那也是间接的、不准确的。

    但以上就是我们能够获得的全部信息,在它们的基础上,我们是否能给出一个完整预测,让它的准确度比机器人搜集的单次预测汇总更高?用了卡尔曼滤波,这个问题可以迎刃而解。

    卡尔曼滤波眼里的机器人问题

    还是上面这个问题,我们有一个状态,它和速度、位置有关:

    我们不知道它们的实际值是多少,但掌握着一些速度和位置的可能组合,其中某些组合的可能性更高:

    卡尔曼滤波假设两个变量(在我们的例子里是位置和速度)都应该是随机的,而且符合高斯分布。每个变量都有一个均值 [公式] ,它是随机分布的中心;有一个方差 [公式] ,它衡量组合的不确定性。

    在上图中,位置和速度是不相关的,这意味着我们不能从一个变量推测另一个变量。

    那么如果位置和速度相关呢?如下图所示,机器人前往特定位置的可能性取决于它拥有的速度。

    这不难理解,如果基于旧位置估计新位置,我们会产生这两个结论:如果速度很快,机器人可能移动得更远,所以得到的位置会更远;如果速度很慢,机器人就走不了那么远。

    这种关系对目标跟踪来说非常重要,因为它提供了更多信息:一个可以衡量可能性的标准。这就是卡尔曼滤波的目标:从不确定信息中挤出尽可能多的信息!

    为了捕获这种相关性,我们用的是协方差矩阵。简而言之,矩阵的每个值是第 [公式] 个变量和第 [公式] 个变量之间的相关程度(由于矩阵是对称的, [公式] 和 [公式] 的位置可以随便交换)。我们用 [公式] 表示协方差矩阵,在这个例子中,就是 [公式] 。

    用矩阵描述问题

    为了把以上关于状态的信息建模为高斯分布(图中色块),我们还需要 [公式] 时的两个信息:最佳估计 [公式] (均值,也就是 [公式] ),协方差矩阵 [公式] 。(虽然还是用了位置和速度两个变量,但只要和问题相关,卡尔曼滤波可以包含任意数量的变量)

    接下来,我们要通过查看当前状态(k-1时)来预测下一个状态(k时)。这里我们查看的状态不是真值,但预测函数无视真假,可以给出新分布:

    我们可以用矩阵 [公式] 表示这个预测步骤:

    它从原始预测中取每一点,并将其移动到新的预测位置。如果原始预测是正确的,系统就会移动到新位置。

    这是怎么做到的?为什么我们可以用矩阵来预测机器人下一刻的位置和速度?下面是个简单公式:

    换成矩阵形式:

    这是一个预测矩阵,它能给出机器人的下一个状态,但目前我们还不知道协方差矩阵的更新方法。这也是我们要引出下面这个等式的原因:如果我们将分布中的每个点乘以矩阵A,那么它的协方差矩阵会发生什么变化

    把这个式子和上面的最佳估计 [公式] 结合,可得:

    外部影响

    但是,除了速度和位置,外因也会对系统造成影响。比如模拟火车运动,除了列车自驾系统,列车操作员可能会手动调速。在我们的机器人示例中,导航软件也可以发出停止指令。对于这些信息,我们把它作为一个向量 [公式] ,纳入预测系统作为修正。

    假设油门设置和控制命令是已知的,我们知道火车的预期加速度 [公式] 。根据运动学基本定理,我们可得:

    把它转成矩阵形式:

    [公式] 是控制矩阵, [公式] 是控制向量。如果外部环境异常简单,我们可以忽略这部分内容,但是如果添加了外部影响后,模型的准确率还是上不去,这又是为什么呢?

    外部不确定性

    当一个国家只按照自己的步子发展时,它会自生自灭。当一个国家开始依赖外部力量发展时,只要这些外部力量是已知的,我们也能预测它的存亡。

    但是,如果存在我们不知道的力量呢?当我们监控无人机时,它可能会受到风的影响;当我们跟踪轮式机器人时,它的轮胎可能会打滑,或者粗糙地面会降低它的移速。这些因素是难以掌握的,如果出现其中的任意一种情况,预测结果就难以保障。

    这要求我们在每个预测步骤后再加上一些新的不确定性,来模拟和“世界”相关的所有不确定性:

    如上图所示,加上外部不确定性后, [公式] 的每个预测状态都可能会移动到另一点,也就是蓝色的高斯分布会移动到紫色高斯分布的位置,并且具有协方差 [公式] 。换句话说,我们把这些不确定影响视为协方差 [公式] 的噪声。

    这个紫色的高斯分布拥有和原分布相同的均值,但协方差不同。

    我们在原式上加入 [公式] :

    简而言之,这里:

    [公式] 是基于 [公式] 和 [公式] 校正后得到的预测。

    [公式] 是基于 [公式] 和 [公式] 得到的预测。

    现在,有了这些概念介绍,我们可以把传感器数据输入其中。

    通过测量来细化估计值

    我们可能有好几个传感器,它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点,它可以读取位置,可以读取速度,重点是,它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。

    请注意,读数的规模和状态的规模不一定相同,所以我们把传感器读数矩阵设为 [公式] 。

    把这些分布转换为一般形式:

    卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说,由于种种因素,传感器记录的信息其实是不准的,一个状态事实上可以产生多种读数。

    我们将这种不确定性(即传感器噪声)的协方差设为 [公式] ,读数的分布均值设为 [公式] 。

    现在我们得到了两块高斯分布,一块围绕预测的均值,另一块围绕传感器读数。

    如果要生成靠谱预测,模型必须调和这两个信息。也就是说,对于任何可能的读数 [公式] ,这两种方法预测的状态都有可能是准的,也都有可能是不准的。重点是我们怎么找到这两个准确率。

    最简单的方法是两者相乘:

    两块高斯分布相乘后,我们可以得到它们的重叠部分,这也是会出现最佳估计的区域。换个角度看,它看起来也符合高斯分布:

    事实证明,当你把两个高斯分布和它们各自的均值和协方差矩阵相乘时,你会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了:我们必须有一个公式来从旧的参数中获取这些新参数!

    结合高斯

    让我们从一维看起,设方差为 [公式] ,均值为 [公式] ,一个标准一维高斯钟形曲线方程如下所示:

    那么两条高斯曲线相乘呢?

    把这个式子按照一维方程进行扩展,可得:

    如果有些太复杂,我们用k简化一下:

     

    以上是一维的内容,如果是多维空间,把这个式子转成矩阵格式:

    这个矩阵 [公式] 就是我们说的卡尔曼增益,easy!

    把它们结合在一起

    截至目前,我们有用矩阵 [公式] 预测的分布,有用传感器读数 [公式] 预测的分布。把它们代入上节的矩阵等式中:

    相应的,卡尔曼增益就是:

    考虑到 [公式] 里还包含着一个 [公式] ,我们再精简一下上式:

    最后, [公式] 是我们的最佳估计值,我们可以把它继续放进去做另一轮预测:

    希望这篇文章能对你有用!

    展开全文
  • 文章目录1 前言2 通过无迹变换实现线性化3 无迹卡尔曼滤波算法4 总结5 参考文献 1 前言 前文介绍了扩展卡尔曼滤波,它对非线性函数线性化的方法是泰勒展开式,但这不是唯一线性化的方法 本文介绍的无迹卡尔曼滤波...

    1 前言

    • 前文介绍了扩展卡尔曼滤波,它对非线性函数线性化的方法是泰勒展开式,但这不是唯一线性化的方法
    • 本文介绍的无迹卡尔曼滤波(unscented Kalman filter, UKF)是应用无迹变换进行线性化
    • 无迹变换要比泰勒展开式更加准确,所以对于非线性滤波效果更好

    2 通过无迹变换实现线性化

    • 这里不做公式推导和证明,仅给出算法所需无迹变换实现线性化的公式
    • 无迹变换是通过从高斯分布中提取σ\sigma点(χ[i]\chi^{[i]}),再对这些σ\sigma点进行非线性变换,具体方法见第3部分
    • 对于nn维高斯分布要提取2n+12n+1σ\sigma点(χ[i]\chi^{[i]})
    • 这些σ\sigma点(χ[i]\chi^{[i]})位于均值附近,对称分布:
      χ[0]=μχ[i]=μ+((n+λ)Σ)i  i=1,,nχ[i]=μ((n+λ)Σ)in  i=n+1,,2n式中:λ=α2(n+k)n    (α,k:为尺度参数,决定σ点距离均值的远近)注意:(n+λ)Σ是矩阵开平方,例如:B=AATA=B, matlab函数为sqrtm() \begin{aligned} \chi^{[0]} &= \mu\\ \chi^{[i]} &= \mu + \left (\sqrt{(n+\lambda)\Sigma}\right )_i ~~ i = 1, \cdots, n\\ \chi^{[i]} &= \mu - \left (\sqrt{(n+\lambda)\Sigma}\right )_{i-n} ~~ i = n+1, \cdots, 2n\\ \end{aligned}\\ \text{式中:}\lambda = \alpha^2(n+k) - n~~~~(\alpha,k\text{:为尺度参数,决定$\sigma$点距离均值的远近})\\ \text{注意:$\sqrt{(n+\lambda)\Sigma}$是矩阵开平方,例如:$\sqrt{B} = A \Rightarrow A^TA = B$, matlab函数为sqrtm()}
    • 有两个权重:
      • wm[i]w_m^{[i]}:用于计算均值
      • wc[i]w_c^{[i]}:用于计算方差
        wm[0]=λn+λwc[0]=λn+λ+(1α2+β)wm[i]=wc[i]=12(n+λ)    i=1,,2n \begin{aligned} w_m^{[0]} &= \frac{\lambda}{n + \lambda}\\ w_c^{[0]} &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\\ w_m^{[i]} &= w_c^{[i]} = \frac{1}{2(n + \lambda)} ~~~~i = 1,\cdots, 2n \end{aligned}
    • σ\sigma点(χ[i]\chi^{[i]})进行非线性变换:
      y[i]=g(χ[i]) y^{[i]} = g(\chi^{[i]})
    • 从变换后的点y[i]y^{[i]}中计算均值和方差
      μ=i=02nwm[i]y[i]Σ=i=02nwc[i](y[i]μ)(y[i]μ)T \begin{aligned} \mu' &= \sum_{i=0}^{2n} w_m^{[i]} y^{[i]} \\ \Sigma' &= \sum_{i=0}^{2n} w_c^{[i]} (y^{[i]} - \mu')(y^{[i]} - \mu')^T \\ \end{aligned}
    • 到这里无迹变换线性化的公式就结束了,我们看看无迹卡尔曼滤波算法是怎样的步骤

    3 无迹卡尔曼滤波算法

    • 这里给出无迹卡尔曼滤波算法,并与扩展卡尔曼滤波做一下简要对比:

      • 252\sim 5行:是扩展卡尔曼滤波的第3,43,4行,进行状态预测
        • 22行:根据先验置信度计算σ\sigma点(χt1\chi_{t-1})
        • 33行:根据σ\sigma点(χt1\chi_{t-1})运用非线性状态预测函数gg进行状态预测
        • 4,54,5行:根据预测的状态计算该状态的均值和方差(具体公式不做解释)
      • 6136\sim 13行:是扩展卡尔曼滤波的第464 \sim 6行,进行测量更新
        • 66行:计算新的σ\sigma点(χtˉ\bar{\chi_{t}})
        • 77行:对新的σ\sigma点(χtˉ\bar{\chi_{t}})进行观测值预测
        • 8,98,9行:计算观测值z^t\hat{z}_t和不确定度StS_t,对应于卡尔曼滤波即是计算HtΣˉtHtT+QtH_t \bar{\Sigma}_t H_t^T + Q_t
        • 1010行:计算状态和观测值的协方差矩阵,对应于卡尔曼滤波即是计算ΣˉtHtT\bar{\Sigma}_t H_t^T
        • 111311 \sim 13行:计算卡尔曼增益、状态均值和方差
      • 无迹卡尔曼滤波算法

    在这里插入图片描述

      • 扩展卡尔曼滤波算法

    在这里插入图片描述

    4 实例

    • 这里采用扩展卡尔曼滤波的例子
    • 实例:雷达监测空中抛物轨迹
      • 从空中位置(x(0)=0,y(0)=500)(x(0)=0,y(0)=500)水平抛射出一个物体(初始水平速度为vx(0)=50v_x(0)=50,初始竖直速度为vy(0)=0v_y(0)=0
      • 物体受重力g=9.8g=9.8和阻尼力(与速度的平方成正比)的影响
      • 水平和竖直阻尼系数分别为kx=0.01,ky=0.05k_x=0.01,k_y=0.05,不确定度为零均值白噪声δaxN(0,0.09),δayN(0,0.09)\delta a_x \sim N(0, 0.09),\delta a_y \sim N(0, 0.09)
      • 在坐标原点处有一雷达,可测得距离rr,角度α\alpha, 不确定度为零均值白噪声δrN(0,64),δαN(0,0.01)\delta r \sim N(0, 64),\delta \alpha \sim N(0, 0.01)

    在这里插入图片描述

    • 状态变量: 物体横向位置x(k)x(k),物体横向速度vx(k)v_x(k),物体纵向位置y(k)y(k),物体纵向速度vy(k)v_y(k)
      X(k)=[x(k),vx(k),y(k),vy(k)] X(k) = [x(k), v_x(k), y(k), v_y(k)]
    • 状态方程:
      x(k+1)=x(k)+vx(k)Tvx(k+1)=vx(k)(kxvx2(k)+δax)Ty(k+1)=y(k)+vy(k)Tvy(k+1)=vy(k)+(kyvy2(k)g+δay)T \begin{aligned} x(k+1) &= x(k) + v_x(k) \cdot T\\ v_x(k+1) &= v_x(k) - (k_x \cdot v_x^2(k) + \delta a_x)\cdot T\\ y(k+1) &= y(k) + v_y(k) \cdot T \\ v_y(k+1) &= v_y(k) + (k_y \cdot v_y^2(k) - g + \delta a_y)\cdot T \end{aligned}
    • 观测方程:
      r(k)=x(k)2+y(k)2+δrα(k)=arctanx(k)y(k)+δα \begin{aligned} r(k) &= \sqrt{x(k)^2 + y(k)^2 } + \delta r\\ \alpha(k) &= \arctan{\frac{x(k)}{y(k)}} + \delta \alpha\\ \end{aligned}
    • 仿真时间总为t=15t=15,采样周期T=0.1T=0.1
    • 无迹卡尔曼滤波(UKF)与扩展卡尔曼滤波(EKF)效果如图:

    在这里插入图片描述

    • 上图可以看出无迹卡尔曼滤波也达到良好的滤波效果,但是为了更好地与扩展卡尔曼滤波进行比较,给出两种滤波方法在XXYY两个方向的误差图, 可以看出UKF略优于EFK

    在这里插入图片描述

    在这里插入图片描述

    • matlab 代码
    clear;
    close all;
    clc
    %% 初值设定
    x_0 = 0;                    %初始x位置
    y_0 = 500;                  %初始y位置
    v_x_0 = 50;                 %初始x速度
    v_y_0 = 0;                  %初始y速度
    g = 9.8;                    % 重力加速度
    k_x = 0.01;                 % 阻尼系数
    k_y = 0.05;                 % 阻尼系数
    sigma_a_x = 0.09;           % 状态转移不确定度方差
    sigma_a_y = 0.09;           % 状态转移不确定度方差
    sigma_r = 64;               % 测量不确定度方差
    sigma_alpha = 0.01;         % 测量不确定度方差
    
    t = 15;                     % 仿真时间
    T = 0.1;                    % 采样周期 
    len = fix(t/T);             % 仿真步数
    %% 真实轨迹
    X = zeros(len, 4);
    X(1,:) = [x_0, v_x_0, y_0, v_y_0];
    for k = 2 : len
        x_k = X(k-1,1);
        v_x_k = X(k-1,2);
        y_k = X(k-1,3);
        v_y_k = X(k-1,4);
        x_k = x_k + v_x_k * T;
        v_x_k = v_x_k - (k_x*v_x_k^2 + sqrt(sigma_a_x)*randn(1,1)) * T;
        y_k = y_k + v_y_k * T;
        v_y_k = v_y_k + (k_y*v_y_k^2 - g + sqrt(sigma_a_y)*randn(1,1)) * T;   
        X(k,:) = [x_k, v_x_k, y_k, v_y_k];
    end
    X_temp = X;
    %% 雷达测量
    Z = zeros(len, 2);
    for k = 1 : len
        x_k = X(k,1);
        y_k = X(k,3);
        r = sqrt(x_k^2 + y_k^2) + sqrt(sigma_r)*randn(1,1);
        alpha = atan(x_k / y_k) * 180 / pi + sqrt(sigma_alpha)*randn(1,1);
        Z(k,:) = [r, alpha];
    end
    %% UKF 无迹卡尔曼滤波
    n = 4;                                                      % 状态变量的数量
    alpha_lambda = 1; k_lambda = 1;                             % 尺度参数,决定σ点距离均值的远近
    lambda = alpha_lambda^2 * (n + k_lambda) - n;               % 用于计算σ点
    chi_t_1 = zeros(2*n + 1, n);                                % σ点
    chi_t = zeros(2*n + 1, n);                                  % σ点
    w_m = zeros(2*n + 1, 1);                                    % 计算用于计算均值
    w_m(1, 1) = lambda / (n + lambda);
    w_m(2:end, 1) = 1 / (2*(n + lambda));
    w_c = zeros(2*n + 1, 1);                                    % 计算用于计算方差
    w_c(1,1) = lambda / (n + lambda) + (1 - alpha_lambda^2 + 2);
    w_c(2:end, 1) = 1 / (2*(n + lambda));              
    R_k = diag([0; sigma_a_x; 0; sigma_a_y]);                   % 状态转移误差的协方差矩阵
    Q_k = diag([sigma_r, sigma_alpha]);                         % 测量函数误差的协方差矩阵
    sigma_k = 10 * eye(n);                                      % 最优状态协方差矩阵
    sigma_bar_k= 10 * eye(n);                                   % 预测状态协方差矩阵
    mu_k = [0, 40, 400, 0]';                                    % 最优状态均值矩阵
    mu_bar_k = zeros(n,1);                                      % 预测的状态均值矩阵
    z_k = zeros(4,1);                                           % 观测矩阵
    X_UKF_est = zeros(len,n);                                   % UKF后的状态存储
    for k = 1 : len
        %% 1 状态预测
        % line: 2
        temp = sqrtm((n + lambda) * sigma_k);
        for i = 1 : 2 * n + 1
           if i == 1
               chi_t_1(i,:) =  mu_k';
           elseif i <= n+ 1
               chi_t_1(i,:) = mu_k' + real(temp(i - 1, :));
           else
               chi_t_1(i,:) = mu_k' - real(temp(i - n - 1, :));
           end
        end
        % line: 3
        chi_star = zeros(2*n + 1, n);
        chi_star(:,1) = chi_t_1(:,1) + chi_t_1(:,2)*T;
        chi_star(:,2) = chi_t_1(:,2) - (k_x*chi_t_1(:,2).^2)*T;
        chi_star(:,3) = chi_t_1(:,3) + chi_t_1(:,4)*T;
        chi_star(:,4) = chi_t_1(:,4) + (k_y*chi_t_1(:,4).^2 - g)*T;
        % line: 4
        mu_bar_k  = chi_star' * w_m;
        % line: 5
        temp  = zeros(n,n);
        for i = 1 : 2 * n + 1
            temp = temp + w_c(i, 1) * (chi_star(i,:)' - mu_bar_k) * (chi_star(i,:)' - mu_bar_k)';     
        end
        sigma_bar_k = temp  + R_k;
        %% 2 观测更新
        % line: 6
        temp = sqrtm((n + lambda) * sigma_k);
        for i = 1 : 2 * n + 1
           if i == 1
               chi_t(i,:) =  mu_bar_k';
           elseif i <= n+ 1       
               chi_t(i,:) = mu_bar_k' + real(temp(i - 1, :));
           else
               chi_t(i,:) = mu_bar_k' - real(temp(i - n - 1, :));
           end
        end
        % line: 7
        x_k = chi_t(:,1);
        y_k = chi_t(:,3);
        r = sqrt(x_k.^2 + y_k.^2);
        alpha = atan(x_k ./ y_k) * 180 / pi;
        Z_k = [r,alpha] ;
        % line: 8
        z_k = w_m' * Z_k;
        % line: 9
        temp = zeros(2,2);
        for i = 1 : 2 * n + 1
            temp = temp + w_c(i, 1) * (Z_k(i,:) - z_k)' * (Z_k(i,:) - z_k);     
        end
        S_k = temp  + Q_k;   
        % line: 10
        Sigma_x_z = zeros(4,2);
        for i = 1 : 2 * n + 1
            Sigma_x_z = Sigma_x_z + w_c(i, 1) * (chi_t(i,:)' - mu_bar_k) * (Z_k(i,:) - z_k);     
        end
        % line: 11
        K_k = Sigma_x_z * S_k^(-1);
        % line: 12
        mu_k = mu_bar_k + K_k * (Z(k,:) - z_k)';  
        % line: 13
        sigma_k = sigma_bar_k - K_k * S_k * K_k';
        %% 3 存储EKF后的值
        X_UKF_est(k,:) = mu_k;
    end
    %% EKF 扩展卡尔曼滤波
    R_k = diag([0; sigma_a_x; 0; sigma_a_y]);	% 状态转移误差的协方差矩阵
    Q_k = diag([sigma_r, sigma_alpha]);         % 测量函数误差的协方差矩阵
    sigma_k = 10 * eye(4);                      % 最优状态协方差矩阵
    sigma_bar_k= 10 * eye(4);                   % 预测状态协方差矩阵
    mu_k = [0, 40, 400, 0]';                    % 最优状态均值矩阵
    mu_bar_k = zeros(4,1);                      % 预测的状态均值矩阵
    z_k = zeros(4,1);                           % 观测矩阵
    X_EKF_est = zeros(len,4);                       %EKF后的状态存储
    for k = 1 : len
        % 1 状态预测
        x1 = mu_k(1) + mu_k(2)*T;
        v_x1 = mu_k(2) - (k_x*mu_k(2)^2)*T;
        y1 = mu_k(3) + mu_k(4)*T;
        v_y1 = mu_k(4) + (k_y*mu_k(4)^2 - g)*T;
        mu_bar_k = [x1; v_x1; y1; v_y1];            % 预测的均值   
        G_k = zeros(4,4);                       % 状态雅可比矩阵
        G_k(1,1) = 1; G_k(1,2) = T;
        G_k(2,2) = 1 - 2*k_x*v_x1*T;
        G_k(3,3) = 1; G_k(3,4) = T;
        G_k(4,4) = 1 + 2*k_y*v_y1*T;    
        sigma_bar_k = G_k * sigma_k * G_k' + R_k;
        % 2 观测更新
        r = sqrt(x1*x1+y1*y1);
        alpha = atan(x1/y1)*180/pi;
        z_k = [r, alpha]';                      % h(\bar{\mu}_t) 
        H_k = zeros(2,4);                       % 状态雅可比矩阵
        x = mu_k(1); y = mu_k(3); 
        H_k(1,1) = x / r;   H_k(1,3) = y / r;
        H_k(2,1) = (1/y)/(1 + (x/y)^2);   H_k(2,3) = (-x/(y^2))/(1 + (x/y)^2);
        K_k = sigma_bar_k * H_k' * (H_k * sigma_bar_k * H_k' + Q_k)^(-1);
        mu_k = mu_bar_k + K_k * (Z(k,:)' - z_k);
        sigma_k = (eye(4) - K_k * H_k) * sigma_bar_k;
        % 3 存储EKF后的值
        X_EKF_est(k,:) = mu_k;
    end
    %% 绘图 
    figure, hold on, grid on;
    plot(X(:,1),X(:,3),'-g');                                               %真实位置
    plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180),'-b');      %观测位置
    plot(X_UKF_est(:,1),X_UKF_est(:,3), 'c');                               %UKF最优位置
    plot(X_EKF_est(:,1),X_EKF_est(:,3), 'r');                               %EKF最优位置
    xlabel('X'); 
    ylabel('Y'); 
    title('UKF simulation');
    legend('real', 'measurement', 'UKF', 'EKF');
    axis([-5,230,290,530]);
    
    figure, hold on, grid on;
    T_plot = linspace(0, T, len);
    plot(T_plot, abs(X(:,1) - X_UKF_est(:,1)),'-c');                                      %UKF X误差
    plot(T_plot, abs(X(:,1) - X_EKF_est(:,1)), '-r');                                     %EKF X误差 
    xlabel('t'); 
    ylabel('X error'); 
    title('UKF EKF X error simulation');
    legend('UKF X error', 'EKF X error');
    
    figure, hold on, grid on;
    plot(T_plot, abs(X(:,3) - X_UKF_est(:,3)),'-c');                                      %UKF Y误差
    plot(T_plot, abs(X(:,3) - X_EKF_est(:,3)), '-r');                                     %EKF Y误差 
    xlabel('t'); 
    ylabel('Y error'); 
    title('UKF EKF Y error simulation');
    legend('UKF Y error', 'EKF Y error');
    

    5 总结

    • 无迹卡尔曼滤波优缺点:
      • 缺点:
        1. 计算速度慢于扩展卡尔曼滤波
        2. 对于近似是高斯分布的场合,滤波效果较好;但是对于置信度的非高斯分布的场合,滤波效果较差
      • 优点:
        1. 对于线性系统,滤波效果与卡尔曼滤波相同
        2. 对于非线性系统,无迹卡尔曼滤波效果优于扩展卡尔曼滤波
        3. 无需计算雅克比矩阵(对于雅克比矩阵很难求得的场合也适用)

    6 参考文献

    • Thrun, & Sebastian. (2005). Probabilistic robotics.
    展开全文
  • 无迹卡尔曼滤波算法(UKF)详细介绍及其仿真 著名学者Julier等提出近似非线性函数的均值和方差远比近似非线性函数本身更容易,因此提出了基于确定性采样的UKF算法。 该算法的核心思想是:采用UT变换,利用一组Sigma...

    著名学者Julier等提出近似非线性函数的均值和方差远比近似非线性函数本身更容易,因此提出了基于确定性采样的UKF算法。
    该算法的核心思想是:采用UT变换,利用一组Sigma采样点来描述随机变量的高斯分布,然后通过非线性函数的传递,再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。
    相比于EKF,UKF的估计精度能够达到泰勒级数展开的二阶精度。

    1. UT变换
    2. 采样策略
    3. UKF算法流程
    4. UKF算法仿真

    UT变换
    采样策略:
    根据Sigma点采样策略不同,相应的Sigma点以及均值权值和方差权值也不尽相同,因此UT变换的估计精度也会有差异,但总体来说,其估计精度能够达到泰勒级数展开的二阶精度。
    为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性,所以采样点的选取应满足:
    采样策略
    下面介绍两种经常使用的采样策略:比例采样和比例修正对称采样
    比例采样
    比例修正对称采样
    比例修正对称采样
    系统
    UKF
    UKF
    至此UKF算法介绍完毕。
    UKF算法仿真:
    仿真的例子与上一篇EKF仿真的非线性系统相同。
    主程序:

    clear all;
    clc;
    tf = 50; 
    Q = 10;w=sqrt(Q)*randn(1,tf); 
    R = 1;v=sqrt(R)*randn(1,tf);
    P =eye(1);
    x=zeros(1,tf);
    Xnew=zeros(1,tf);
    x(1,1)=0.1; 
    Xnew(1,1)=x(1,1);
    z=zeros(1,tf);
    z(1)=x(1,1)^2/20+v(1); 
    zjian=zeros(1,tf);
    zjian(1,1)=z(1);
    linear = 0.5;
    for k = 2 : tf 
        % 模拟系统 
        x(:,k) = linear * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1)^2)) + 8 * cos(1.2*(k-1)) + w(k-1); %状态值 
        z(k) = (x(:,k)^2 / 20) + v(k);%观测值
        f=@(x)(linear * x + (2.5 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)));
        h=@(x)(x^2 / 20);
       [Xnew(:,k),P(:,:,k)] = ukf(f,Xnew(:,k-1),P(:,:,k-1),h,z(k),Q,R); 
    end
       figure;
       t = 2 : tf;
       plot(t,x(1,t),'b',t,Xnew(1,t),'r:');
       legend('真实值','UKF估计值');
    

    Sigma点集选取(2n+1个):

    function X = sigmas(x,P,c)  % x:参考点,P:协方差,c:系数,X:Sigma点
    A = c*chol(P)';  
    Y = x(:,ones(1,numel(x)));  
    X = [x Y+A Y-A]; 
    

    UT变换:

    function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R)  %f:非线性函数,X: sigma点 Wm:均值权值 Wc:方差权值
    L = size(X,2);  
    y = zeros(n,1);  
    Y = zeros(n,L);  
    for k=1:L  
        Y(:,k) = f(X(:,k));  %非线性传递后结果 r
        y = y+Wm(k)*Y(:,k);  %均值
    end  
    Y1 = Y-y(:,ones(1,L));  
    P = Y1*diag(Wc)*Y1'+R;    %协方差
    

    UKF:

    function [x,P] = ukf(fstate, x, P, hmeas, z, Q, R)    
    L = numel(x);                                 %状态数量
    m = numel(z);                                 %量测数量 
    a = 1e-3;                                     %默认  
    ki = 0;                                       %默认
    beta = 2;                                     %默认  
    lambda = a^2*(L+ki)-L;                          
    c = L+lambda;                                
    Wm = [lambda/c 0.5/c+zeros(1,2*L)];          
    Wc = Wm;  
    Wc(1) = Wc(1)+(1-a^2+beta);                
    c = sqrt(c);  
    X = sigmas(x,P,c);                            
    [x1,X1,P1,X2] = ut(fstate,X,Wm,Wc,L,Q);                       
    [z1,Z1,P2,Z2] = ut(hmeas,X1,Wm,Wc,m,R);          
    % 滤波部分  
    P12 = X2*diag(Wc)*Z2';                          
    K = P12*inv(P2);  
    x = x1+K*(z-z1);                                
    P = P1-K*P12'; 
    

    仿真结果:
    UKF仿真结果

    展开全文
  • 卡尔曼滤波在工程中的地位就好比牛顿力学三定律在经典力学的地位,如果数学领域有诺贝尔奖,卡尔曼滤波完全有资格获得此殊荣。现在我们看一下在视觉开发过程中卡尔曼滤波的神奇之处。毫无疑问,在使用视觉跟踪处理的...

    卡尔曼滤波在工程中的地位就好比牛顿力学三定律在经典力学的地位,如果数学领域有诺贝尔奖,卡尔曼滤波完全有资格获得此殊荣。现在我们看一下在视觉开发过程中卡尔曼滤波的神奇之处。

    毫无疑问,在使用视觉跟踪处理的过程中,首先你要先做到识别物体与定位,确定位置之后就可以使用卡尔曼滤波进行相应的处理了

    5d3f468ffa08af53eaac3eba020b92f7.png
    一图梳理卡尔曼滤波的过程与原理

    首先是最基本的卡尔曼滤波的五段式

    378fed54c080cb81676713b946e524c5.png
    原理性的东西难度很高,数学推导的过程大家可以去看一下黄小平老师的《卡尔曼滤波原理及应用》

    首先经过状态转移矩阵A对这一时刻的状态进行时间更新,之后进行协方差矩阵P的预测更新,得到X和P的一步预测值,之后计算增益矩阵K,K用于激活X和P的修正部分。之后使用得到的测量值(这个测量值实际上是带有噪声的,与真实的值有较大区别)从而得到X的修正值以及得到P的值,此时的X即为滤波之后的值。

    这里卡尔曼滤波的取信值取决于Q和R的值,Q越大越信任模型值,R越大越信任测量值,这种时候应该通过实验细调Q和R来得到最好的滤波效果。跑仿真也就图个乐,真调参还得看实验(暴论)

    得到的X如果设置为三维的向量,则X=[位置,速度,加速度],之后我们就可以预测再物体运动的相对较短时间内的下一时刻的位置实现预测了

    有关卡尔曼滤波的代码我就不放了,网上有很多,这里着重数一下无迹卡尔曼滤波(UKF)

    首先熟悉一下什么是UT变换:

    1.在原状态中按照某一规则选取一些采样点,使这些采样点的均值和方差等于原状态的均值和协方差

    c54fc419cfdba481019ab2475145e257.png

    具体过程:

    1.选取2n+1个Sigma点X和对应的权值

    来计算y的统计特征,这里的n为相应的状态维度

    d2c13bd3bc3c14e84b6b58082729adcf.png
    其中P为协方差矩阵

    计算这个Sigma点相应的权值

    640f37100a453bfc1ab9092e75d85984.png
    其中上标为第几个采样点,lamda为一个缩放比例参数用于降低总的预测误差,lamda=a^2(n+k)-n,这里a可以控制采样点的分布,注意要保证(n+lamda)P为半正定矩阵

    下面为无迹卡尔曼滤波的算法具体过程:

    1.首先实现非线性系统的描述:

    44c4953985da2a285ff52790a982f1f6.png
    f为非线性状态方程函数,h为非线性观测方程函数

    2.进行UT变换得到一组Sigma点集及其对应的权值

    efd349b4eb4dee91a2b21cbd0703a5b9.png

    3.计算2n+1个Sigma点集的进一步预测,其中i=1,2……2n+1

    f0a3c9e1a54282efda49540f2f2770a9.png

    之后继续计算系统状态量的预测和协方差矩阵,其由Sigma点集的预测值加权得到(时间更新)

    a93c5c54e6248af44a91b412a0ae1056.png

    4d1cb5eb217a157ab6cb538f2013b535.png

    4.根据预测值和预测的协方差矩阵再次进行UT变换,产生新的Sigma点集:

    ce11c491b1906a6aa7e39ebeff264f60.png
    这里可以对比着线性卡尔曼滤波来学习

    5.将步骤4中得到的新Sigma点集带入观测方程,得到相应的观测之后的预测量(状态更新)

    b523a27f3099b3461750d2332e599e04.png

    之后进行与上面一样的加权求和过程得到系统预测均值及协方差

    c482c31e71ae5764083b8565a5c254a4.png

    6.计算卡尔曼滤波增益矩阵K

    fca96db53125d8e4643aad7fa388db07.png

    7.最后计算系统的状态更新与协方差更新

    56d91c2013c8f512950342ecdfd7d3bd.png

    下面贴出我之前写过的UKF相关代码,在Matlab上可以直接跑。其中非线性系统的离散化我使用的为f(t+dt)-f(t),具体的不同离散化方法大家可以自行百度尝试。

    function 

    本人接触卡尔曼滤波的时间也不是很长,文章中难免会有纰漏,有错误的地方还请大家指正

    参考

    1.黄小平《卡尔曼滤波原理及应用》

    展开全文
  • 动态定位中野值的存在,使无迹卡尔曼滤波UKF(Unscented Kalman Filter)的结果不再准确甚至发散。针对这一问题,提出了一种具有抗野值性能的UKF算法。该算法将经典UKF算法与野值的剔除相结合,通过对新息序列的判断...
  • 基本理论介绍前面介绍无迹卡尔曼滤波时,介绍了对于非线性的分布比较难处理时,可以直接用蒙特卡罗近似原分布。而这一点在粒子滤波中被大量使用。对于一个概率密度函数为$p(mathbb x)$的分布,我们知道其均值和方差...
  • EKF与UKF 一引言 严格说来,所有的系统都是非线性的,其中许多还是强非线性的因此,非线性系统估计问题广泛存在于飞行器导航目标跟踪及工业控制等领域中,具有重要的理论意义和广阔的应用前景 可以说,所有的非线性估计都...
  • 一、本文简介使用之前文章(参考3)中的数据,但是对非线性函数使用了无迹卡尔曼(UKF)来滤波。二、环境语言:julia 1.5第三方包:DataFrames, CSV, LinearAlgebra, Plots, PyPlot, StatsBase, Distributions三、...
  • 扩展卡尔曼滤波主要思路:将递推公式用一阶泰勒展开近似,缺点也比较明显,某些情况下,局部线性化并不能很好的逼近真实值。对于某系统模型有: $$ mathbb x_t = g(mathbb x_{t-1}, mathbb u_t) + e_t $$ 其中,$e_t...
  • 扩展卡尔曼滤波_无迹卡尔曼滤波_扩展信息滤波_l粒子滤波算法.rar
  • 无迹卡尔曼滤波

    2017-12-18 08:56:27
    无迹卡尔曼滤波,该文件用于编写无迹卡尔曼滤波算法及其测试,主要子程序包括:轨迹发生器、系统方程,测量方程、UKF滤波器。对于新手非常有用,有注释。。。
  • 最后,通过预设的模糊逻辑函数和卡方检验值求取系统噪声估计值,得到具有系统噪声统计特性调整的自适应无迹卡尔曼滤波算法.所提出的算法可以克服低成本惯性测量单元难以准确获知先验知识的缺陷.通过SINS/GPS组合导航...
  • 在无迹卡尔曼滤波的基础上进行了改进,设计的自适应无迹卡尔曼滤波算法,适合有无迹卡尔曼滤波基础的人进行学习。
  • python实现的无迹卡尔曼滤波算法,ukf 1 ~3 的观测方程不一样,感觉观测方程是平方时 有点莫名其妙。有兴趣的朋友下载自行研究。
  • 目录标题0 引言1 无迹变换1.1 什么是无迹变换1.2 一般形式的无迹变换1.3 比例无迹变换2 无迹卡尔曼滤波的假设2.1 与卡尔曼滤波相同的假设2.2 与卡尔曼滤波不同的假设3 无迹卡尔曼滤波算法框架3.1 可加性噪声条件下的...
  • 针对条件线性高斯状态空间模型, 提出cubature 卡尔曼滤波-卡尔曼滤波算法(CKF-KF), 分别应用CKF 和KF 估计模型中的非线性和线性状态. 该算法对非线性与线性状态均进行cubature 采样, 并将两种样本通过线性方程和量测...
  • 无迹卡尔曼滤波(UKF)

    千次阅读 2020-01-07 10:55:01
    无迹卡尔曼滤波算法用于解决系统方程非线性的情况。无迹卡尔曼滤波算法采用无迹变换对状态值进行采样,采样2n+1个sigma点用于计算均值与协方差。相比于EKF对系统方程线性化,无迹卡尔曼滤波近似程度更高。 系统方程...

空空如也

空空如也

1 2 3 4 5 6
收藏数 114
精华内容 45
关键字:

无迹卡尔曼滤波算法