www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > particle.m
function particle 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 + sqrt(R) * randn]; xhatArr = [x]; PArr = [P]; xhatPartArr = [xhatPart]; close all; for k = 1 : tf % 系统仿真 x = sqrt(40^2-(x-40)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%状态方程 y = x^2 + sqrt(R) * randn;%观测方程 % 卡尔曼滤波 F = (40-xhat)/sqrt(40^2-(xhat-40)^2); P = F * P * F' + Q; H = xhat / 10; K = P * H' * inv(H * P * H' + R); xhat = sqrt(40^2-(xhat-40)^2)+ 8 * cos(1.2*(k-1)) ;%预测 xhat = xhat + K * (y - xhat^2 );%更新 P = (1 - K * H) * P; for i = 1 : N xpartminus(i) = sqrt(40^2-(xpart(i)-40)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; ypart = xpartminus(i)^2 ; 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); xArr = [xArr x]; yArr = [yArr y]; xhatArr = [xhatArr xhat]; PArr = [PArr P]; xhatPartArr = [xhatPartArr xhatPart]; t = 0 : tf; if k == 20 end end figure; plot(t, xArr, 'b.', t,xhatArr,'r',t, xhatPartArr, 'k-'); xlabel('time step'); ylabel('state'); legend('True state','KF', 'Particle filter estimate');