粒子滤波算法matlab实例

粒子滤波（Particle Filter）是一种基于蒙特卡罗方法的滤波算法，主要应用于非线性、非高斯的状态估计问题。下面是一个用MATLAB实现的粒子滤波算法的示例。

首先，我们定义一个非线性的系统模型来模拟实际情况。假设我们要估计一个小车的位置，其状态由位置和速度两个量组成。小车的运动可以用下面的非线性方程描述：

$$
x_k = f(x_{k-1}) + w_k
$$

其中，$x_k$ 表示第 $k$ 个时刻的状态向量，$f$ 是一个非线性函数，$w_k$ 是一个高斯噪声。我们将系统模型表示为MATLAB函数：

function x_k = systemModel(x_km1)
% Nonlinear system model
% x_k: state vector at time k
% x_km1: state vector at time k-1

% System parameters
dt = 0.1; % Time step
a = -0.2; % Acceleration

% State transition function
f = @(x) [x(1)+x(2)*dt+0.5*a*dt^2; x(2)+a*dt];

% Generate noise
w = mvnrnd([0; 0], [0.01, 0; 0, 0.01]);

% Calculate new state vector
x_k = f(x_km1) + w';
end

接下来，我们定义一个观测模型来模拟测量过程。假设我们可以通过GPS定位系统得到小车的位置信息，但是存在一定的误差。我们将观测模型表示为MATLAB函数：

function z_k = observationModel(x_k)
% Observation model
% z_k: observation vector at time k
% x_k: state vector at time k

% Generate noise
v = mvnrnd([0; 0], [0.1, 0; 0, 0.1]);

% Calculate observation vector
z_k = x_k(1) + v(1);
end

现在，我们可以使用上述两个模型来实现粒子滤波算法。下面是MATLAB代码：

% Number of particles
N = 100;

% Initialize particles
x = mvnrnd([0; 0], [1, 0; 0, 1], N)';

% Initialize weights
w = ones(1, N) / N;

% Initialize estimate
x_est = mean(x, 2);

% Number of time steps
T = 100;

% Loop over time steps
for k = 2:T
    % Predict
    x_pred = zeros(2, N);
    for i = 1:N
        x_pred(:, i) = systemModel(x(:, i));
    end
    
    % Update weights
    for i = 1:N
        z_k = observationModel(x_pred(:, i));
        w(i) = w(i) * normpdf(z_k, x_pred(1, i), 0.1);
    end
    w = w / sum(w);
    
    % Resample
    x_resampled = zeros(2, N);
    w_cumsum = cumsum(w);
    for i = 1:N
        j = find(w_cumsum >= rand(), 1);
        x_resampled(:, i) = x_pred(:, j);
    end
    x = x_resampled;
    
    % Estimate state
    x_est = mean(x, 2);
    
    % Plot
    plot(x(1, :), x(2, :), '.', x_est(1), x_est(2), 'o');
    axis([-10, 10, -10, 10]);
    drawnow;
end

在这个代码中，我们首先初始化粒子、权重和估计值。然后，我们在每个时间步骤中执行三个步骤：

1. 预测：使用系统模型预测下一个状态；
2. 更新权重：使用观测模型更新粒子的权重；
3. 重采样：根据权重重新采样粒子。

最后，我们根据粒子的平均值计算状态估计，并将粒子和估计值绘制在图像上。

这就是一个简单的粒子滤波算法的MATLAB实现示例。