卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战
原标题:卡尔曼滤波器算法浅析及matlab实战
作者:Liu_LongPo
出处:Liu_LongPo的博客
卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响,所以最优估计也可看做是滤波过程。
卡尔曼滤波器的核心内容就是5条公式,计算简单快速,适合用于少量数据的预测和估计。
下面我们用一个例子来说明一下卡尔曼算法的应用。
假设我们想在有一辆小车,在 t 时刻其速度为 Vt ,位置坐标为 Pt,ut 表示 t 时刻的加速度,那么我们可以用Xt表示 t 时刻的状态,如下:
则我们可以得到,由t-1 时刻到 t 时刻,位置以及速度的转换如下:
用向量表示上述转换过程,如下:
如下图:
那么我们可以得到如下的状态转移公式:
(1)
其中矩阵 F 为状态转移矩阵,表示如何从上一状态来推测当前时刻的状态,B 为控制矩阵,表示控制量u如何作用于当前矩阵,上面的公式 x 有顶帽子,表示只是估计值,并不是最优的。
有了状态转移公式就可以用来推测当前的状态,但是所有的推测都是包含噪声的,噪声越大,不确定越大,协方差矩阵用来表示这次推测带来的不确定性
协方差矩阵
假设我们有一个一维的数据,这个数据每次测量都不同,我们假设服从高斯分布,那么我们可以用均值和方差来表示该数据集,我们将该一维数据集投影到坐标轴上,如下图:
可以看到,服从高斯分布的一维数据大部分分布在均值附近。
现在我们来看看服从高斯分布的二维数据投影到坐标轴的情况,如下图:
二维数据比一维数据稍微复杂一点,投影后有3种情况,分别是:
左图:两个维的数据互不相关;
中图:两个维的数据正相关,也就是 y 随着 x 的增大而增大(假设两个维分别为 x 和 y)
右图:两个维的数据负相关,也就是 y 随着 x 的增大而减小。
那怎么来表示两个维的数据的相关性呢?答案就是协方差矩阵。
状态协方差矩阵传递
在公式(1)之中,我们已经得到了状态的转移公式,但是由上面可知,二维数据的协方差矩阵对于描述数据的特征是很重要的,那么我们应该如何更新或者说传递我们的二维数据的协方差矩阵呢?假如我们用 P 来表示状态协方差,即
那么加入状态转换矩阵 F ,得到
(2)
也即:
因此我们便得到了协方差的转换公式。
现在我们得到了两个公式,运用这两个公式能够对现在状态进行预测。按照我们的正常思路来理解,预测结果不一定会对嘛,肯定有误差。而且在我们大多数回归算法或者是拟合算法中,一般思路都是先预测,然后看看这个预测结果跟实际结果的误差有多大,再根据这个误差来调整预测函数的参数,不断迭代调整参数直到预测误差小于一定的阈值。
卡尔曼算法的迭代思想也类似,不过这里根据误差调整的是状态 X 。
在这里,我们的实际数据就是 Z, 如下图:
其中,矩阵 H 为测量系统的参数,即观察矩阵,v 为观测噪声, 其协方差矩阵为R
那么我们的状态更新公式如下:
其中K 为卡尔曼系数, Z-Hx 则为残差,也就是我们说的,预测值与实际值的误差。
K的作用:
1.K 权衡预测协方差P和观察协方差矩阵R那个更加重要,相信预测,残差的权重小,相信观察,残差权重大,由 K 的表达是可以退出这个结论
2,将残差的表现形式从观察域转换到状态域(残差与一个标量,通过K 转换为向量),由 状态 X 的更新公式可得到该结论。
至此,我们已经得到了 t 状态下的最优估计值 Xt。但为了能让我们的迭代算法持续下去,我们还必须更新状态协方差的值。
状态协方差的更新
以上就是卡尔曼滤波算法的思想,只有简单的 5 条公式,总结如下:
Matlab 实现functionkalmanFiltering%%clcclose all%%%%Deion:kalmanFiltering%Author:Liulongpo%Time:2015-4-2916:42:34%%%Z=(1:2:200);%观测值汽车的位置也就是我们要修改的量noise=randn(1,100);%方差为1的高斯噪声Z=Z+noise;X=[0;0];%初始状态P=[10;01];%状态协方差矩阵F=[11;01];%状态转移矩阵Q=[0.0001,0;0,0.0001];%状态转移协方差矩阵H=[1,0];%观测矩阵R=1;%观测噪声方差figure;hold on;fori=1:100%基于上一状态预测当前状态X_=F*X;%更新协方差Q系统过程的协方差这两个公式是对系统的预测P_=F*P*F'+Q;% 计算卡尔曼增益K = P_*H'/(H*P_*H'+R);% 得到当前状态的最优化估算值 增益乘以残差X = X_+K*(Z(i)-H*X_);%更新K状态的协方差P = (eye(2)-K*H)*P_;scatter(X(1), X(2),4); %画点,横轴表示位置,纵轴表示速度endend效果如下
其中 x 轴为位置,y轴为速度。
在代码中,我们设定x的变化是 1:2:200,则速度就是2,可以由上图看到,值经过几次迭代,速度就基本上在 2 附近摆动,摆动的原因是我们加入了噪声。
接下来来看一个实际例子。
我们的数据为 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
这是运用光流法从视频中获取角点的实际x轴坐标,总共有6个数据,也就是代表了一个点的连续6帧的x轴坐标。接下来这个例子,我们将实现用5帧的数据进行训练,然后预测出第6帧的x轴坐标。
在上一个matlab例子中,我们的训练数据比较多,因此我们的初始状态设置为[0,0],也就是位置为0,速度为0,在训练数据比较多的情况下,初始化数据为0并没有关系,因为我们在上面的效果图中可以看到,算法的经过短暂的迭代就能够发挥作用。
但在这里,我们的训练数据只有5帧,所以为了加快训练,我们将位置状态初始化为第一帧的位置,速度初始化为第二帧与第一帧之差。
代码如下:
KF.m
function[predData,dataX]=KF(dataZ)%%%%Deion:kalmanFiltering%Author:Liulongpo%Time:2015-4-2916:42:34%%%Z=dataZ';len = length(Z);%Z=(1:2:200); %观测值 汽车的位置 也就是我们要修改的量noise=randn(1,len); %方差为1的高斯噪声dataX = zeros(2,len);Z=Z+noise;X=[Z(1) ; Z(2)-Z(1) ]; %初始状态 分别为 位置 和速度P=[1 0;0 1]; %状态协方差矩阵F=[1 1;0 1]; %状态转移矩阵Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵H=[1,0]; %观测矩阵R=1; %观测噪声方差%figure;%hold on;for i = 1:len%基于上一状态预测当前状态 % 2x1 2x1X_ = F*X;% 更新协方差 Q系统过程的协方差 这两个公式是对系统的预测% 2x1 2x1 1x2 2x2P_ = F*P*F'+Q;%计算卡尔曼增益K=P_*H'/(H*P_*H'+R);%得到当前状态的最优化估算值增益乘以残差X=X_+K*(Z(i)-H*X_);%更新K状态的协方差P=(eye(2)-K*H)*P_;dataX(:,i)=[X(1);X(2)];%scatter(X(1),X(2),4);%画点,横轴表示位置,纵轴表示速度endpredData=F*X;end
testKF.m
functiontestKF%%clcclose all%%%data=load('D:a.txt');%data=[149.360,150.06,151.44,152.81,154.19,157.72,157.47,159.33,153.66];data=[149.360,150.06,151.44,152.81,154.19,157.72];[predData,DataX]=KF(data');error = DataX(1,:) - data;i = 1:length(data);figuresubplot 311scatter(i,data,3),title('原始数据')subplot 312scatter(i,DataX(1,:),3),title('预测数据')subplot 313scatter(i,error,3),title('预测误差')predData(1)%{scatter(i,error,3);figurescatter(i,data,3)figurescatter(i,predData(1,:),3)%}end效果如下:
预测结果为: 155.7493 ,跟实际结果 157.72 仅有1.9 的误差,可以看到,卡尔曼滤波器算法对于少量数据的预测效果还是挺不错的。当然,预测位置的同时,我们也得到了预测速度
责任编辑:
卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战相关推荐
- 图片缩放 算法 matlab,图像放大算法总结及MATLAB源程序.doc
图像放大算法总结及MATLAB源程序 1,插值算法(3种): (1)最邻近插值(近邻取样法): 最近插值的的思想很简单就是把这个非整数坐标作一个四舍五入,取最近的整数点坐标处的点的颜色.可见,最邻近插 ...
- kmeans聚类算法matlab代码,K-Means算法实现(Matlab)
K-Means算法具体内容可以参考我博客的相关文章,这里只使用Matlab对其进行实现,其他内容不多赘述 K-Means算法 1.生成随机样本点 首先利用 mvnrnd 函数生成3组满足高斯分布的数据 ...
- matlab实现神经网络算法,人工神经网络matlab代码
求一段神经网络MATLAB代码 50 function [presim ss net] = simnonlin( y,d,n )% y-- 时间序列数据,列向量% d-- 时间延迟参数,正整数% n- ...
- matlab去雾算法论文,基于matlab的图像去雾算法详细讲解与实现-附matlab实现源代码.doc...
本文主要介绍基于Retinex理论的雾霭天气图像增强及其实现.并通过编写两个程序来实现图像的去雾功能. 1 Rentinex理论 Retinex(视网膜Retina"和大脑皮层Cortex& ...
- 怎么用matlab编写quad8算法,quad8 是 matlab 中调用( )公式的命令
quad8 是 matlab 中调用( )公式的命令 更多相关问题 [判断题]对于糖尿病患者来说,心脏病.卒中和猝死的风险减少一半. [单选题]孔子曰:人皆可为尧舜.只要我们从小形成良好的生活态度.人 ...
- matlab ocr识别算法,ocr识别MATLAB
[实例简介] 可以运行,主要是MATLAB.识别率不是很高.可以修改符合自己的代码 [实例截图] [核心代码] OCR(1) ├── 107215817chess_Matlab.zip ├── 778 ...
- dijkstra的matlab程序,最短路径算法dijkstra的matlab程序,让大家来找茬,交流
%初始化 MAXNUM=5; MAXINT=32767; dij=MAXINT*ones(MAXNUM,MAXNUM); dij(1,2)=10; dij(1,4)=30; dij(1,5)=100; ...
- 用matlab实现理查森外推算法,Matlab数值积分(2)
实验目的: 掌握理查森外推法 实验要求: 1. 给出理查森外推算法 2. 用Matlab实现理查森外推算法 3. 用Matlab实现自适应积分算法 实验内容: 1. 理查森外推算法,数学知识:利用Ri ...
- 数字水印算法matlab源程序 matlab版数字水印算法 /DCT/DWT/LSB/HVS/W-SVD数字水印源码 数字水印的嵌入和提取 W-SVD数字水印实现
发以下多套系统源码: 1.matlab版数字水印算法 2.MATLAB数字水印 源代码+文档 3.数字水印技术matlab代码 4.数字水印 JPEG压缩 matlab代码 5.数字水印 添加噪声 m ...
最新文章
- oracle 主键_Oracle约束详解
- ps 命令查看进程状态
- python脚本第一行_python脚本第一行如何写
- 开发监控云组态软件的组成
- (转)比较全的OA模板
- 基于Linux系统开发在线词典
- IE8卸载再回到IE6
- 基于jsp的学生培训管理系统
- 俄亥俄州立大学计算机专业排名,俄亥俄州立大学计算机科学专业
- python安装后使用pip报错解决方法
- 游戏手柄(JoyStick)的延时处理
- JWS webservice 实验
- 如何理解keras中的shape/input_shape
- waterfall model
- [玩转UE4/UE5动画系统>Control Rig篇] 之 Control Rig + Fullbody IK版的足部IK实现(附项目代码)
- 清明节海报设计软件测试,PS清明节海报设计教程
- NPOI Word 编程学习总结
- yourls短链接项目部署及API使用
- 利用/proc/mounts检查已经被系统挂载的设备
- Oracle学习之基础
热门文章
- android进程自动启动时间,如何统计Android App启动时间
- php中的类 对象的方法的区别,php中类和对象的区别是什么
- tinyint对应什么数据类型_学习西门子S7-200系列PLC不得不掌握的数据类型
- 计算机视觉:图像分类定位(单一目标检测)python实现
- 二分查找 寻找指定数 python 实现
- JavaScript实现depth First Search深度优先搜索算法(附完整源码)
- JavaScript实现hornerMethod霍纳法算法(附完整源码)
- wxWidgets:wxMemoryFSHandler类用法
- boost::sort模块实现spreadsort wstring 排序示例
- boost::graph模块实现bellman算法的测试程序