卫星导航实践题Satellite-navigation-practice-questions-II（利用卡尔曼滤波器模拟目标跟踪过程）

最近学习了一些GPS卫星导航的一些知识，用matlab实现了三道实践题

比较简单，仅供参考，有问题欢迎指出

实践内容II : 利用卡尔曼滤波器模拟目标跟踪过程

• 建议时间：约3-4h
• 工具：MATLAB
• 内容

1. 模型建立：初始距离80km，速度8km/s，采样间隔0.5s，其量测模型是目标到传感器的距离，其噪声标准偏差km，该模型的状态向量为距离和他的变化率，在车辆驾驶中的速度噪声标准偏差（均为高斯白噪声），模型总观测时间为20s，并且有以下公式供参考：

 

     2. 根据模型生成真实值和测量值；

     3. 根据卡尔曼滤波计算出估计值；

• 要求
  1. 显示距离、速度的真实曲线、测量曲线和滤波曲线；
  2. 显示距离、速度的偏差曲线和RMSE曲线；
  3. 显示距离、速度的卡尔曼增益曲线和协方差曲线；

 代码：

建立模型

%建立模型
% 初始条件
initial_distance = 80; % 初始距离为 80km
initial_velocity = 8; % 速度为 8km/s
sampling_interval = 0.5; % 采样间隔为 0.5s
total_time = 20; % 总观测时间为 20s
% 状态转移矩阵
F = [1, sampling_interval; 0, 1];
% 观测矩阵
H = [1, 0];
% 初始状态向量
x_initial = [initial_distance; initial_velocity];
% 状态噪声标准差
q1_std_dev = 1; % 距离的噪声标准差（自己设定）
q2_std_dev = 0.01; % 速度的噪声标准差（自己设定）
Q = [q1_std_dev^2, 0; 0, q2_std_dev^2];
% 观测噪声标准差
r_std_dev = q1_std_dev; % 观测噪声标准差（自己设定）
R = r_std_dev^2;

 生成状态：

% 生成真实状态
num_steps = total_time / sampling_interval; % 步数
true_position = zeros(1, num_steps);
true_velocity = zeros(1, num_steps);
x_true = x_initial;
for k = 1:num_steps
    true_position(k) = x_true(1);
    true_velocity(k) = x_true(2);
    % 根据状态转移方程生成真实状态（无噪声）
    x_true = F * x_true;
end
% 添加观测噪声以模拟观测值
measurements = true_position + r_std_dev * randn(1, num_steps); % 观测值（添加观测噪声）
% 初始化估计值和估计误差协方差
x_estimate = x_initial; % 初始估计值为初始状态向量
P_estimate = eye(2); % 初始估计误差协方差为对角矩阵（假设相对较大）

% 迭代进行卡尔曼滤波
num_steps = total_time / sampling_interval; % 步数
filtered_states = zeros(2, num_steps);
for k = 1:num_steps
    % 预测步骤
    [x_predict, P_predict] = kalman_predict(F, x_estimate, P_estimate, Q);
    % 更新步骤（如果有观测值）
    if  k <= length(measurements)  % 假设有观测值
        z = measurements(k);
        [x_estimate, P_estimate, K] = kalman_update(H, x_predict, P_predict, R, z);
        % 保存卡尔曼增益和协方差矩阵的变化
        kalman_gains(:, k) = K;
        covariance_values(:, k) = diag(P_estimate);

    else
        x_estimate = x_predict;
        P_estimate = P_predict;
    end
    % 存储滤波后的状态值
    filtered_states(:, k) = x_estimate;
end

 绘图：

%绘制图像
% 绘制真实状态、测量值和滤波后的状态曲线
time_steps = (0:sampling_interval:total_time-sampling_interval);
figure;
subplot(2, 1, 1);
plot(time_steps, true_position, 'b', 'LineWidth', 2);
hold on;
plot(time_steps, measurements, 'r+', 'MarkerSize', 8);
plot(time_steps, filtered_states(1, :), 'g--', 'LineWidth', 2);
title('Distance: True vs Measurements vs Filtered');
xlabel('Time');
ylabel('Distance');
legend('True Distance', 'Measurements', 'Filtered Distance');
grid on;
subplot(2, 1, 2);
plot(time_steps, true_velocity, 'b', 'LineWidth', 2);
hold on;
plot(time_steps, filtered_states(2, :), 'g--', 'LineWidth', 2);
title('Velocity: True vs Filtered');
xlabel('Time');
ylabel('Velocity');
legend('True Velocity', 'Filtered Velocity');
grid on;
%显示距离、速度的偏差曲线和RMSE曲线
% 计算距离和速度的偏差
distance_error = true_position - filtered_states(1, :);
velocity_error = true_velocity - filtered_states(2, :);
% 计算均方根误差（RMSE）
rmse_distance = sqrt(mean(distance_error.^2));
rmse_velocity = sqrt(mean(velocity_error.^2));
% 绘制距离和速度的偏差曲线
figure;
subplot(2, 1, 1);
plot(time_steps, distance_error, 'b', 'LineWidth', 2);
title('Distance Error');
xlabel('Time');
ylabel('Error');
grid on;
subplot(2, 1, 2);
plot(time_steps, velocity_error, 'b', 'LineWidth', 2);
title('Velocity Error');
xlabel('Time');
ylabel('Error');
grid on;
% 绘制RMSE曲线
figure;
bar([rmse_distance, rmse_velocity]);
title('RMSE: Distance vs Velocity');
ylabel('RMSE');
xticklabels({'Distance', 'Velocity'});
grid on;

%显示距离、速度的卡尔曼增益曲线和协方差曲线；
% 绘制卡尔曼增益和协方差曲线
time_steps = (0:sampling_interval:total_time-sampling_interval);
figure;
subplot(2, 1, 1);
plot(time_steps, kalman_gains(1, :), 'b', 'LineWidth', 2);
hold on;
plot(time_steps, kalman_gains(2, :), 'r', 'LineWidth', 2);
title('Kalman Gains: Distance vs Velocity');
xlabel('Time');
ylabel('Kalman Gain');
legend('Distance Kalman Gain', 'Velocity Kalman Gain');
grid on;
subplot(2, 1, 2);
plot(time_steps, covariance_values(1, :), 'b', 'LineWidth', 2);
hold on;
plot(time_steps, covariance_values(2, :), 'r', 'LineWidth', 2);
title('Covariance Values: Distance vs Velocity');
xlabel('Time');
ylabel('Covariance');
legend('Distance Covariance', 'Velocity Covariance');
grid on;

结果：

 

 

 

总结一下：

1. 初始化参数→建立运行模型→建立卡尔曼滤波模型→观测、更新→作图
2. 噪声标准差不同结果不同

 GitHub - newton-fly/Satellite-navigation-practice-questions: Some practice questions on satellite navigation