粒子滤波中重采样算法matlab代码
时间: 2023-09-15 16:21:15 浏览: 43
以下是一个简单的粒子滤波器重采样算法的Matlab代码示例:
% 粒子滤波器重采样算法示例
% 模拟一个简单的一维状态空间模型,其中状态是速度,观测是位置
% 假设我们只能通过GPS测量位置,速度的初始值为0
% 模拟真实状态轨迹
T = 100; % 总时间步数
dt = 0.1; % 时间步长
a = 0.2; % 加速度
v = zeros(1,T); % 速度
x = zeros(1,T); % 位置
for t=2:T
v(t) = v(t-1) + a*dt;
x(t) = x(t-1) + v(t)*dt;
end
% 模拟GPS观测
sigma = 1.0; % 观测误差标准差
y = x + sigma*randn(1,T); % 观测位置
% 初始化粒子
N = 1000; % 粒子数
x_particles = zeros(N,T); % 状态粒子
w_particles = ones(N,T)/N; % 权重粒子
x_particles(:,1) = zeros(N,1); % 初始速度为0
% 粒子滤波主循环
for t=2:T
% 预测
x_particles(:,t) = x_particles(:,t-1) + a*dt + sigma*randn(N,1); % 状态预测
% 更新
w_particles(:,t) = w_particles(:,t-1) .* normpdf(y(t), x_particles(:,t), sigma); % 权重更新
w_particles(:,t) = w_particles(:,t) / sum(w_particles(:,t)); % 权重归一化
% 重采样
if 1/sum(w_particles(:,t).^2) < N/2 % 判断是否需要重采样
idx = randsample(N,N,true,w_particles(:,t)); % 重采样
x_particles(:,t) = x_particles(idx,t);
w_particles(:,t) = ones(N,1)/N;
end
end
% 绘制粒子滤波结果
figure;
hold on;
plot(1:T, x, '-k', 'LineWidth', 2);
plot(1:T, y, 'ob', 'MarkerSize', 6);
plot(1:T, x_particles, '.r', 'MarkerSize', 2);
legend('真实状态', 'GPS观测', '粒子滤波结果');
xlabel('时间');
ylabel('状态');
title('粒子滤波结果');
grid on;