某一物体在重力场做自由落体运动、观测装置对其位移进行检测,在传感器受到未知的独立分布随机信号的干扰下,我们需要估计该物体的运动位移和速度,该系统是二维状态估计系统。
不考虑x方向上位置变化,只估计y轴上物体的运动情形,
跟踪过程整理如下:
目标运动的状态方程为
X
(
k
)
=
Φ
X
(
k
−
1
))
+
B
U
(
k
)
+
T
W
(
k
)
X(k)=\Phi X(k-1))+BU(k)+\Tau W(k)
X(k)=ΦX(k−1))+BU(k)+TW(k)
观测站的测量方程为∶
Z
(
k
)
=
H
X
(
k
)
+
V
(
k
)
Z(k)=HX(k)+V(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('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%