www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > ex/Ex1111.m

    function Particle

% Particle filter 

y = 0; % 初始状态
Q = 1; % 过程噪声协方差
R = 1; % 测量噪声协方差
tf = 50; % 仿真长度

N = 100; % 粒子滤波器粒子数

yhat = y;
P = 2;
yhatPart = y;

% 初始化粒子过滤器
for i = 1 : N
    ypart(i) = y + sqrt(P) * randn;
end

yArr = [y];
rArr = [y + sqrt(R) * randn];
yhatArr = [y];
PArr = [P];
yhatPartArr = [yhatPart];

close all;

for k = 1 : tf
    % 系统仿真
x1=0:10:900;
y1=sqrt(-(x1-450).^2+450^2)+ sqrt(Q) * randn;%状态方程
x2=900:10:1800;
y2=-sqrt(-(x2-1350).^2+450^2)+ sqrt(Q) * randn;%状态方程
x3=1800:2500;
y3=sqrt(Q) * randn;%状态方程
   r = y1 + sqrt(R) * randn;%观测方程
 
   
     %  卡尔曼滤波
   x1=0:10:900;
   F1=(450-x1)/sqrt(-(x1-450).^2+450^2);
   P1 = F1 * P * F1' + Q;
   H1 = yhat / 10;
   K1 = P1 * H1' * inv(H1 * P1 * H1' + R);
   yhat1 = sqrt(-(yhat-450).^2+450^2);%预测
   yhat1 = yhat1 + K1 * (r - yhat);%更新
   P1 = (1 - K1 * H1) * P1;
   
   x2=900:10:1800;
   F2=(x2-1350)/sqrt(-(x2-1350).^2+450^2);
   P2 = F2 * P * F2' + Q;
   H2 = yhat / 10;
   K2 = P2 * H2' * inv(H2 * P2 * H2' + R);
   yhat2 = -sqrt(-(yhat-1350).^2+450^2);%预测
   yhat2 = yhat2 + K2 * (r - yhat);%更新
   P2 = (1 - K2 * H2) * P2;
    
   x3=1800:2500;
   F3=1;
   P3 = F3 * P * F3' + Q;
   H3 = yhat / 10;
   K3 = P3 * H3' * inv(H3 * P3 * H3' + R);
   yhat3 = 0;%预测
   yhat3 = yhat3 + K3 * (r - yhat);%更新
   P3 = (1 - K3 * H3) * P3;
 end
end   
    for i = 1 : N
       x1=0:10:900;
       y1partminus(i)=sqrt(-(ypart(i)-450).^2+450^2)+ sqrt(Q) * randn;
       x2=900:10:1800;
       y2partminus(i)=-sqrt(-(ypart(i)-1350).^2+450^2)+ sqrt(Q) * randn;
       x3=1800:2500;
       y3partminus(i)=sqrt(Q) * randn;
      end
        r1part = y1partminus(i);
        r2part = y2partminus(i);
        r3part = y3partminus(i);
        
        vhat1 = r - r1part;%观测和预测的差
        vhat2 = r - r2part;
        vhat3 = r - r3part;
        vhat = mean((vhat1+vhat2+vhat3)/3)
        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
    yhatPart = mean(ypart);
    yArr = [yArr y];
    rArr = [rArr r];
    yhatArr1 = [y1];yhatArr2 = [y2];yhatArr3 = [y3];
    yhatArr1 = [yhatArr1 yhat1];
    yhatArr2 = [yhatArr2 yhat2];
    yhatArr3 = [yhatArr3 yhat3];
    PArr = [PArr P];
    yhatPartArr = [yhatPartArr yhatPart];
   
    
 t = 0 : tf;
    if k == 20   
    end
end

figure;
plot(t, yArr, 'b.', t,yhatArr1,'r',t,yhatArr2,'r',t,yhatArr3,'r',t, yhatPartArr, 'k-');
xlabel('time step'); ylabel('state');
legend('True state','KF', 'Particle filter estimate');