粒子滤波的仿真.docx

v % Particle filter example, adapted from Gordon, Salmond, and Smith paper. x= ; % initial state Q= 1;% process noise covariance R= 1;% measurement noise covariance tf= 50; % simulation length N= 100; % number of particles in the particle filter xhat = x;% 有疑问 P= 2;% 有疑问 xhatPart = x;% 有疑问% Initialize the particle filter. for i=1:N xpart(i) =x+ sqrt(P) * randn; % 产生 100 个期望值为 x ,方差为 p ,的随机数, 这 100 个随机数满足正太分布 end xArr = [x]; yArr = [x^2 / 20 + sqrt(R) * randn]; xhatArr = [x]; PArr = [P]; xhatPartArr = [xhatPart]; close all; % 关闭程序运行产生的所有窗口。 for k=1: tf% System simulation x= *x+ 25 *x/ (1+ x^2) +8* cos(*(k-1)) + sqrt(Q) * randn;% 状态方程 y= x^2 / 20 + sqrt(R) * randn;% 观测方程% Extended Kalman filter F= + 25 * (1- xhat^2) / (1+ xhat^2)^2; P=F*P* F'+ Q; H= xhat / 10; K=P* H'* (H*P* H'+ R)^(-1); xhat = * xhat + 25 * xhat / (1+ xhat^2) +8* cos(*(k-1));% 预测 xhat = xhat +K* (y- xhat^2 / 20);% 更新 P= (1-K* H)* P; % Particle filter 粒子滤波从这开始 for i=1:N xpartminus(i) = * xpart(i) + 25 * xpart(i) / (1+ xpart(i)^2) +8* cos(*(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 % Normalize the likelihood of each a priori estimate. 规范每个先验估计的可能性 qsum = sum(q); for i=1:N q(i) = q(i) / qsum;% 归一化权重 end % Resample. for i=1:Nu= rand; % uniform random number between 0 and 1 qtempsum = 0; for j=