ex3210.asv

本程序是基于机动目标跟踪课题的整个算法程序

function Particle

% Particle filter 

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

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

xhat = x;
P = 2;
xhatPart = x;

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

xArr = [x];
yArr = [x + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];

close all;

for k = 1 : tf
    % 系统仿真
    t=0:180;
    t1=20000/300;
    t2=t1+200*pi/20;
    if  t>=0&&t<=t1
        x=0;
elseif t>t1&&t<=t2
    x=sqrt(562.5^2-(x+4500)^2)+ sqrt(Q) * randn;
else
    x=-9000;
end
    y = x + sqrt(R) * randn;%观测方程
     %  卡尔曼滤波
     if t>t1&&t<=t2
    F = (x+4500)/sqrt(562.5^2-(x+4500)^2);
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * inv(H * P * H' + R);
    xhat = sqrt(562.5^2-(xhat+4500)^2);%预测
    xhat = xhat + K * (y - xhat);%更新
    P = (1 - K * H) * P;
else F=0;
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * inv(H * P * H' + R);
    xhat = sqrt(562.5^2-(xhat+4500)^2);%预测
    xhat = xhat + K * (y - xhat);%更新
    P = (1 - K * H) * P;
   
    for i = 1 : N
        xpartminus(i) = sqrt(562.5^2-(xpartminus+4500)^2) + sqrt(Q) * randn;
        ypart = xpartminus(i);
        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');