Kalman滤波在自由落体运动目标追踪中的应用

%功能说明:Kalman滤波用于自由落体运动目标跟踪问题 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function main %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N=1000;%仿真时间，时间序列总数 %噪声 Q=[0,0;0,0];%过程方差为0，即下落过程忽略空气阻力 R=1;%观测噪声方差 W=sqrt(Q)*randn(2,N);%既然Q为0，则W=0; V=sqrt(R)*randn(1,N);%测量噪声V(k) %系统矩阵 A=[1,1;0,1];%状态转移矩阵 B=[0.5;1];%控制量 U=-1; H=[1,0];%观测矩阵 %初始化 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); err_P=zeros(N,2); err_P(1,1)=P0(1,1); err_P(1,2)=P0(2,2); I=eye(2); %二维系统 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for k=2:N %物体下落，受状态方程的驱动 X(:,k)=A*X(:,k-1)+B*U+W(k); %位移传感器对目标进行观测 Z(k)=H*X(:,k)+V(k); %Kalman滤波 X_pre=A*Xkf(:,k-1)+B*U; %状态预测 P_pre=A*P0*A'+Q; %协方差预测 Kg=P_pre*H'*inv(H*P_pre*H'+R); %计算Kalman增益 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估计的位移与真实位移之间的偏差 kalman_err_v=zeros(1,N); %Kalman估计的速度与真实速度之间的偏差 for k=1:N messure_err_x(k)=Z(k)-X(1,k); kalman_err_x(k)=Xkf(1,k)-X(1,k); kalman_err_v(k)=Xkf(2,k)-X(2,k); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %画图输出 %噪声图 figure plot(W); xlabel('采样时间/s'); ylabel('过程噪声'); figure plot(V); xlabel('采样时间/s'); ylabel('测量噪声'); %位置偏差 figure hold on,box on; plot(messure_err_x,'-r.'); %测量的位移误差 plot(messure_err_x,'-g.'); %Kalman估计位置误差 legend('测量位置','Kalman估计位置') xlabel('采样时间/s'); ylabel('位置偏差/m'); %Kalman速度偏差 figure plot(kalman_err_v); xlabel('采样时间/s'); ylabel('速度偏差'); %均方值 figure plot(err_P(:,1)); xlabel('采样时间/s'); ylabel('位移误差均方值'); figure plot(err_P(:,2)); xlabel('采样时间/s'); ylabel('速度误差均方值');