粒子滤波 目标跟踪 matlab

### 粒子滤波目标跟踪的MATLAB实现

#### 初始化参数设置
为了初始化粒子滤波器，在MATLAB中需要定义一些基本参数，比如粒子数量、初始状态以及噪声模型等。这一步骤对于后续的粒子传播和重采样至关重要[^1]。

% 参数设定
numParticles = 500; % 粒子数目
stateDim = 4;       % 状态维度 (x,y,v_x,v_y)
initialStateMean = [250, 200, 0, 0]; % 初始位置及速度均值
initialCovariance = eye(stateDim)*1e3; % 较大方差表示不确定性高
processNoiseStd = sqrt([10, 10, 1, 1]); % 过程噪音标准偏差
measurementNoiseStd = sqrt(1);          % 测量噪音标准偏差

#### 创建粒子群并赋予权重
接下来创建一个由`numParticles`个粒子组成的集合，并给定每个粒子相同的初始权重。这里还涉及到从先验分布中抽取样本作为各粒子的位置[^2]。

particles = repmat(initialStateMean', numParticles, 1) ...
            + randn(numParticles,stateDim).*sqrt(diag(initialCovariance));
weights = ones(size(particles,1),1)/size(particles,1);

#### 预测阶段
预测步骤涉及利用运动模型来推测下一时刻可能的状态变化情况。此过程会引入一定的随机扰动以模拟实际系统的不确定因素。

function predict_particles()
    global particles processNoiseStd
    
    for i=1:size(particles,1)
        % 假设简单的匀速直线运动模型加上高斯白噪
        particles(i,:) = particles(i,:) + [randn*processNoiseStd(1),...
                                           randn*processNoiseStd(2),...
                                           randn*processNoiseStd(3),...
                                           randn*processNoiseStd(4)];
    end
end

#### 更新阶段
当获得新的测量数据后，就需要调整各个粒子对应的权值大小。具体来说就是计算每颗粒子与真实观测之间的相似度得分，进而完成权值更新操作。

function update_weights(measurement)
    global particles weights measurementNoiseStd
    
    for i=1:length(weights)
        predictedMeasurement = h_function(particles(i,:)); % 将状态转换成可比较的形式
        
        diff = abs(predictedMeasurement -diff.^2/(2*measurementNoiseStd^2)) / ... 
                     sqrt(2*pi*measurementNoiseStd^2);
        
        weights(i) *= prod(likelihood);
    end
    
    normalizeWeights(); % 归一化处理后的权值之和应等于1
end

#### 调整函数h_function
该辅助函数负责将粒子的状态向量映射到可以同传感器读数相匹配的空间内。例如在一个二维平面上追踪物体时，只需提取出(x,y)坐标即可[^3]。

function z_pred = h_function(x_state)
    z_pred = x_state(1:2); % 只取前两个分量即为位置信息
end

#### 绘图展示结果
最后可以通过绘制图表的方式直观地观察整个过程中真值轨迹、测量点位以及经过粒子滤波估计得到的结果三者间的关系。

plot(trueStates(:,1), trueStates(:,2), 'b-', ...
     measurements(:,1), measurements(:,2), 'r.', ...
     estimatedStates(:,1), estimatedStates(:,2), 'g-o');
legend('True State', 'Measurements', 'Estimated by PF');
xlabel('X Position'), ylabel('Y Position')
title('Particle Filter Tracking Performance')
grid on;