粒子滤波搜寻放射源的matlab动态过程可视化模型模拟代码

以下是一个简单的粒子滤波器（PF）模拟代码，用于搜寻放射源。该代码使用MATLAB编写，并且通过动态可视化来展示PF的工作过程。请注意，这只是一个简单的模拟，实际应用中需要更详细和复杂的算法和模型。

% 粒子滤波搜索放射源的MATLAB模拟代码

% 初始化
N = 100; % 粒子数量
T = 50; % 时间步数
x_true = zeros(2, T); % 真实位置
z = zeros(1, T); % 观测值
w = zeros(N, T); % 权重
x = zeros(2, N, T); % 粒子位置
x(:,:,1) = repmat([0; 0], [1, N]); % 初始位置
x_est = zeros(2, T); % 估计位置
resampling_counter = 0; % 重采样计数器

% 定义模型参数
sigma_v = 1; % 过程噪声标准差
sigma_w = 0.5; % 观测噪声标准差
alpha = 0.95; % 重采样阈值

% 生成真实位置和观测值
for t = 2:T
    x_true(:,t) = x_true(:,t-1) + sigma_v * randn(2,1);
    z(t) = norm(x_true(:,t)) + sigma_w * randn(1);
end

% 运行粒子滤波器
for t = 2:T
    % 预测
    for i = 1:N
        x(:,i,t) = x(:,i,t-1) + sigma_v * randn(2,1);
        w(i,t) = norm(x(:,i,t)) - z(t);
    end
    % 权重归一化
    w(:,t) = exp(-w(:,t).^2/(2*sigma_w^2));
    w(:,t) = w(:,t) / sum(w(:,t));
    % 估计位置
    x_est(:,t) = sum(repmat(w(:,t), [2,1]) .* x(:,:,t), 2);
    % 判断是否需要重采样
    ESS = 1/sum(w(:,t).^2);
    if ESS < alpha*N
        % 重采样
        resampling_counter = resampling_counter + 1;
        % 等概率采样
        cumsum_w = cumsum(w(:,t));
        for i = 1:N
            j = find(cumsum_w >= rand, 1, 'first');
            x(:,i,t) = x(:,j,t);
        end
        w(:,t) = repmat(1/N, [N, 1]);
    end
end

% 动态可视化
figure;
for t = 2:T
    % 绘制真实位置和观测值
    subplot(2,1,1);
    plot(x_true(1,1:t), x_true(2,1:t), 'b-', 'DisplayName', 'True position');
    hold on;
    plot([0, x_true(1,t)], [0, x_true(2,t)], 'r-', 'DisplayName', 'Observation');
    plot(x_true(1,t), x_true(2,t), 'ro');
    hold off;
    axis([-10, 10, -10, 10]);
    title(sprintf('Time step %d', t));
    legend('show');
    % 绘制粒子位置和估计位置
    subplot(2,1,2);
    plot(x(1,:,t), x(2,:,t), 'k.', 'MarkerSize', 5, 'DisplayName', 'Particles');
    hold on;
    plot(x_est(1,1:t), x_est(2,1:t), 'g-', 'LineWidth', 2, 'DisplayName', 'Estimated position');
    hold off;
    axis([-10, 10, -10, 10]);
    title(sprintf('Resampling counter: %d, ESS: %.2f', resampling_counter, ESS));
    legend('show');
    drawnow;
end

该代码将生成一个动态可视化，显示真实位置、观测值、粒子位置和估计位置。在每个时间步骤中，粒子将根据预测模型进行移动，并计算其权重。然后，通过重采样和归一化权重来更新粒子位置和权重。最后，估计位置将基于所有粒子的加权平均值计算得出。

请注意，此代码仅用于演示目的，实际应用中需要更多的模型和算法来处理复杂的问题。