自适应粒子滤波（Adaptive Particle Filter, APF）是一种用于状态估计的有效方法，特别适用于非线性和非高斯系统。
文章目录
- 应用场景
- MATLAB 代码示例
- 代码说明
- 结果
- 扩展说明
以下是一个基于自适应粒子滤波的简单应用示例，模拟一个一维目标跟踪问题。
应用场景
假设我们要跟踪一个在一维空间中移动的目标，其运动模型为匀速运动，状态由目标位置和速度组成。观测模型为目标位置的测量，受到高斯噪声的影响。
MATLAB 代码示例
以下是自适应粒子滤波的 MATLAB 代码示例：
% 自适应粒子滤波示例：一维目标跟踪
% 清空环境
clc;
clear;
close all;
% 参数设置
numParticles = 100; % 粒子数量
numSteps = 50; % 时间步数
dt = 1; % 时间间隔
sigma_process = 0.5; % 过程噪声标准差
sigma_measure = 1; % 测量噪声标准差
% 初始化状态
true_position = zeros(numSteps, 1);
measurements = zeros(numSteps, 1);
estimated_position = zeros(numSteps, 1);
% 初始位置和速度
true_position(1) = 0; % 初始真实位置
velocity = 1; % 目标速度
% 粒子滤波初始化
particles = zeros(numParticles, 1); % 粒子位置
weights = ones(numParticles, 1) / numParticles; % 粒子权重
% 生成真实轨迹和测量值
for k = 2:numSteps
true_position(k) = true_position(k-1) + velocity * dt + sigma_process * randn;
measurements(k) = true_position(k) + sigma_measure * randn; % 观测
end
% 粒子滤波过程
for k = 1:numSteps
% 预测步骤
particles = particles + velocity * dt + sigma_process * randn(numParticles, 1);
% 更新权重
for i = 1:numParticles
weights(i) = normpdf(measurements(k), particles(i), sigma_measure);
end
weights = weights / sum(weights); % 归一化权重
% 重采样
indices = randsample(1:numParticles, numParticles, true, weights);
particles = particles(indices);
% 状态估计
estimated_position(k) = mean(particles); % 估计位置
end
% 绘制结果
figure;
hold on;
plot(true_position, 'g-', 'LineWidth', 2, 'DisplayName', '真实位置');
plot(measurements, 'r.', 'DisplayName', '测量值');
plot(estimated_position, 'b-', 'LineWidth', 2, 'DisplayName', '估计位置');
xlabel('时间步');
ylabel('位置');
legend show;
title('自适应粒子滤波 - 一维目标跟踪');
grid on;
hold off;
代码说明
-
参数设置：
numParticles：粒子数量。numSteps：时间步数。sigma_process和sigma_measure：过程噪声和测量噪声的标准差。
-
状态初始化：
- 初始化真实位置和速度，生成真实轨迹和测量值。
-
粒子滤波过程：
- 预测步骤：更新每个粒子的状态。
- 权重更新：根据观测值更新每个粒子的权重。
- 重采样：根据权重进行重采样，以减少粒子退化。
-
状态估计：通过计算粒子位置的均值来估计目标位置。
-
绘制结果：绘制真实位置、测量值和估计位置的曲线。
结果
运行代码后，将输出滤波示意图，显示真实位置、测量值和估计位置。通过这种方式，可以直观地看到自适应粒子滤波在目标跟踪中的应用效果：
可以根据需要进一步扩展此代码，例如加入自适应机制，动态调整粒子数量或噪声参数，以应对不同的环境变化：
扩展说明
- 有效粒子数量计算：
使用 Neff = 1 / sum(weights.^2) 计算有效粒子数量，以判断当前粒子的有效性。 - 自适应调整：
如果有效粒子数量低于设定阈值，增加粒子数量（最多到 1000），并添加新的粒子。
如果有效粒子数量高于阈值，减少粒子数量（最少到 50），并保留前 numParticles 个粒子。 - 记录自适应粒子数量：
在每个时间步中记录当前的粒子数量，以便后续绘制。
结果绘制： - 绘制真实位置、测量值和估计位置的变化。
单独绘制自适应粒子数量的变化曲线。
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
