Kalman 滤波用于自由落体运动目标跟踪问题

Posted 2023-01-09 west_gege

某一物体在重力场做自由落体运动、观测装置对其位移进行检测，在传感器受到未知的独立分布随机信号的干扰下，我们需要估计该物体的运动位移和速度，该系统是二维状态估计系统。
不考虑x方向上位置变化，只估计y轴上物体的运动情形，
跟踪过程整理如下:
目标运动的状态方程为
$$X（k）=\Phi X（k-1））+BU（k）+\mathrm{T}W（k）$$

观测站的测量方程为∶
$$Z(k)=HX(k)+V(k)$$

用MATLAB仿真此过程如下。

function LinerKalman2D %Kalman 滤波用于自由落体运动目标跟踪问题
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% %% %%%%%%% % %% %
%系统信息
F=[ 1,1;0,1]; %状态转移矩阵

B=[ 0.5; 1 ]; %控制量

H=[ 1,0]; %。观测矩阵

T= 1000; %仿真时间,时间序列总数

I=eye(2); %单位矩阵，二维系统

%噪声
U=-10 * ones( 1,T'); %控制量

Q=[ 0,0;0,0]; %。过程噪声方差为0，即下落过程忽略空气阻力

R= 1; %。观测噪声方差

W = sqrt (Q) * randn( 2,T); % 既然Q为0，则W=0;在此写出,方便对照理解

V= sqrt(R) * randn( 1,T);   % 测量噪声V(k)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  %%%%% % %%
%初始化
X = zeros(2,T); %物体真实状态

X(:,1)= [95;1]; %初始位移和速度

P0= [ 10,0;0,1 ]; %初始协方差

Z=zeros( 1, T); %观测量

Z(1)=H * X( :, 1 ) ; %初始观测值

Xkf=zeros( 2,T); %Kalman估计状态初始化

Xkf( : ,1)= X( :,1 ) ;

%位移和速度的均方误差
Pmse = zeros( 2,T);
Pmse( 1 ,1)= P0( 1, 1); % 位移均方误差

Pmse( 1 ,2)= P0(2,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:T
    %物体下落,受状态方程的驱动
    X( :,k )=F*X( : ,k-1 )+B* U( 1 ,k )+W(k );
    %位移传感器对目标进行观测
    Z(k)=H * X( :,k )+V(k ) ;
    %得到观测信息后开始Kalman滤波
    %% prideict
    Xpre =F * Xkf( : ,k-1 )+B*U(k); %第一步:状态预测

    Ppre=F*P0* F'+Q;                % 第二步:协方差预测
    
    %% update
    K=Ppre* H'* inv(H * Ppre * H'+R);    %第三步:计算Kalman增益
    Xkf( : ,k )= Xpre+K*(Z( k)-H * Xpre) ;%第四步:状态更新
    P0=( I-K* H)* Ppre ; % 第五步:协方差更新

    %误差均方值
    Pmse( 1,k )= P0( 1,1);
    %保存位移均方误差
    Pmse( 2,k)= P0( 2,2);
    %保存速度均方误差
end
%%%%% %%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%误差计算
messure_err_x = zeros( 1 ,T); %位移的测量误差

kalman_err_x = zeros( 1 ,T); %Kalman估计的位移与真实位移之间的偏差

kalman_err_v = zeros( 1,T); %Kalman估计的速度与真实速度之间的偏差

for k= 1 :T
    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( V);
title( 'messure noise')
figure %位移偏差图

hold on , box on ;
plot ( messure_err_x , '-r. ');%测量的位移误差
plot( kalman_err_x,'-g. '); % Kalman估计位置误差
legend('测量误差', 'Kalman估计误差')
figure % Kalman速度偏差图
plot ( kalman_err_v ) ;
title('速度偏差')
figure %。位移均方值
plot( Pmse(1,:));
title('位移误差均方值')
figure %速度均方值
plot( Pmse(2,: ) );
title('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%