卡尔曼滤波matlab仿真
来源:互联网 发布:阿里的大数据应用 编辑:程序博客网 时间:2024/05/27 09:48
一、温度测量
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:Kalman滤波用于温度测量的实例(一维) function main %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化参数N=120; %采样点个数A=1; B=1; H=1; Q=0.01; R=0.25; W=sqrt(Q)*randn(1,N); V=sqrt(R)*randn(1,N); CON=25; %温度真实值%分配空间X=zeros(1,N); Z=zeros(1,N); Xkf=zeros(1,N); P=zeros(1,N); X(1)=25.1; P(1)=0.01; Z(1)=24.9; Xkf(1)=Z(1); I=eye(1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %kalman过程for k=2:N X(k)=A*X(k-1)+B*W(k); Z(k)=H*X(k)+V(k); X_pre(k)=A*Xkf(k-1); P_pre(k)=A*P(k-1)*A'+Q; Kg=P_pre(k)/(H*P_pre(k)*H'+R); Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k)); P(k)=(I-Kg*H)*P_pre(k); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%误差过程Err_Messure=zeros(1,N); Err_Kalman=zeros(1,N); for k=1:N Err_Messure(k)=Z(k)-X(k); Err_Kalman(k)=Xkf(k)-X(k); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%t=1:N; figure; plot(t,CON*ones(1,N),'-b',t,X,'-r',t,Z,'g+',t,Xkf,'-k'); legend('期望值','真实值','测量值','kalman估计值'); xlabel('采样时间'); ylabel('温度'); title('Kalman Filter Simulation'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure; plot(t,Err_Messure,'-b',t,Err_Kalman,'-k'); legend('测量误差','kalman误差'); xlabel('采样时间'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%empt
二、自由落体追踪(二维x,v)
%kalman滤波用于自由落体运动目标追踪(二维,x,v)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化参数N=1000;Q=[0,0;0,0];R=0.5;w=sqrt(Q)*randn(2,N);v=sqrt(R)*randn(1,N);A=[1,1;0,1];%状态转移矩阵B=[0.5;1];%控制矩阵U=-10;%控制变量H=[1,0];%观测矩阵%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%分配空间,x,p0,z,xkfx=zeros(2,N);%物体真实状态x(:,1)=[95;1];%初始位移和速度p0=[10,0;0,1];%初始误差z=zeros(1,N);%观测值z(1)=H*x(:,1);%观测真实值,第一列的列向量,xkf=zeros(2,N);%kalman估计状态xkf(:,1)=x(:,1);%kalman估计状态初始化,第一列的列向量,err_p=zeros(N,2);err_p(1,1)=p0(1,1);err_p(1,2)=p0(2,2);I=eye(2);%2*2单位矩阵表明二维系统%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%kalman迭代过程for k=2:N; x(:,k)=A*x(:,k-1)+B*U+w(k);%模型 z(k)=H*x(:,k)+v(k);%模型 x_pre=A*xkf(:,k-1)+B*U;%① p_pre=A*p0*A'+Q;%② kg=p_pre*H'/(H*p_pre*H'+R);%③ xkf(:,k)=x_pre+kg*(z(k)-H*x_pre);%④ p0=(I-kg*H)*p_pre;%⑤ err_p(k,1)=p0(1,1);%位移误差均方值 err_p(k,2)=p0(2,2);%速度误差均方值end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%误差计算messure_err_x=zeros(1,N);kalman_err_x=zeros(1,N);kalman_err_v=zeros(1,N);for k=1:N messure_err_x(k)=z(k)-x(1,k);%位移的测量误差 kalman_err_x(k)=xkf(1,k)-x(1,k);%kalman估计位移与真实位移之间偏差 kalman_err_v(k)=xkf(2,k)-x(2,k);%kalman估计速度与真实速度之间偏差end%噪声图figure;plot(w);xlabel('采样时间');ylabel('过程噪声');figure;plot(v);xlabel('采样时间');ylabel('测量噪声');%位置误差figure;hold on,box on;plot(messure_err_x,'-r.');plot(kalman_err_x,'-g.');legend('测量位置','kalman估计位置');xlabel('采样时间');ylabel('位置偏差');%kalman速度偏差figure;plot(kalman_err_v);xlabel('采样时间');ylabel('速度偏差');%均方值figure;plot(err_p(:,1));xlabel('采样时间');ylabel('位移误差均方值');figure;plot(err_p(:,1));xlabel('采样时间');ylabel('速度误差均方值');
三、GPS导航定位
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Kalman滤波在船舶GPS导航定位系统中的应用(四维,x方向x,v ,y方向x,v; ) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function main T=1; %采样周期N=80/T; %总的采样次数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]; Q=4e-4 ;R=100*eye(2);X=zeros(4,N); X(:,1)=[100,2,200,20]; %x四维,初始位置(-100,200)水平速度2,垂直速度20 Z=zeros(2,N); Z(:,1)=[X(1,1),X(3,1)]; %z只观测到位置,没有速度Xkf=zeros(4,N); Xkf(:,1)=X(:,1); P0=eye(4); %协方差矩阵初始化,4*4单位矩阵%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for t=2:N %模型 X(:,t)=F*X(:,t-1)+sqrt(Q)*randn(4,1);%目标真实轨迹 randn(4,1)4*1随机矩阵 Z(:,t)=H*X(:,t)+sqrt(R)*randn(2,1); %观测轨迹randn(2,1)2*1随机矩阵end for i=2:N X_pre=F*Xkf(:,i-1); P_pre=F*P0*F'+Q; K=P_pre*H'/(H*P_pre*H'+R); Xkf(:,i)=X_pre+K*(Z(:,i)-H*X_pre); P0=(eye(4)-K*H)*P_pre; 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+'); %Kalman估计轨迹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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
阅读全文
0 0
- 卡尔曼滤波matlab仿真
- 卡尔曼滤波基本原理及matlab仿真
- 【滤波】卡尔曼matlab仿真代码
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.3
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.4
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.3
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.4
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-7.1
- 基于matlab仿真对卡尔曼滤波的理解
- 扩展卡尔曼滤波(EKF)仿真
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.5.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.6.1
- koa2后端热更新
- yum安装的docker的卸载
- 学习GitHub
- Leetcode--Search in Rotated Sorted Array
- 深入理解Laravel框架--手动创建laravel框架(2)
- 卡尔曼滤波matlab仿真
- PHP代码审计实战之XDcms
- 靠谱的网站专题页seo优化技巧 恰当的网站专题页面能将时效性较强的事件转化为自己网站的流量,很多站点也有针对站内的重点产品或服务制作专门的页面,采用靠谱的seo优化技巧,让专题页的效果达到最佳。
- 从零开始开发Android版2048 (十)
- 趕快升級你的裝備去買V R
- C++风格_Google特色
- 安装配置maven时,使用mvn命令,报出mvn不是内部或外部命令解决方法
- 记一次揪心的MySQL数据恢复过程
- 二级指针如何分配动态内存并赋值