untitled.asv

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

x1=0:10:900;
y1=sqrt(-(x1-450).^2+450^2);
x2=900:10:1800;
y2=-sqrt(-(x2-1350).^2+450^2);
x3=1800:3000;
y3=0;
figure;
plot(x1,y1,'k',x2,y2,'k',x3,y3,'k');

x = 0; % 初始状态
Q = 1; % 过程噪声协方差
R = 1; % 测量噪声协方差
tf = 3000; % 仿真长度
N = 500; % 粒子滤波器粒子数

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
    % 系统仿真
    t1=0:900;
    x1=sqrt(-(x1-450).^2+450^2);
    y = x + sqrt(R) * randn;%观测方程
    %  卡尔曼滤波
    F = (450-x1)/sqrt(-(x1-450).^2+450^2);
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * inv(H * P * H' + R);
    xhat = sqrt(-(x1-450).^2+450^2);%预测
    xhat = xhat + K * (y - xhat);%更新
    P = (1 - K * H) * P;
    for i = 1 : N
        xpartminus(i) =sqrt(-(xpart(i)-450).^2+450^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
    xhatPart1 = mean(xpart);
    xArr1 = [xArr x];
    yArr1 = [yArr y];
    xhatArr1 = [xhatArr xhat];
    PArr1 = [PArr P];
    xhatPartArr1 = [xhatPartArr xhatPart];
   
    
    t2=900:1800;
    x2=-sqrt(-(x2-1350).^2+450^2);
    y = x + sqrt(R) * randn;%观测方程
    %  卡尔曼滤波
    F = (x2-1350)/sqrt(-(x2-1350).^2+450^2);
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * inv(H * P * H' + R);
    xhat = sqrt(-(x2-450).^2+450^2);%预测
    xhat = xhat + K * (y - xhat);%更新
    P = (1 - K * H) * P;
    for i = 1 : N
        xpartminus(i) =-sqrt(-(xpart(i)-1350).^2+450^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
    xhatPart2 = mean(xpart);
    xArr2 = [xArr x];
    yArr2 = [yArr y];
    xhatArr2 = [xhatArr xhat];
    PArr2 = [PArr P];
    xhatPartArr2 = [xhatPartArr xhatPart];
    
    t3=1800:3000;
    x3=0;
    y = x + sqrt(R) * randn;%观测方程
     %  卡尔曼滤波
    F = 1;
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * inv(H * P * H' + R);
    xhat = x3;%预测
    xhat = xhat + K * (y - xhat);%更新
    P = (1 - K * H) * P;
    for i = 1 : N
        xpartminus(i) =xpart(i) + 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
    xhatPart3 = mean(xpart);
    xArr3 = [xArr x];
    yArr3 = [yArr y];
    xhatArr3 = [xhatArr xhat];
    PArr3 = [PArr P];
    xhatPartArr3 = [xhatPartArr xhatPart];
    
    t1 = 0 : 900;t2=900:1800;t3=1800:3000
    if k == 20   
    end
end

figure;
plot(t1, xArr, 'b.',t2, xArr, 'b.',t3, xArr, 'b.');