粒子滤波跟踪系统 - 蒙特卡洛方法实现

粒子滤波跟踪系统MATLAB实现，包含权值更新和重采样机制：

%% 粒子滤波跟踪系统 - 蒙特卡洛方法实现
% 作者：MATLAB助手
% 功能：使用粒子滤波进行目标跟踪，包含完整的蒙特卡洛模拟、权值更新和重采样

clear all; close all; clc;
rng(42); % 设置随机种子，确保结果可重复

%% 1. 系统参数设置
disp('=== 粒子滤波目标跟踪系统 ===');
fprintf('初始化参数...\n');

% 跟踪参数
n_particles = 500;          % 粒子数量
n_steps = 100;              % 时间步数
dt = 0.1;                   % 时间间隔

% 状态空间维数 (x, y, vx, vy)
state_dim = 4;              % 状态维度
obs_dim = 2;                % 观测维度 (仅位置)

% 过程噪声（系统噪声）
Q = diag([0.1, 0.1, 0.5, 0.5]);  % 过程噪声协方差矩阵

% 观测噪声
R = diag([1, 1]);           % 观测噪声协方差矩阵

% 初始状态
initial_state = [0; 0; 1; 0.5];  % [x, y, vx, vy]

% 重采样阈值
resample_threshold = n_particles * 0.5;  % 有效粒子数阈值

% 存储变量
true_states = zeros(state_dim, n_steps);      % 真实状态
observations = zeros(obs_dim, n_steps);       % 观测值
estimated_states = zeros(state_dim, n_steps); % 估计状态
particle_weights = zeros(n_particles, n_steps); % 粒子权重历史
effective_samples = zeros(1, n_steps);         % 有效粒子数历史

%% 2. 粒子滤波核心函数定义

% 2.1 状态转移函数（运动模型）
function new_state = state_transition(state, dt, Q)
    % 常速度模型
    % state: [x; y; vx; vy]
    % dt: 时间间隔
    % Q: 过程噪声协方差
    
    % 状态转移矩阵
    F = [1, 0, dt, 0;
         0, 1, 0, dt;
         0, 0, 1, 0;
         0, 0, 0, 1];
    
    % 预测状态
    predicted_state = F * state;
    
    % 添加过程噪声
    process_noise = mvnrnd(zeros(size(state)), Q)';
    new_state = predicted_state + process_noise;
end

% 2.2 观测函数
function observation = observation_model(state, R)
    % 观测位置 (x, y)
    % state: [x; y; vx; vy]
    % R: 观测噪声协方差
    
    % 观测矩阵
    H = [1, 0, 0, 0;
         0, 1, 0, 0];
    
    % 预测观测
    predicted_obs = H * state;
    
    % 添加观测噪声
    obs_noise = mvnrnd(zeros(2,1), R)';
    observation = predicted_obs + obs_noise;
end

