www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > ex/Particle.m
function Particle % Particle filter x = 0.1; % 初始状态 Q = 50; % 过程噪声协方差 R = 50; % 测量噪声协方差 tf1 = 100; % 仿真长度 tf = 150; N = 50; % 粒子滤波器粒子数 xhat = x; P = 2; xhatPart = x; % 初始化粒子过滤器 for i = 1 : N xpart(i) = x + sqrt(P) * randn; end xArr = [x]; yArr = [-x^2 + sqrt(R) * randn]; xhatArr = [x]; PArr = [P]; xhatPartArr = [xhatPart]; close all; for k =1:tf1; % 系统仿真 x = -(x-50)^2 + 5*k + sqrt(Q) * randn;%状态方程 y = -x^2 + sqrt(R) * randn;%观测方程 % 卡尔曼滤波 F = -2*(x-50) ; P = F * P * F' + Q; H = -xhat^2 ; K = P * H' * inv(H * P * H' + R); xhat = -(xhat-50)^2+5*k;%预测 xhat = xhat + K * (y + xhat^2);%更新 P = (1 - K * H) * P; for i = 1 : N xpartminus(i) = -(xpart(i) - 50)^2 + 5*k + sqrt(Q) * randn; ypart = -(xpartminus(i))^2; vhat = y - ypart;%观测和预测的差 vhat0=sqrt(y.^2-ypart.^2); q(i) = (1 / (sqrt(R^2) * sqrt(2*pi))) * exp(-vhat^2 /( 2 * R^2)); 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); xArr = [xArr x]; yArr = [yArr y]; xhatArr = [xhatArr xhat]; PArr = [PArr P]; xhatPartArr = [xhatPartArr xhatPart]; x0=100; xhat1 = x0; xhatPart1 = x0; % 初始化粒子过滤器 for i = 1 : N xpart1(i) = x0 + sqrt(P) * randn; end xArr1 = [x0]; yArr1 = [3*x0 + sqrt(R) * randn]; xhatArr1 = [x0]; xhatPartArr1 = [xhatPart1]; close all; % 系统仿真 x1 = 3*(x0+1) + sqrt(Q) * randn;%状态方程 y1 = 3*x1+ sqrt(R) * randn;%观测方程 % 卡尔曼滤波 F1 = 3 ; P1 = F1 * P * F1' + Q; H1 = 3*xhat1; K1 = P1* H1' * inv(H1 * P1 * H1' + R); xhat1 = 3 * (xhat1+1) ;%预测 xhat1 = xhat1 + K1 * (y1 - 3*xhat1);%更新 P1 = (1 - K1 * H1) * P1; for i = 1 : N xpartminus1(i) = 3* (xpart1(i)+1) + sqrt(Q) * randn; ypart1 = 3*(xpartminus1(i)); vhat1 = y1 - ypart1;%观测和预测的差 vhat00=sqrt(y1.^2-ypart1.^2); q1(i) = (1 / (sqrt(R^2) * sqrt(2*pi))) * exp(-vhat1^2 /( 2 * R^2)); end %正常化的可能性,每个先验估计 qsum = sum(q1); for i = 1 : N q1(i) = q1(i) / qsum;%归一化权重 end % 重采样 for i = 1 : N u = rand; % 均匀随机数介于0和1 qtempsum = 0; for j = 1 : N qtempsum = qtempsum + q(j); if qtempsum >= u xpart1(i) = xpartminus1(j); break; end end xhatPart1 = mean(xpart1); xArr1 = [xArr1 x1]; yArr1 = [yArr1 y1]; xhatArr1 = [xhatArr1 xhat1]; PArr = [PArr P]; xhatPartArr1 = [xhatPartArr1 xhatPart1]; t1 = 0 : tf1; t2=100:150; end end figure; plot(t1, xArr, 'b.', t2, xArr1, 'b.',t1,xhatArr,'r',t2,xhatArr1,'r',t1, xhatPartArr, 'k-',t2,xhatPartArr1, 'k-'); xlabel('time step'); ylabel('state'); legend('True state','True state', 'KF', 'KF', 'Particle filter estimate'); figure; m1=1:tf1;m2=100:150; subplot(2,1,1),plot(m1,vhat0); subplot(2,1,2),plot(m2,vhat00);