用matlab写一段粒子滤波用于目标跟踪的代码

以下是一个简单的粒子滤波器的Matlab代码，用于目标跟踪：

% 初始化粒子滤波器
numParticles = 1000;
particles = repmat(struct('x', zeros(2,1), 'w', 1/numParticles), numParticles, 1);

% 初始化目标位置
targetPos = [50; 50];

% 初始化噪声模型
sigmaV = 1;
sigmaW = 10;

% 初始化粒子位置和权重
for i = 1:numParticles
    particles(i).x = [rand*100; rand*100];
    particles(i).w = 1/numParticles;
end

% 开始迭代
for t = 1:100
    % 移动目标
    targetPos = targetPos + sigmaV*randn(2,1);
    
    % 更新粒子位置和权重
    for i = 1:numParticles
        particles(i).x = particles(i).x + sigmaW*randn(2,1);
        particles(i).w = exp(-norm(particles(i).x - targetPos)^2/(2*sigmaV^2));
    end
    
    % 重新采样粒子
    weights = [particles.w];
    [sortedWeights, index] = sort(weights, 'descend');
    cumSumWeights = cumsum(sortedWeights);
    cumSumWeights = cumSumWeights/cumSumWeights(end);
    newParticles = particles(index);
    for i = 1:numParticles
        r = rand;
        j = find(cumSumWeights >= r, 1);
        newParticles(i).x = particles(j).x;
        newParticles(i).w = 1/numParticles;
    end
    particles = newParticles;
    
    % 绘制粒子和目标位置
    clf;
    scatter([particles.x(1,:)], [particles.x(2,:)], 10, 'filled');
    hold on;
    scatter(targetPos(1), targetPos(2), 'r', 'filled');
    axis([0 100 0 100]);
    pause(0.1);
end

在这个例子中，我们使用 `numParticles` 个粒子来估计目标的位置。我们使用高斯噪声模型来模拟目标位置的变化和粒子位置的测量误差。在每个时间步骤中，我们首先移动目标的位置，然后更新每个粒子的位置和权重。我们使用高斯分布来计算粒子的权重，其中均值为目标位置，方差为 `sigmaV`。然后，我们重新采样粒子，以便更好地表示目标位置的不确定性。最后，我们绘制所有粒子的位置和目标位置。

请注意，这是一个非常简单的示例，只为了说明如何使用粒子滤波器进行目标跟踪。在实际应用中，您需要更复杂的模型和更多的粒子来获得更好的结果。