% 2.3 重要性权重计算函数
function weight = compute_weight(observation, predicted_obs, R)
    % 计算粒子权重（观测似然）
    % observation: 实际观测值
    % predicted_obs: 粒子预测的观测值
    % R: 观测噪声协方差
    
    % 计算观测残差
    residual = observation - predicted_obs;
    
    % 计算观测似然（多元高斯分布）
    % p(z|x) ~ N(z; Hx, R)
    exponent = -0.5 * (residual' / R * residual);
    normalization = 1 / sqrt((2*pi)^length(observation) * det(R));
    weight = normalization * exp(exponent);
    
    % 避免数值下溢
    weight = max(weight, 1e-100);
end

% 2.4 系统重采样函数
function [new_particles, new_weights] = systematic_resample(particles, weights)
    % 系统重采样算法
    % particles: 当前粒子集合
    % weights: 当前粒子权重
    
    n_particles = size(particles, 2);
    
    % 归一化权重
    weights = weights / sum(weights);
    
    % 计算累积分布函数(CDF)
    cdf = cumsum(weights);
    
    % 生成均匀分布的起始点
    u0 = rand() / n_particles;
    
    % 系统重采样
    new_particles = zeros(size(particles));
    new_weights = ones(n_particles, 1) / n_particles;
    
    i = 1;
    for j = 1:n_particles
        u = u0 + (j-1) / n_particles;
        
        while u > cdf(i) && i < n_particles
            i = i + 1;
        end
        
        new_particles(:, j) = particles(:, i);
    end
end

% 2.5 有效粒子数计算
function N_eff = effective_sample_size(weights)
    % 计算有效粒子数
    % weights: 粒子权重
    
    % 归一化权重
    weights = weights / sum(weights);
    
    % 计算有效粒子数
    N_eff = 1 / sum(weights.^2);
end

% 2.6 粒子滤波主函数
function [estimated_state, particles, weights, N_eff] = ...
    particle_filter_step(particles, weights, observation, dt, Q, R)
    
    n_particles = size(particles, 2);
    
    % 1. 预测步骤（状态转移）
    for i = 1:n_particles
        particles(:, i) = state_transition(particles(:, i), dt, Q);
    end
    
    % 2. 更新步骤（权重计算）
    for i = 1:n_particles
        % 预测观测
        H = [1, 0, 0, 0; 0, 1, 0, 0];
        predicted_obs = H * particles(:, i);
        
        % 计算权重（观测似然）
        weights(i) = compute_weight(observation, predicted_obs, R);
    end
    
    % 3. 权重归一化
    weights = weights / sum(weights);
    
    % 4. 计算有效粒子数
    N_eff = effective_sample_size(weights);
    
    % 5. 重采样（如果需要）
    if N_eff < n_particles * 0.5
        [particles, weights] = systematic_resample(particles, weights);
        fprintf('  执行重采样 (N_eff = %.1f)\n', N_eff);
    end
    
    % 6. 状态估计（加权平均）
    estimated_state = zeros(size(particles, 1), 1);
    for i = 1:n_particles
        estimated_state = estimated_state + weights(i) * particles(:, i);
    end
end

%% 3. 生成模拟数据
fprintf('生成模拟轨迹...\n');

% 3.1 生成真实轨迹（带机动）
true_states(:, 1) = initial_state;

for t = 2:n_steps
    % 基本运动模型
    F = [1, 0, dt, 0;
         0, 1, 0, dt;
         0, 0, 1, 0;
         0, 0, 0, 1];
    
    % 添加一些机动（转弯）
    if t == 30
        true_states(3:4, t-1) = [0.5; 1.2];  % 改变速度
    elseif t == 60
        true_states(3:4, t-1) = [1.5; -0.5]; % 再次改变速度
    end
    
    % 状态转移
    true_states(:, t) = F * true_states(:, t-1);
    
    % 添加过程噪声（真实世界的随机扰动）
    true_states(:, t) = true_states(:, t) + mvnrnd(zeros(state_dim,1), Q*0.5)';
end

% 3.2 生成带噪声的观测
for t = 1:n_steps
    H = [1, 0, 0, 0; 0, 1, 0, 0];
    true_obs = H * true_states(:, t);
    observations(:, t) = true_obs + mvnrnd(zeros(obs_dim,1), R)';
end

%% 4. 粒子滤波初始化
fprintf('初始化粒子滤波...\n');

% 4.1 初始化粒子集合
particles = zeros(state_dim, n_particles);

% 在初始状态周围随机生成粒子
for i = 1:n_particles
    % 在初始状态周围添加随机扰动
    initial_noise = mvnrnd(zeros(state_dim,1), diag([2, 2, 1, 1]))';
    particles(:, i) = initial_state + initial_noise;
end

% 4.2 初始化权重
weights = ones(n_particles, 1) / n_particles;

% 4.3 初始估计
estimated_states(:, 1) = initial_state;
particle_weights(:, 1) = weights;
effective_samples(1) = n_particles;

%% 5. 运行粒子滤波
fprintf('开始粒子滤波跟踪...\n');

% 进度条
h = waitbar(0, '粒子滤波处理中...');

for t = 2:n_steps
    % 更新进度条
    waitbar(t/n_steps, h, sprintf('处理中: %d/%d (%.1f%%)', t, n_steps, t/n_steps*100));
    
    fprintf('时间步 %d/%d: ', t, n_steps);
    
    % 执行粒子滤波步骤
    [estimated_state, particles, weights, N_eff] = ...
        particle_filter_step(particles, weights, observations(:, t), dt, Q, R);
    
    % 存储结果
    estimated_states(:, t) = estimated_state;
    particle_weights(:, t) = weights;
    effective_samples(t) = N_eff;
    
    fprintf('估计位置: (%.2f, %.2f), N_eff: %.1f\n', ...
        estimated_state(1), estimated_state(2), N_eff);
end

% 关闭进度条
close(h);

%% 6. 性能评估
fprintf('\n=== 性能评估 ===\n');

% 6.1 计算跟踪误差
position_errors = zeros(1, n_steps);
velocity_errors = zeros(1, n_steps);

for t = 1:n_steps
    % 位置误差
    true_pos = true_states(1:2, t);
    est_pos = estimated_states(1:2, t);
    position_errors(t) = norm(true_pos - est_pos);
    
    % 速度误差
    true_vel = true_states(3:4, t);
    est_vel = estimated_states(3:4, t);
    velocity_errors(t) = norm(true_vel - est_vel);
end

% 6.2 计算统计指标
mean_position_error = mean(position_errors);
std_position_error = std(position_errors);
max_position_error = max(position_errors);

mean_velocity_error = mean(velocity_errors);
std_velocity_error = std(velocity_errors);

fprintf('位置跟踪性能:\n');
fprintf('  平均误差: %.4f\n', mean_position_error);
fprintf('  标准差: %.4f\n', std_position_error);
fprintf('  最大误差: %.4f\n', max_position_error);
fprintf('\n速度估计性能:\n');
fprintf('  平均误差: %.4f\n', mean_velocity_error);
fprintf('  标准差: %.4f\n', std_velocity_error);

% 6.3 计算RMSE
rmse_position = sqrt(mean(position_errors.^2));
rmse_velocity = sqrt(mean(velocity_errors.^2));
fprintf('\nRMSE指标:\n');
fprintf('  位置RMSE: %.4f\n', rmse_position);
fprintf('  速度RMSE: %.4f\n', rmse_velocity);

%% 7. 可视化结果

% 7.1 创建主图窗口
figure('Position', [50, 50, 1400, 900], 'Name', '粒子滤波跟踪结果');

% 7.2 轨迹跟踪图
subplot(3, 4, [1, 2, 5, 6]);
hold on;
grid on;

% 绘制真实轨迹
plot(true_states(1, :), true_states(2, :), 'b-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
% 绘制估计轨迹
plot(estimated_states(1, :), estimated_states(2, :), 'r-', 'LineWidth', 2, 'DisplayName', '估计轨迹');
% 绘制观测点
scatter(observations(1, :), observations(2, :), 30, 'g', 'filled', 'DisplayName', '观测值');

% 标记起点和终点
plot(true_states(1, 1), true_states(2, 1), 'bo', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '起点');
plot(true_states(1, end), true_states(2, end), 'bs', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '终点');
plot(estimated_states(1, end), estimated_states(2, end), 'rs', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '估计终点');

% 绘制当前粒子（最后时刻）
scatter(particles(1, :), particles(2, :), 20, 'm', 'filled', 'AlphaData', 0.3, ...
    'MarkerFaceAlpha', 0.3, 'DisplayName', '粒子');

xlabel('X 位置');
ylabel('Y 位置');
title('目标跟踪轨迹');
legend('Location', 'best');
axis equal;

% 7.3 位置误差图
subplot(3, 4, 3);
plot(1:n_steps, position_errors, 'b-', 'LineWidth', 1.5);
hold on;
plot([1, n_steps], [mean_position_error, mean_position_error], 'r--', 'LineWidth', 1.5);
grid on;
xlabel('时间步');
ylabel('位置误差');
title(sprintf('位置跟踪误差 (平均: %.3f)', mean_position_error));
legend('误差', '平均值', 'Location', 'best');

% 7.4 速度误差图
subplot(3, 4, 4);
plot(1:n_steps, velocity_errors, 'b-', 'LineWidth', 1.5);
hold on;
plot([1, n_steps], [mean_velocity_error, mean_velocity_error], 'r--', 'LineWidth', 1.5);
grid on;
xlabel('时间步');
ylabel('速度误差');
title(sprintf('速度估计误差 (平均: %.3f)', mean_velocity_error));
legend('误差', '平均值', 'Location', 'best');

% 7.5 有效粒子数变化
subplot(3, 4, 7);
plot(1:n_steps, effective_samples, 'b-', 'LineWidth', 1.5);
hold on;
plot([1, n_steps], [resample_threshold, resample_threshold], 'r--', 'LineWidth', 1.5);
grid on;
xlabel('时间步');
ylabel('有效粒子数');
title('有效粒子数变化');
legend('N_{eff}', '重采样阈值', 'Location', 'best');
ylim([0, n_particles]);

% 7.6 粒子权重分布（最后时刻）
subplot(3, 4, 8);
sorted_weights = sort(weights, 'descend');
bar(1:min(50, n_particles), sorted_weights(1:min(50, n_particles)));
xlabel('粒子序号（按权重降序）');
ylabel('权重');
title('粒子权重分布（最后时刻）');
grid on;

% 7.7 X方向跟踪
subplot(3, 4, 9);
plot(1:n_steps, true_states(1, :), 'b-', 'LineWidth', 2, 'DisplayName', '真实值');
hold on;
plot(1:n_steps, estimated_states(1, :), 'r-', 'LineWidth', 1.5, 'DisplayName', '估计值');
plot(1:n_steps, observations(1, :), 'g.', 'MarkerSize', 8, 'DisplayName', '观测值');
grid on;
xlabel('时间步');
ylabel('X 位置');
title('X方向跟踪');
legend('Location', 'best');

% 7.8 Y方向跟踪
subplot(3, 4, 10);
plot(1:n_steps, true_states(2, :), 'b-', 'LineWidth', 2, 'DisplayName', '真实值');
hold on;
plot(1:n_steps, estimated_states(2, :), 'r-', 'LineWidth', 1.5, 'DisplayName', '估计值');
plot(1:n_steps, observations(2, :), 'g.', 'MarkerSize', 8, 'DisplayName', '观测值');
grid on;
xlabel('时间步');
ylabel('Y 位置');
title('Y方向跟踪');
legend('Location', 'best');

% 7.9 X方向速度估计
subplot(3, 4, 11);
plot(1:n_steps, true_states(3, :), 'b-', 'LineWidth', 2, 'DisplayName', '真实值');
hold on;
plot(1:n_steps, estimated_states(3, :), 'r-', 'LineWidth', 1.5, 'DisplayName', '估计值');
grid on;
xlabel('时间步');
ylabel('Vx 速度');
title('X方向速度估计');
legend('Location', 'best');

% 7.10 Y方向速度估计
subplot(3, 4, 12);
plot(1:n_steps, true_states(4, :), 'b-', 'LineWidth', 2, 'DisplayName', '真实值');
hold on;
plot(1:n_steps, estimated_states(4, :), 'r-', 'LineWidth', 1.5, 'DisplayName', '估计值');
grid on;
xlabel('时间步');
ylabel('Vy 速度');
title('Y方向速度估计');
legend('Location', 'best');

sgtitle('粒子滤波跟踪系统 - 蒙特卡洛方法实现', 'FontSize', 14, 'FontWeight', 'bold');

%% 8. 粒子演化动画（可选）
fprintf('\n生成粒子演化动画...\n');

create_animation = true;  % 设置为false跳过动画生成

if create_animation
    % 选择关键帧
    key_frames = [1, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100];
    
    figure('Position', [100, 100, 1200, 500], 'Name', '粒子演化过程');
    
    for k = 1:length(key_frames)
        t = key_frames(k);
        
        % 重建该时刻的粒子状态（需要从历史中获取）
        % 注意：实际中我们不会保存所有时刻的所有粒子，这里为了演示需要特殊处理
        % 这里我们简单展示最后时刻的粒子状态
        
        subplot(2, 5, k);
        hold on;
        grid on;
        
        % 绘制真实轨迹（到当前时刻）
        plot(true_states(1, 1:t), true_states(2, 1:t), 'b-', 'LineWidth', 1.5);
        
        % 绘制估计轨迹（到当前时刻）
        plot(estimated_states(1, 1:t), estimated_states(2, 1:t), 'r-', 'LineWidth', 1.5);
        
        % 绘制观测值（到当前时刻）
        scatter(observations(1, 1:t), observations(2, 1:t), 20, 'g', 'filled');
        
        % 绘制当前时刻的粒子（这里用最后时刻的粒子代替）
        scatter(particles(1, :), particles(2, :), 15, 'm', 'filled', 'AlphaData', 0.2, ...
            'MarkerFaceAlpha', 0.2);
        
        % 标记当前估计位置
        plot(estimated_states(1, t), estimated_states(2, t), 'ro', 'MarkerSize', 8, 'LineWidth', 2);
        
        xlabel('X');
        ylabel('Y');
        title(sprintf('t = %d, N_{eff} = %.1f', t, effective_samples(t)));
        axis equal;
        
        if k == 1
            legend('真实轨迹', '估计轨迹', '观测值', '粒子', '当前估计', 'Location', 'best');
        end
    end
    
    sgtitle('粒子滤波演化过程（关键帧）', 'FontSize', 12, 'FontWeight', 'bold');
end

%% 9. 蒙特卡洛模拟分析
fprintf('\n=== 蒙特卡洛模拟分析 ===\n');

% 进行多次蒙特卡洛模拟以评估算法性能
n_monte_carlo = 20;  % 蒙特卡洛模拟次数
mc_errors = zeros(n_monte_carlo, n_steps);
mc_effective_samples = zeros(n_monte_carlo, n_steps);

fprintf('进行 %d 次蒙特卡洛模拟...\n', n_monte_carlo);

h_mc = waitbar(0, '蒙特卡洛模拟进行中...');

for mc_run = 1:n_monte_carlo
    waitbar(mc_run/n_monte_carlo, h_mc, sprintf('模拟: %d/%d', mc_run, n_monte_carlo));
    
    % 重新生成数据（不同的噪声实现）
    mc_true_states = zeros(state_dim, n_steps);
    mc_observations = zeros(obs_dim, n_steps);
    
    % 生成新的轨迹
    mc_true_states(:, 1) = initial_state + mvnrnd(zeros(state_dim,1), diag([0.1, 0.1, 0.1, 0.1]))';
    
    for t = 2:n_steps
        F = [1, 0, dt, 0;
             0, 1, 0, dt;
             0, 0, 1, 0;
             0, 0, 0, 1];
        
        % 相同的机动模式
        if t == 30
            mc_true_states(3:4, t-1) = [0.5; 1.2];
        elseif t == 60
            mc_true_states(3:4, t-1) = [1.5; -0.5];
        end
        
        mc_true_states(:, t) = F * mc_true_states(:, t-1);
        mc_true_states(:, t) = mc_true_states(:, t) + mvnrnd(zeros(state_dim,1), Q*0.5)';
        
        % 生成观测
        H = [1, 0, 0, 0; 0, 1, 0, 0];
        true_obs = H * mc_true_states(:, t);
        mc_observations(:, t) = true_obs + mvnrnd(zeros(obs_dim,1), R)';
    end
    
    % 运行粒子滤波
    mc_particles = zeros(state_dim, n_particles);
    for i = 1:n_particles
        initial_noise = mvnrnd(zeros(state_dim,1), diag([2, 2, 1, 1]))';
        mc_particles(:, i) = initial_state + initial_noise;
    end
    
    mc_weights = ones(n_particles, 1) / n_particles;
    mc_estimated_states = zeros(state_dim, n_steps);
    mc_estimated_states(:, 1) = initial_state;
    
    for t = 2:n_steps
        [mc_estimated_state, mc_particles, mc_weights, mc_N_eff] = ...
            particle_filter_step(mc_particles, mc_weights, mc_observations(:, t), dt, Q, R);
        
        mc_estimated_states(:, t) = mc_estimated_state;
        mc_effective_samples(mc_run, t) = mc_N_eff;
    end
    
    % 计算误差
    for t = 1:n_steps
        true_pos = mc_true_states(1:2, t);
        est_pos = mc_estimated_states(1:2, t);
        mc_errors(mc_run, t) = norm(true_pos - est_pos);
    end
end

close(h_mc);

% 计算蒙特卡洛统计
mean_mc_error = mean(mc_errors, 1);
std_mc_error = std(mc_errors, 0, 1);
mean_effective_samples = mean(mc_effective_samples, 1);

% 绘制蒙特卡洛分析结果
figure('Position', [100, 100, 1000, 400], 'Name', '蒙特卡洛分析');

subplot(1, 2, 1);
hold on;
grid on;

% 绘制误差带
x = 1:n_steps;
fill([x, fliplr(x)], [mean_mc_error + std_mc_error, fliplr(mean_mc_error - std_mc_error)], ...
    [0.8, 0.8, 1], 'EdgeColor', 'none', 'FaceAlpha', 0.5, 'DisplayName', '±1标准差');

% 绘制平均误差
plot(x, mean_mc_error, 'b-', 'LineWidth', 2, 'DisplayName', '平均误差');

xlabel('时间步');
ylabel('位置误差');
title(sprintf('蒙特卡洛模拟误差 (N=%d)', n_monte_carlo));
legend('Location', 'best');

subplot(1, 2, 2);
hold on;
grid on;

% 绘制有效粒子数
plot(x, mean_effective_samples, 'b-', 'LineWidth', 2, 'DisplayName', '平均有效粒子数');
plot([1, n_steps], [resample_threshold, resample_threshold], 'r--', 'LineWidth', 1.5, 'DisplayName', '重采样阈值');

xlabel('时间步');
ylabel('有效粒子数');
title('有效粒子数变化（蒙特卡洛平均）');
legend('Location', 'best');
ylim([0, n_particles]);

sgtitle('粒子滤波蒙特卡洛模拟分析', 'FontSize', 12, 'FontWeight', 'bold');

%% 10. 保存结果
fprintf('\n=== 保存结果 ===\n');

% 创建保存目录
results_dir = 'particle_filter_results';
if ~exist(results_dir, 'dir')
    mkdir(results_dir);
end

% 保存主要结果
save(fullfile(results_dir, 'particle_filter_tracking.mat'), ...
    'true_states', 'observations', 'estimated_states', ...
    'particle_weights', 'effective_samples', ...
    'position_errors', 'velocity_errors', ...
    'n_particles', 'n_steps', 'dt', 'Q', 'R');

% 保存性能指标
performance_metrics = struct();
performance_metrics.mean_position_error = mean_position_error;
performance_metrics.std_position_error = std_position_error;
performance_metrics.max_position_error = max_position_error;
performance_metrics.mean_velocity_error = mean_velocity_error;
performance_metrics.std_velocity_error = std_velocity_error;
performance_metrics.rmse_position = rmse_position;
performance_metrics.rmse_velocity = rmse_velocity;

save(fullfile(results_dir, 'performance_metrics.mat'), 'performance_metrics');

% 保存蒙特卡洛结果
if exist('mean_mc_error', 'var')
    save(fullfile(results_dir, 'monte_carlo_results.mat'), ...
        'mc_errors', 'mean_mc_error', 'std_mc_error', ...
        'mean_effective_samples', 'n_monte_carlo');
end

% 保存关键图像
saveas(gcf, fullfile(results_dir, 'tracking_results.png'));

fprintf('结果已保存到文件夹: %s\n', results_dir);
fprintf('\n=== 粒子滤波跟踪系统执行完成 ===\n');

程序功能说明

1. 核心算法组件

• 粒子滤波框架：完整的预测-更新-重采样循环
• 状态转移模型：常速度模型（可扩展为其他运动模型）
• 观测模型：带噪声的位置观测
• 重要性权重计算：基于观测似然函数
• 系统重采样：避免粒子退化问题
• 有效粒子数监测：自适应重采样触发

2. 蒙特卡洛方法

• 粒子初始化：在初始状态周围随机采样
• 状态预测：通过状态转移方程传播粒子
• 权重更新：基于观测数据更新粒子权重
• 重采样：根据权重重新分配粒子
• 状态估计：加权平均得到最终估计

3. 系统特性

• 非线性处理能力：粒子滤波天然适合非线性系统
• 非高斯噪声：可以处理各种噪声分布
• 多模态分布：能够表示多峰后验分布
• 自适应重采样：根据有效粒子数自动调整

4. 性能评估

• 跟踪误差分析：位置和速度误差统计
• RMSE计算：均方根误差评估
• 蒙特卡洛模拟：多次运行评估算法稳定性
• 有效粒子数监测：算法健康状态指标

5. 可视化功能

• 轨迹跟踪图：真实、估计和观测轨迹对比
• 误差分析图：位置和速度误差随时间变化
• 粒子分布：粒子权重和空间分布
• 蒙特卡洛分析：多次模拟的统计结果
• 动画展示：粒子演化过程（可选）

参考代码 用粒子滤波跟踪,蒙特卡洛方法，权值更新，重采样 www.3dddown.com/cna/96621.html

使用说明

基本使用：

1. 直接运行程序，将自动生成模拟数据并执行粒子滤波跟踪
2. 程序会显示详细的跟踪过程和性能指标
3. 自动保存所有结果和图像

参数调整：

• n_particles：粒子数量（增加提高精度，但增加计算量）
• n_steps：跟踪时间步数
• Q：过程噪声协方差（调整系统不确定性）
• R：观测噪声协方差（调整观测可靠性）
• resample_threshold：重采样触发阈值

扩展功能：

1. 更换运动模型：修改state_transition函数
2. 更换观测模型：修改observation_model函数
3. 添加非线性：在状态转移或观测模型中添加非线性项
4. 更换重采样方法：实现其他重采样算法（如残差重采样）
5. 并行化：使用parfor循环加速粒子处理

算法优势

1. 非线性处理：不受线性假设限制
2. 多峰分布：能够处理多目标或模糊状态
3. 灵活性：易于适应不同的系统和观测模型
4. 收敛性：随着粒子数增加，近似真实后验分布

应用场景

1. 目标跟踪：雷达、视频、GPS目标跟踪
2. 机器人定位：SLAM中的定位问题
3. 金融预测：非线性时间序列预测
4. 信号处理：非线性滤波问题
5. 生物信息学：基因序列分析