卫星轨道的估计问题(Matlab)(三):标准重采样粒子滤波(SIR)对新问题的尝试

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:N
    x_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;
end
hold on ;grid on;
plot(1:N,x_real,'b-');%plot(1:N,x_test,'r-');
%PF
for j = 2:N
   X_ = 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:numbers
       x(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));
   end
   w = w/sum(w);
   for i = 1:numbers
       xp(i) = x(find(rand<=cumsum(w),1));
   end
   X_PF(j) = mean(xp); 
end
plot(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:N
    x = 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;
end
X_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:N
    x = 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:num
        particl(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;
    end
    X_pf(:,j) = sumXp/num;
end

可是关键问题在于在尝试到后面发现权值${\omega }_i$在第二次迭代的时候就已经以极快的速度衰减到了$Nan$,并且令循环无法进行下去而报错:

等号右侧的输出数目不足，不满足赋值要求。

出错 SIR (line 76)
       Xp = particl(find(rand <= cumsum(w),1)).xp;   % 粒子权重大的将多得到后代
 

所以说总算是亲自体会到了粒子权重衰减过快带来的巨大影响了。