基于粒子滤波的运动目标跟踪MATLAB实现

基于粒子滤波的运动目标跟踪MATLAB实现代码，结合了状态预测、权重更新、重采样等核心步骤，并针对实时性和鲁棒性进行了优化：

一、核心代码实现

matlab 复制代码

%% 参数设置
numParticles = 500;    % 粒子数量（平衡精度与计算量）
dt = 1;                % 时间步长
processNoise = 0.5;    % 过程噪声标准差（运动不确定性）
measurementNoise = 2;  % 测量噪声标准差（观测不确定性）
stateDim = 4;          % 状态维度 [x, y, vx, vy]

%% 初始化粒子
particles = randn(stateDim, numParticles) * 5;  % 初始状态随机分布
weights = ones(1, numParticles) / numParticles;  % 均匀权重

%% 主循环（逐帧处理）
for frame = 1:numFrames
    % 1. 预测阶段：状态传播
    particles = predictParticles(particles, dt, processNoise);
    
    % 2. 更新阶段：计算权重
    measurements = getMeasurement(frame);  % 获取当前帧目标观测值
    weights = updateWeights(particles, measurements, measurementNoise);
    
    % 3. 重采样：避免粒子退化
    [particles, weights] = resampleParticles(particles, weights);
    
    % 4. 状态估计：加权平均
    estimatedState = mean(particles, 2);
    
    % 5. 可视化
    plotTrackingResults(frame, particles, estimatedState);
end

%% 函数定义
% 粒子预测（运动模型）
function particles = predictParticles(particles, dt, noise)
    % 匀速运动模型（可替换为匀加速或其他模型）
    noiseMatrix = diag([noise, noise, noise, noise]);
    particles = particles + [1, 0, dt, 0;
                             0, 1, 0, dt;
                             0, 0, 1, 0;
                             0, 0, 0, 1] * particles + sqrt(noiseMatrix) * randn(size(particles));
end

% 权重更新（观测似然）
function weights = updateWeights(particles, z, noise)
    % 计算粒子与观测的欧氏距离
    dist = sqrt(sum((particles(1:2,:) - z).^2, 2));
    % 高斯似然函数
    weights = exp(-0.5 * (dist / noise).^2);
    weights = weights / sum(weights);  % 归一化
end

% 系统重采样（低方差重采样）
function [newParticles, newWeights] = resampleParticles(particles, weights)
    N = size(particles, 2);
    cumWeights = cumsum(weights);
    indices = zeros(1, N);
    T = linspace(0, 1-1/N, N) + rand()/N;
    
    for i = 1:N
        idx = find(cumWeights >= T(i), 1);
        indices(i) = idx;
    end
    newParticles = particles(:, indices);
    newWeights = ones(1, N) / N;  % 重置权重
end

二、关键改进与优化

运动模型扩展
- 支持匀速（CV）、匀加速（CA）等模型，通过修改predictParticles函数实现
- 示例：匀加速模型需扩展状态维度至[x, y, vx, vy, ax, ay]

观测模型优化

可替换为特征匹配（如颜色直方图、HOG特征）提升遮挡场景鲁棒性

示例：

matlab 复制代码

function weights = updateWeights(particles, z, featureExtractor)
    % 提取粒子特征与观测特征
    particleFeatures = extractFeatures(particles);
    obsFeature = featureExtractor(z);
    % 计算巴氏距离作为似然
    weights = exp(-0.1 * pdist2(particleFeatures, obsFeature, 'bhattacharyya'));
    weights = weights / sum(weights);
end

自适应重采样策略
- 引入有效粒子数判断，仅在Neff < 0.5*N时触发重采样，减少计算量
- 改进混合重采样算法（系统+残差重采样），平衡多样性与效率
粒子多样性增强
- 粒子群优化（PSO）扰动：在重采样后添加随机扰动
matlab 复制代码
```
particles = particles + 0.1*randn(size(particles));  % 扰动幅度可调
```

三、应用场景与参数调整

场景类型	推荐参数设置	改进措施
室内匀速运动	`numParticles=200`, `dt=0.5`	使用匀速模型+颜色特征
户外机动目标	`numParticles=800`, `noise=1.2`	匀加速模型+多特征融合
低分辨率目标	`numParticles=1000`, `featureExtractor=HOG`	增加粒子数+直方图匹配
实时跟踪	`numParticles=300`, 开启GPU加速	降采样+并行计算

参考代码基于粒子滤波的运动目标跟踪程序 youwenfan.com/contentcse/98221.html

可有效跟踪匀速/机动目标，并在复杂场景下保持较高鲁棒性。具体参数需根据实际场景进行调优。