-
扩展卡尔曼滤波算法.pdf
2020-08-20 02:36:13扩展卡尔曼滤波算法 作者 niewei120 nuaa EKF 算法是在标准 Kalman 滤波算法的基础上发展起来的 它的基本思想是 在滤波值附近 应用泰勒展开算法将非线性系统展开 对于二阶以上的高阶项全部都省去 从而原系统就变 成... -
扩展卡尔曼滤波算法ckf.m
2020-03-31 12:59:54扩展卡尔曼滤波算法的matlab实例,将扩展卡尔曼滤波简介明了的用代码表示,有助于读者了解该算法,对算法有深一步的认识 -
扩展卡尔曼滤波算法的matlab程序.pdf
2019-11-28 20:56:54扩展卡尔曼滤波算法的matlab程序 -
改进的扩展卡尔曼滤波算法研究_贺军义.pdf
2019-09-01 14:48:29改进的扩展卡尔曼滤波算法研究,来源于一位老师对卡尔曼滤波的介绍,以及对此的改进算法的介绍。 -
扩展卡尔曼滤波算法_地理坐标系到载体坐标系转换
2020-07-02 06:53:19扩展卡尔曼滤波算法 作者niewei120nuaa EKF 算法是在标准 Kalman 滤波算法的基础上发展起来的它的基本思想是在滤波值附近应用泰勒展开算法将非线性系统展开对于二阶以上的高阶项全部都省去从而原系统就变成了一个... -
TDOA_AOA定位的扩展卡尔曼滤波算法MATLAB源代码
2018-09-22 06:51:32TDOA_AOA定位的扩展卡尔曼滤波算法MATLAB源代码 -
卡尔曼滤波算法和扩展卡尔曼滤波算法
2009-04-15 13:57:05很有用的卡尔曼滤波算法实例和 扩展卡尔曼滤波算法实例 -
卡尔曼_扩展卡尔曼滤波算法实例解析
2021-01-12 02:44:51本文将以车辆三自由度模型为基础,利用扩展卡尔曼滤波,通过车辆的侧向加速度来估计横摆角速度、质心侧偏角、纵向速度等...2)扩展卡尔曼滤波算法搭建;3)模型整合及仿真工况设置;4)仿真及结果分析。车辆模型搭建本...本文将以车辆三自由度模型为基础,利用扩展卡尔曼滤波,通过车辆的侧向加速度来估计横摆角速度、质心侧偏角、纵向速度等三个参数,通过一个实际的仿真案例来进行具体介绍扩展卡尔曼滤波的使用。
一般地,卡尔曼滤波会选择比较容易获取的参数,来估计不易测量的参数。
在这里,脚主把卡尔曼参数估计仿真分为四个步骤:
1)车辆模型搭建;
2)扩展卡尔曼滤波算法搭建;
3)模型整合及仿真工况设置;
4)仿真及结果分析。
车辆模型搭建
本例中,利用车辆三自由度模型(如下图)进行参数估计,需要知道车辆的输入信号(车轮转角、纵向加速度)和输出信号(侧向加速度),所以需要自己搭建一个车辆模型来创造这些数据。即对车辆模型输入一个方向盘转角和纵向加车速,得到侧向加速度。
在实车上,这一步是可以忽略的,因为我们可以通过传感器直接测量卡尔曼滤波所需的信号。
脚主暂时选择比较简便的方法,借助carsim中的车辆模型来完成这项工作,仅需要设置好我们关注的车辆基本参数及信号接口即可。
质心到前后轴距离、沿Z轴转动惯量、质量设置位置如下图。
前后轮侧偏刚度设置位置如下图。
方向盘转角到车轮转角的传动比设置位置如下图。
输出接口选择输出横摆角速度、质心侧偏角、纵向车速、方向盘转角、纵向加速度、侧向加速度。前三个是待估计的参数,用于与仿真结果对比;后三个是车辆的输入输出信号,会作为卡尔曼滤波算法的输入。
一直用别人的车辆模型也不合适,后面脚主会自己动手搭建车辆模型,这样就可以避免联合仿真的麻烦,仿真可以全部在simulink中实现了。更重要的是自己搭建车辆模型更加能加深对车辆的理解,这个是商业软件所无法替代的。
扩展卡尔曼滤波算法搭建
扩展卡尔曼滤波算法就是把上文提到的5个核心公式表达出来。再次强调一下:需要使用非线性函数f、h来表示状态方程和输出方程;系统矩阵A、输出矩阵H需要用f、h函数求偏导后的雅克比矩阵表示。
脚主这里借助以前搭建的卡尔曼滤波算法,稍微改动一下,得到如下图的扩展卡尔曼滤波算法。
用5个function表达卡尔曼滤波算法其实比较繁琐,但是可以更好地表达5个公式之间的时序关系,便于初学者理解。
模型整合及仿真工况设置
把上述两部分内容组合起来就是整个基于扩展卡尔曼滤波的参数估计仿真模型,如下图。基本思路就是,carsim模型输出滤波算法所需的信号,然后进行参数估计,输出估计的结果,最后将估计结果与车辆实际信号对比,来验证算法的有效性。
为了验证算法,还需要在carsim中设置一下仿真工况。
1)初始车速为30km/h进行滑行。
2)方向盘转角按下图的曲线执行。
仿真及结果分析
运行模型,得到估计的横摆角速度、质心侧偏角、纵向车速,与车辆实际的状态对比如下图。
横摆角速度估计结果:
质心侧偏角估计结果:
纵向车速估计结果:
从图中可以看出,横摆角速度、质心侧偏角、纵向车速的估计值与实际值基本一致,算法可靠有效。
以上,介绍了扩展卡尔曼滤波算法进行参数估计的一个实例,仅供大家参考。
-
扩展卡尔曼滤波算法测试的目标追踪-雷达-激光雷达数据
2018-10-10 15:40:45借鉴的大神的用于扩展卡尔曼滤波算法测试的目标追踪-雷达-激光雷达数据,详情请见博客:http://blog.csdn.net/adamshan/article/details/78265754 -
物体的运动轨迹预测扩展卡尔曼滤波算法实现
2018-08-14 20:41:00运动物体的轨迹预测,分别使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及数据拟合方法实现。本例代码仅含扩展卡尔曼滤波部分代码。 本例仅为本人在研究轨迹预测问题时为理解算法原理所写,针对具体问题请自行... -
强跟踪有限微分扩展卡尔曼滤波算法在异步电机中的应用
2020-06-20 10:22:28针对异步电机直接转矩控制低速转矩脉动大的问题,提出了一种自适应强跟踪有限微分扩展卡尔曼滤波算法(STFDEKF)。该算法采用多项式近似技术和一阶中心差分法计算非线性函数的偏导数,它具有二阶非线性近似的能力;同时... -
始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别?
2019-12-27 10:52:23原始卡尔曼滤波算法(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!
把它们结合在一起
截至目前,我们有用矩阵
预测的分布,有用传感器读数
预测的分布。把它们代入上节的矩阵等式中:
相应的,卡尔曼增益就是:
考虑到
里还包含着一个
,我们再精简一下上式:
最后,
是我们的最佳估计值,我们可以把它继续放进去做另一轮预测:
希望这篇文章能对你有用!
-
python卡尔曼滤波室内定位_基于扩展卡尔曼滤波算法的室内定位跟踪系统
2020-12-29 15:59:46基于扩展卡尔曼滤波算法的室内定位跟踪系统凌海波,周先存【摘要】摘要:为了解决无线室内定位系统实时跟踪位置坐标误差较大问题,提出一种基于扩展卡尔曼滤波(EKF)算法的室内定位方法。系统采用基于WiFi信号指纹...基于扩展卡尔曼滤波算法的室内定位跟踪系统
凌海波,周先存
【摘
要】
摘要:为了解决无线室内定位系统实时跟踪位置坐标误差较大问题,
提出一种基于扩展卡尔曼滤波
(EKF)
算法的室内定位方法。系统采用基于
WiFi
信号指纹定位,然后利用扩展卡尔曼滤波对估算的位置进行滤波,以改善
WiFi
指纹定位方法的精度,达到对目标实时跟踪。仿真和实验结果表明,该算法有
效地改善了系统定位精度,能较好地满足室内定位的需求。
【期刊名称】
河北工程大学学报(自然科学版)
【年
(
卷
),
期】
2016(033)002
【总页数】
5
【关键词】
无线局域网;室内定位;扩展卡尔曼滤波;实时跟踪
无线室内定位技术已经被用于很多基于位置的应用,随着智能手机的普及,其
应用越来越广泛。现有最新理论成果仍然面临很多挑战,
WiFi
信号会受到多径
效应和环境动态变化的影响,在无线局域网络中对目标进行监测并跟踪,由于
在室内复杂环境中,目标的状态会随着时间的变化而变化,并且传感器获取的
数据受到室内环境的影响,此时直接用定位算法计算的位置和实际的位置会有
很大的误差,从而影响了室内定位系统的精度
[1-3]
。跟踪定位系统应以最小的
计算复杂度获得较高的定位精度,文献
[4-6]
提出了利用卡尔曼滤波、扩展卡尔
曼滤波、粒子滤波等算法实现跟踪,其中扩展卡尔曼滤波算法收敛速度快,在
实时跟踪定位中有着很广泛的应用。本文采用扩展卡尔曼滤波算法随定位算法
估算的位置坐标进行滤波处理,进一步提高室内定位的精度。
1
基于
WiFi
信号指纹定位算法描述
-
基于扩展卡尔曼滤波算法的室内定位跟踪系统
2020-06-17 07:20:06为了解决无线室内定位系统实时跟踪位置坐标误差较大问题,提出一种基于扩展卡尔曼滤波(EKF)算法的室内定位方法。系统采用基于WiFi信号指纹定位,然后利用扩展卡尔曼滤波对估算的位置进行滤波,以改善WiFi指纹定位方法的... -
抑制多径的BD2/GPS双模自适应扩展卡尔曼滤波算法
2020-10-16 08:29:35针对这一问题提出了一种抑制多径的BD2/GPS双模自适应扩展卡尔曼滤波算法。通过观测误差协方差估计和粗差检测来调整卫星参与定位的受信任程度和个数,分析了多径效应对伪距残差和多普勒残差的影响,同时对比了原始EKF... -
4 概率机器人 Probabilistic Robotics 扩展卡尔曼滤波算法
2020-10-03 19:44:59文章目录1 前提介绍2 通过泰勒展式进行线性化3 扩展卡尔曼滤波算法(EKF)4 扩展卡尔曼滤波实例5 扩展卡尔曼滤波(EKF)公式推导5.1 预测公式推导5.2 测量更新公式推导6 扩展卡尔曼滤波的优缺点7 参考文献 ...文章目录
1 前提介绍
- 扩展卡尔曼滤波顾名思义是卡尔曼滤波的扩展形式(卡尔曼滤波教程),但是在哪些方面进行扩展呢?这就要从卡尔曼滤波的三个前提条件入手:
- 状态转移函数必须是线性函数:
- 测量函数必须是线性函数:
- 初始的置信度函数服从正态分布:
- 扩展卡尔曼滤波对第个前提条件进行扩展:
- 状态转移函数不必是线性函数
- 测量函数必须不必是线性函数:
- 若服从正态分布,那么变换后的变量服从怎样的分布?
-
- 线性变换,那么变量也服从正态分布
-
- 非线性变换,那么变量将不再服从正态分布,且解析形式很难求得
- 扩展卡尔曼滤波与卡尔曼滤波状态转移函数和测量函数对比:
- 用函数代替矩阵
- 用函数代替矩阵
- 对于任意函数将会导致置信度不再服从正态分布,那么置信度更新将不再存在封闭解,换句话说贝叶斯滤波将不能进行
- 那么为了克服上述问题,只能退而求其次,采用将上述两个非线性函数进行线性化
2 通过泰勒展式进行线性化
- 泰勒展开式与线性化:
- 如果服从正态分布,是非线性函数,那么为了线性化,应该在什么位置对进行一阶泰勒展开呢?
- 答案是应该在的均值出进行展开
- 这是因为等于均值的概率最大
- 变换如下图:
- 右下角为服从正态分布,等于均值的概率最大(峰值)
- 右上角为对进行非线性变换,虚线是在处的一阶泰勒展开式,用来近似实线部分
- 左上角虚线为通过线性化,求得的的概率分布;实线为实际的概率分布,可见误差在允许的范围内
- 线性化状态转移函数
- 对于高斯分布,后验概率是状态变量最可能的状态
- 所以对在处展开(即)
- 线性化测量函数:
- 对于高斯分布,是状态变量最可能的状态
- 所以对在处展开(即)
3 扩展卡尔曼滤波算法(EKF)
- 通过上一部分对状态转移函数和观测函数的线性化,使得这两个函数用两个线性函数表示,使得置信度还服从正态分布
- 这里先给出扩展卡尔曼滤波算法,并与卡尔曼滤波进行对比,具体公式推导见下面
-
- 扩展卡尔曼滤波算法
-
- 卡尔曼滤波算法
- 将扩展卡尔曼滤波与卡尔曼滤波进行一下对比:
- 雅可比矩阵代替
- 雅可比矩阵代替
4 扩展卡尔曼滤波实例
- 这里采用滤波算法 “扩展卡尔曼滤波(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 %% 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_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_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_est(:,1),X_est(:,3), 'r'); %最优位置 xlabel('X'); ylabel('Y'); title('EKF simulation'); legend('real', 'measurement', 'ekf estimated'); axis([-5,230,290,530]);
5 扩展卡尔曼滤波(EKF)公式推导
- 这里的公式推导用卡尔曼滤波公式进行类比,详细过程可以结合卡尔曼滤波公式的推导过程
5.1 预测公式推导
- 预测公式:
- 不同之处对比:
- 那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比
5.2 测量更新公式推导
- 测量更新公式:
- 不同之处对比:
- 那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比
6 扩展卡尔曼滤波的优缺点
- 优点:计算简单效率高
- 缺点:对状态转移函数和测量函数的线性近似,使得结果存在误差
-
- 原函数不确定度(方差)越大,近似效果越差,见下图:
- 右下角的分布均值相同,方差不同
- 右上角变换函数相同
- 左上角的分布,下图误差小于上图,原因在于下图的方差较小
- 原函数不确定度(方差)越大,近似效果越差,见下图:
-
- 原函数局部非线性越强,近似效果越差,见下图:
- 右下角的分布均值不同,方差相同
- 右上角变换函数相同
- 左上角的分布,下图误差小于上图,原因在于下图变换函数的局部非线性越弱
- 原函数局部非线性越强,近似效果越差,见下图:
- 总结:状态变量的不确定度(方差)对EKF影响很大,尽量保持一个小的不确定度
7 参考文献
- Thrun, & Sebastian. (2005). Probabilistic robotics.
- 滤波算法 “扩展卡尔曼滤波(EFK)实例” 讲解
- 扩展卡尔曼滤波顾名思义是卡尔曼滤波的扩展形式(卡尔曼滤波教程),但是在哪些方面进行扩展呢?这就要从卡尔曼滤波的三个前提条件入手:
-
卡尔曼滤波及扩展卡尔曼滤波算法
2011-04-14 23:03:03卡尔曼滤波算法及扩展卡尔曼滤波在信号估计中的应用 -
扩展卡尔曼滤波算法
2019-08-15 00:10:02输入过程: [y1,y2,...,yn][y_1,y_2,...,y_n][y1,y2,...,yn] 已知参数: 非线性状态向量函数=an(xn)a_n(x_n)an(xn) 非线性测量向量函数=bn(xn)b_n(x_n)bn(xn) 过程噪声向量的协方差矩阵=Qw,nQ_{w,n}Qw... -
扩展卡尔曼滤波算法及仿真实例
2018-08-17 20:43:24在阅读本篇博客之前希望读者已经具备线性卡尔曼滤波器的基础,或者提前研读我的前一篇关于线性卡尔曼滤波器的文章:线性卡尔曼滤波算法及示例。下面不说废话,直奔主题了。 一、扩展卡尔曼滤波器(EKF)理论基础 ... -
1iekf 一维的迭代扩展卡尔曼滤波算法
2009-05-09 14:45:101iekf 一维的迭代扩展卡尔曼滤波算法 比较简单的算法 -
扩展卡尔曼滤波_无迹卡尔曼滤波_扩展信息滤波_l粒子滤波算法.rar
2020-11-03 15:36:30扩展卡尔曼滤波_无迹卡尔曼滤波_扩展信息滤波_l粒子滤波算法.rar -
卡尔曼滤波系列——(二)扩展卡尔曼滤波
2019-04-06 16:33:48扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用... -
「滤波算法」“EKF扩展卡尔曼滤波算法”研究
2019-03-30 19:02:481.卡尔曼与扩展卡尔曼公式的汇总 各有5个公式,以卡尔曼为例: 第一个:根据物理模型,我们得出由上一时刻预测出本时刻的状态; 第二个:我们预测结果的协方差,既可以判断预测的准不准,又为了做公式运算使用; ... -
扩展卡尔曼滤波算法的原理于代码实现
2020-06-16 00:47:542、实现扩展卡尔曼滤波器的预备步骤 (1)阶段1 新矩阵的构建 (2) 阶段2 空间模型线性化 3、扩展卡尔曼滤波器的实现 输入过程: [y1,y2,...,yn][y_1,y_2,...,y_n][y1,y2,...,yn] 已知参数: 非线性状态向量... -
扩展卡尔曼滤波SOC算法Simulink模型
2018-11-16 10:20:05扩展卡尔曼滤波SOC算法Simulink模型,里面包括按时积分的SOC计算,包含噪音的SOC计算,和扩展卡尔曼滤波的SOC计算,输出三者的比较曲线,可以用来参考学习。 -
卡尔曼滤波算法_导航系统设计专题(五)—扩展卡尔曼滤波(2)
2020-12-08 11:01:370 前言 上一篇围绕卡尔曼滤波算法的参数选取问题展开,针对非线性对象的状态估计问题,阐述扩展卡尔曼滤波(EKF)与卡尔曼滤波的区别以及扩展卡尔曼滤波算法的核心步骤。本篇将结合实际案例进行详解,同时,提供一种... -
simulink 卡尔曼滤波_扩展卡尔曼滤波参数估计实例解析
2020-11-26 10:46:09本文将以车辆三自由度模型为基础,利用扩展卡尔曼滤波,通过车辆的侧向加速度来估计横摆角速度、质心侧偏角、纵向速度等...2)扩展卡尔曼滤波算法搭建;3)模型整合及仿真工况设置;4)仿真及结果分析。01 车辆模型... -
扩展卡尔曼滤波_导航系统设计专题(五)—扩展卡尔曼滤波(2)
2020-12-16 07:39:02前言上一篇围绕卡尔曼滤波算法的参数选取问题展开,针对非线性对象的状态估计问题,阐述扩展卡尔曼滤波(EKF)与卡尔曼滤波的区别以及扩展卡尔曼滤波算法的核心步骤。本篇将结合实际案例进行详解,同时,提供一种...
-
计算重复出现的单词和次数
-
ApacheBeam实战指南|玩转KafkaIO与Flink
-
Mysql数据库面试直通车
-
-191-4-源码
-
2021-02-25
-
Unity ILRuntime框架设计
-
WLAN中共存的802.11a / n和802.11ac客户端:优化和区分
-
codeup|问题 D: 字符串内排序
-
QT实现基于TCP连接的聊天室
-
电影记录-源码
-
Python下划线命名模式 - 小结(转载)
-
ASP.NET学习——用户增删改查(三层,数据库+源码)
-
Glasterfs 分布式网络文件系统
-
计算机网络复习(第一天)
-
项目管理工具与方法
-
计算机网络复习(第一天)-2
-
华为HCNA路由与交换eNSP实战(5)RIPv2基础配置
-
NFS 网络文件系统
-
RootCluster.github.io:组织网站-源码
-
Kubernetes下日志采集、存储与处理技术实践