卡尔曼滤波算法-Matlab仿真

卡尔曼滤波算法-Matlab仿真

1 实验要求
设有一雷达对某飞行器进行观测，飞行器状态参数为径向距离、速度、加速度。假设飞行器相对雷达径向作匀加速直线运动，并忽略加速度扰动。有初始条件如下图。
观测信号为r， 2秒观测一次，观测噪声为零均值白噪声，方差为0.15(km)^2。
分析该运动情况，并使用Kalman滤波器对观测过程进行仿真。
2 原理
1）卡尔曼滤波算法
卡尔曼估计可分为两个过程：预测和校正。
预测也称时间更新，是使用上一状态的卡尔曼估计值，对当前状态做出预测。
校正也称滤波更新，是使用当前状态的观测值，修正预测值，以获得当前状态的卡尔曼估计值。
2）建模分析
实验所描述的场景可视为离散时间线性随机动态系统，假定所有随机变量都是Gauss的，且状态更新、过程噪声、测量噪声这三个随机过程彼此独立，则对于任意损失函数可以使用基本Kalman滤波公式进行求解。
3 仿真代码

clear
%%
% -------------------------------1.初始化-----------------------------------
Total_time = 200;  % 观测总时长
Interval_time = 2;  % 观测间隔 
N = Total_time/Interval_time;  % 观测点数

X_init = [0 0 0.2]';  % 初始状态值
P_kalm = diag([8 10 5]);  % 初始误差值
V = 0.15;  % 观测噪声

F = [1 2 2;
     0 1 2;
     0 0 1];  % 状态转移矩阵-F
H =[1 0 0];  % 观测矩阵-H

 %%
 % ----------------------------2.定义3条轨迹--------------------------------
 X_true_traj = zeros(3,N);  % 真实轨迹
 X_true_traj(:,1) = X_init;
 X_true_traj(3,:) = 0.2;
 
 Z_meas_traj = zeros(1,N);  % 观测轨迹
 Z_meas_traj(:,1) = 0;
 
 X_pred_traj = zeros(3,N);  % 预测轨迹
 X_pred_traj(:,1) = X_init;
 
 X_kalm_traj = zeros(3,N);  % 滤波轨迹
 X_kalm_traj(:,1) = X_init;

 %% 
 % ----------------------3.离散状态方程和观测方程的迭代----------------------
for i = 2 : N
    X_true_traj(:,i) = F * X_true_traj(:,i-1);  % 状态方程
    Z_meas_traj(i) = H * X_true_traj(:,i) + sqrt(V) * randn;  % 观测方程
end

 %%
 % -------------------------4.进行卡尔曼滤波的迭代--------------------------
for i = 2 : N  % 每两秒进行一次观测
    %时间更新
    X_pred_traj(:,i) = F * X_kalm_traj(:,i-1);  % 状态预测值
    P_pred = F * P_kalm * F';  % 预测值方差
    
    %量测更新值  
    K = P_pred * H' * inv(H * P_pred * H'+ V);  % 卡尔曼增益
    X_kalm_traj(:,i) = X_pred_traj(:,i) + K * (Z_meas_traj(:,i) - H * X_pred_traj(:,i));  % 状态滤波值 
    P_kalm = (eye(3)-K * H) * P_pred;  % 滤波值方差  
end

%%
% ------------------------------5.绘制轨迹---------------------------------
% 距离变化轨迹图
figure(1);
plot((1:N),X_true_traj(1,:),'-black',(1:N),Z_meas_traj(1,:),'ob',(1:N),X_pred_traj(1,:),'+r',(1:N),X_kalm_traj(1,:),'xg')
title('距离变化轨迹');
xlabel('时间t/s');
ylabel('距离r/km');
legend('真实轨迹','观测轨迹','预测轨迹','滤波轨迹');
hold on;

% 速度变化轨迹
figure(2);
plot((1:N),X_true_traj(2,:),'-black',(1:N),X_pred_traj(2,:),'+r',(1:N),X_kalm_traj(2,:),'xg')
title('速度变化轨迹');
xlabel('时间t/s');
ylabel('速度v/(km/s)');
legend('真实轨迹','预测轨迹','滤波轨迹');
hold on;

% 误差变化轨迹图
figure(3)
plot((1:N),abs(X_true_traj(1,:)-X_kalm_traj(1,:)),'b',(1:N),abs(X_true_traj(1,:)-Z_meas_traj(1,:)),'r');
title('误差变化轨迹')
xlabel('时间t/s')
ylabel('误差值/km ')
ylim([-10 10]);
legend('滤波值误差', '观测值误差');
hold off;