一、温度测量
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % 程序说明: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,xkf
- x=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
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
转载于https://blog.csdn.net/try_again_later/article/details/78304691