粒子滤波算法matlab实例

下面是一个简单的粒子滤波算法的 MATLAB 实现示例：

% 参数设置
N = 100; % 粒子数量
T = 20; % 时间步数
sigma = 0.1; % 高斯噪声标准差
Q = 0.1; % 状态转移矩阵噪声协方差
R = 0.1; % 测量噪声协方差

% 生成真实状态和观测值
x = zeros(1, T);
y = zeros(1, T);
x(1) = 0.5;
for t = 2:T
    x(t) = 0.5*x(t-1) + 25*x(t-1)/(1+x(t-1)^2) + 8*cos(1.2*(t-1)) + normrnd(0, sigma);
    y(t) = x(t)^2/20 + normrnd(0, R);
end

% 粒子滤波算法
particles = zeros(N, T);
weights = ones(N, T)/N;
for i = 1:N
    particles(i, 1) = normrnd(x(1), sqrt(sigma));
end
for t = 2:T
    for i = 1:N
        particles(i, t) = normrnd(0.5*particles(i, t-1) + 25*particles(i, t-1)/(1+particles(i, t-1)^2) + 8*cos(1.2*(t-1)), sqrt(Q));
        weights(i, t) = normpdf(y(t), particles(i, t)^2/20, sqrt(R));
    end
    weights(:, t) = weights(:, t)/sum(weights(:, t));
    ess = 1/sum(weights(:, t).^2);
    if ess < N/2
        idx = randsample(N, N, true, weights(:, t));
        particles(:, t) = particles(idx, t);
        weights(:, t) = ones(N, 1)/N;
    end
end

% 绘图
figure;
plot(1:T, x, 'b-', 1:T, y, 'r-', 1:T, mean(particles, 1), 'g--');
legend('真实状态', '观测值', '粒子滤波估计');
xlabel('时间步数');
ylabel('状态值');

在这个示例中，我们生成了一个带有高斯噪声的非线性状态空间模型，并使用粒子滤波算法对其进行估计。结果显示了真实状态、观测值和粒子滤波估计之间的比较。