SIR滤波器
关于粒子滤波的基本知识可以参加下面的博客:
粒子滤波,讲的很通俗易懂
基本粒子滤波算法过程:
SIR算法伪代码:
Matlab代码实现
求解如下所示的滤波问题:
%PF的应用clc,clear;close all;N = 300;%总的迭代数numbers = 1000;%粒子数x = 1;Dv = 10; Dn = 1;x_real = zeros(1,N);x_real(1) = 1;y_real = zeros(1,N);y_real(1) = 1/20;x_test = zeros(1,N);x_test(1) = 1.1;y_test = zeros(1,N);y_test(1) = 0.5605;x_PF = zeros(1,N);x_PF(1) = 1.1;y_PF = zeros(1,N);y_PF(1) = 0.5605;%仿真for j = 2:Nx_real(j) = x_real(j-1)/2 +25*x_real(j-1)/(1+x_real(j-1)^2)+8*cos(1.2*(j-1));y_real(j) = x_real(j)^2/20;x_test(j) = x_test(j-1)/2 +25*x_test(j-1)/(1+x_test(j-1)^2)+8*cos(1.2*(j-1))+ sqrt(Dv)*randn;y_test(j) = x_test(j)^2/20 + sqrt(Dn)*randn;endhold on ;grid on;plot(1:N,x_real,'b-');%plot(1:N,x_test,'r-');%PFfor j = 2:NX_ = x_PF(j-1)/2 +25*x_PF(j-1)/(1+x_PF(j-1)^2)+8*cos(1.2*(j-1)); for i = 1:numbersx(i) = X_+ sqrt(Dv)*randn;y_(i) = x(i)^2/20;w(i) = 1/sqrt(2*pi*Dn)*exp(-(y_test(j)-y_(i))^2/(2*Dn));endw = w/sum(w);for i = 1:numbersxp(i) = x(find(rand<=cumsum(w),1));endX_PF(j) = mean(xp); endplot(1:N,X_PF,'g-');legend('真实值','测量值','滤波后的值');xlabel('次数')
应用于新的问题
% 卫星轨道的估计问题(Matlab)(三):粒子滤波(SIR)对新问题的尝试clc,clear;rng(8);%设置部分月球卫星的部分参数G = 6.67259*10^(-11);M = 5.965*10^(24);a = 3.86*10^8;x0 = [a;0;0;2*pi/28/3600/24];GM = G*M;day = 24*3600;tspan = 0:day/24:28*day;N = 30;M = 4;M2 = 2;%以下将求误差协方差矩阵delta_Q = 1*10^8;Q = delta_Q*diag([1,0.5]);h = day;delta_R = 1*10^8;%测量误差由于混合了状态估计的噪声影响,其数量级也应该在100000km左右R = delta_R*diag([0.5,1]);%以下对卫星轨道进行仿真操作X_real = zeros(M,N);Z_real = zeros(M2,N);w = zeros(M,N);v = zeros(M2,N);X_real(:,1) = x0;Z_real(:,1) = [x0(1);x0(3)]; for j = 2:Nx = X_real(:,j-1);f = [x(1)+h*x(2);...x(2)+h*x(1)*(x(4))^2-h*GM/(x(1))^2;...x(3)+h*x(4);...x(4)-2*h*x(2)*x(4)/x(1)];g = [0,0;h,0;0,0;0,h/x(1)];H = [1 0 0 0;0 0 1 0];w(:,j) = g*sqrtm(Q)*randn(2,1);v(:,j) = sqrtm(R)*randn(2,1);X_real(:,j) = f;Z_real(:,j) = H*x;endX_test = X_real + w;Z_test = Z_real + v;figure(1)hold on;grid on;plot(Z_real(1,:).*cos(Z_real(2,:)),Z_real(1,:).*sin(Z_real(2,:)),'ro');plot(Z_test(1,:).*cos(Z_test(2,:)),Z_test(1,:).*sin(Z_test(2,:)),'b*');%下面将进行SIR滤波X_pf = zeros(M,N);Z_pf = zeros(M2,N);X_pf(:,1) = X_test(:,1);Z_pf(:,1) = Z_test(:,1);num = 100;%有100个粒子w = ones(1,num)/num;particl(1).xp = zeros(1,num);for j = 2:Nx = X_pf(:,j-1);x_ = [x(1)+h*x(2);...x(2)+h*x(1)*(x(4))^2-h*GM/(x(1))^2;...x(3)+h*x(4);...x(4)-2*h*x(2)*x(4)/x(1)];g = [0,0;h,0;0,0;0,h/x(1)];H = [1 0 0 0;0 0 1 0];%以下对所有粒子进行采样和赋权值操作for i = 1:numparticl(i).xp = x_ + g*(mvnrnd([0,0],Q,1))';w(i) = mvnpdf(Z_test(:,j)-H*(particl(i).xp),[0;0],R);end%权值归一化w = w/sum(w);%以下是重采样(轮盘赌)sumXp = 0; for i = 1:num Xp = particl(find(rand <= cumsum(w),1)).xp; % 粒子权重大的将多得到后代 sumXp = sumXp + Xp;endX_pf(:,j) = sumXp/num;end
可是关键问题在于在尝试到后面发现权值ωi\omega_iωi在第二次迭代的时候就已经以极快的速度衰减到了NanNanNan,并且令循环无法进行下去而报错:
等号右侧的输出数目不足，不满足赋值要求。出错 SIR (line 76)Xp = particl(find(rand <= cumsum(w),1)).xp; % 粒子权重大的将多得到后代
所以说总算是亲自体会到了粒子权重衰减过快带来的巨大影响了。
