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

问题描述

    考察如图1所示的问题。某一物体在重力场做自由落体运动，观测装置对其位移进行检测，在传感器受到未知的独立分布随机信号的干扰下，我们需要估计该物体的运动位移和速度。

设置初值及初始化过程

依次设置了时间序列总数，噪声模块，系统矩阵模块的初值，并进行了初始化。

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);       % 二维系统

状态方程与观测方程

状态方程设置如下：

% 物体下落，受状态方程的驱动
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);
% 误差计算
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

画图输出

• 代码部分

1 噪声图

figure
plot(W);
xlabel('采样时间/s');
ylabel('过程噪声');
figure
plot(V);
xlabel('采样时间/s');
ylabel('位置偏差/m');

2 位置误差

figure
title('messure noise')
hold on,box on;
plot(messure_err_x,'-r.');  % 测量的位移误差
plot(kalman_err_x,'-g.');   % kalman估计位置误差
legend('测量位置误差','kalman估计位置误差')
xlabel('采样时间/s');
ylabel('位置偏差/m');

3 Kalman速度偏差

figure
plot(kalman_err_v);
xlabel('采样时间/s');
ylabel('速度偏差');
title('速度偏差')

4 均方值

figure
plot(err_P(:,1));
xlabel('采样时间/s');
ylabel('位移误差均方值');
figure
plot(err_P(:,1));
xlabel('采样时间/s');
ylabel('速度误差均方值');

程序运行结果

噪声结果

位置误差

Kalman速度偏差

均方值

结论

由运行结果可以看出，测量值受到测量噪声的污染，但是Kalman滤波算法很好地降低了噪声的干扰。位置和速度的误差均方值都具有较好的收敛性，可见线性Kalman滤波对高斯噪声的处理是非常有效的。

Matlab代码：https://gitee.com/uestc-eagles/kalman-filter.git