实例介绍
【实例截图】
【核心代码】
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Kalman滤波在船舶GPS导航定位系统中的应用 % 详细原理介绍及中文注释请参考: % 《卡尔曼滤波原理及应用-MATLAB仿真》,电子工业出版社,黄小平著。 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function Kalman clc;clear; T=1; N=80/T; X=zeros(4,N); X(:,1)=[-100,2,200,20]; Z=zeros(2,N); Z(:,1)=[X(1,1),X(3,1)]; delta_w=1e-2; Q=delta_w*diag([0.5,1,0.5,1]) ; R=100*eye(2); F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; H=[1,0,0,0;0,0,1,0]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for t=2:N X(:,t)=F*X(:,t-1) sqrtm(Q)*randn(4,1); Z(:,t)=H*X(:,t) sqrtm(R)*randn(2,1); end Xkf=zeros(4,N); Xkf(:,1)=X(:,1); P0=eye(4); for i=2:N Xn=F*Xkf(:,i-1); P1=F*P0*F' Q; K=P1*H'*inv(H*P1*H' R); Xkf(:,i)=Xn K*(Z(:,i)-H*Xn); P0=(eye(4)-K*H)*P1; end for i=1:N Err_Observation(i)=RMS(X(:,i),Z(:,i)); Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i)); end figure hold on;box on; plot(X(1,:),X(3,:),'-k'); plot(Z(1,:),Z(2,:),'-b.'); plot(Xkf(1,:),Xkf(3,:),'-r '); legend('真实轨迹','观测轨迹','滤波轨迹') figure hold on; box on; plot(Err_Observation,'-ko','MarkerFace','g') plot(Err_KalmanFilter,'-ks','MarkerFace','r') legend('滤波前误差','滤波后误差') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function dist=RMS(X1,X2); if length(X2)<=2 dist=sqrt( (X1(1)-X2(1))^2 (X1(3)-X2(2))^2 ); else dist=sqrt( (X1(1)-X2(1))^2 (X1(3)-X2(3))^2 ); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
标签: 卡尔曼目标跟踪
小贴士
感谢您为本站写下的评论,您的评论对其它用户来说具有重要的参考价值,所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字,对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙,所以请不要反馈意义不大的重复字符,也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明,或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦,又没人会搭理,于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享,对搜索内容的合法性不具有预见性、识别性、控制性,仅供学习研究,请务必在下载后24小时内给予删除,不得用于其他任何用途,否则后果自负。基于互联网的特殊性,平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查;无论平台是否已进行审查,用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场,基于网友分享,根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定,若资源存在侵权或相关问题请联系本站客服人员,点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论