www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > ex/Ex11.m
function Particle % Particle filter x = 0.1; % 初始状态 Q = 1; % 过程噪声协方差 R = 1; % 测量噪声协方差 tf = 50; % 仿真长度 N = 100; % 粒子滤波器粒子数 xhat = x; P = 2; xhatPart = x; % 初始化粒子过滤器 for i = 1 : N xpart(i) = x + sqrt(P) * randn; end xArr = [x]; yArr = [x^2 / 20 + sqrt(R) * randn]; xhatArr = [x]; PArr = [P]; xhatPartArr = [xhatPart]; close all; for k = 1 : tf % 系统仿真 x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%状态方程 y = x^2 / 20 + sqrt(R) * randn;%观测方程 for i = 1 : N xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; ypart = xpartminus(i)^2 / 20; vhat = y - ypart;%观测和预测的差 q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); end %正常化的可能性,每个先验估计 qsum = sum(q); for i = 1 : N q(i) = q(i) / qsum;%归一化权重 end % 重采样 for i = 1 : N u = rand; % 均匀随机数介于0和1 qtempsum = 0; for j = 1 : N qtempsum = qtempsum + q(j); if qtempsum >= u xpart(i) = xpartminus(j); break; end end end xhatPart = mean(xpart); % Plot the estimated pdf's at a specific time. if k == 20 % Particle filter pdf pdf = zeros(81,1); for m = -40 : 40 for i = 1 : N if (m <= xpart(i)) && (xpart(i) < m+1) pdf(m+41) = pdf(m+41) + 1; end end end m = -40 : 40; pdf = (1 / sqrt(P) / sqrt(2*pi)) .* exp(-(m - xhat).^2 / 2 / P); end xArr = [xArr x]; yArr = [yArr y]; xhatArr = [xhatArr xhat]; PArr = [PArr P]; xhatPartArr = [xhatPartArr xhatPart]; end t = 0 : tf; figure; plot(t, xArr, 'b.', t, xhatPartArr, 'k-'); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('time step'); ylabel('state'); legend('True state', 'Particle filter estimate');