最近邻域标准滤波器（NNSF）和概率数据互联滤波器（PDAF）的航迹绘制和对比

最近邻域标准滤波器（NNSF）和概率数据互联滤波器（PDAF）的航迹绘制和对比

clc;clear
format long;close all;
for l=1:6
T=2;x（:1）=[10 1 10 -15]‘;       %初始状态
F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];H=[1 0 0 0;0 0 1 0];R=[400 0;0 400];
for k=1:99
    x（:k+1）=F*x（:k）;
    Z（:k）=H*x（:k+1）+randn*[20 20]‘;    
end
X（:1）=[Z（11） （Z（11）-x（11））/T Z（21） （Z（21）-x（31））/T]‘;    %系统初始状态
%初始协方差矩阵
P（:1:4）=[R（11） R（11）/T R（12） R（12）/T;R（11）/T 2*R（11）/（T^2） R（12）/T 2*R（12）/（T^2）;
    R（12） R（12）/T R（22） R（22）/T;R（12）/T 2*R（12）/（T^2） R（22）/T 2*R（22）/（T^2）];
X_temp=X（:1）;
ZZ（:1）=H*X（:1）;
P0=P（:1:4）;
tao=[0.5*T*T 0;T 0;0 0.5*T*T;0 T];              %过程噪声分布矩阵
Q=[16 0;0 9];                                %过程噪声
for k=1:9
    X（:k+1）=F*X_temp;                          %一步状态预测 
    ZZ（:k+1）=H*X（:k+1）;
    P（:（4*k+1）:（4*k+4））=F*P0*F‘+tao*Q*tao‘;    %一步预测协方差    
    S（:（（2*k-1）:（2*k）））=H*P（:（4*k+1）:（4*k+4））*H‘+R;              %新息
    s=S（:（（2*k-1）:（2*k）））;
    K=P（:（4*k+1）:（4*k+4））*H‘*s^（-1）;           %增益
    X_temp=X（:k+1）+K*[Z（:k）-H*X（:k+1）];   %状态更新
    P0=（eye（4）-K*H）*P（:（4*k+1）:（4*k+4））*（eye（4）+K*H）‘-K*R*K‘;      %协方差更新
end
gama=16;lamda=0.00002;               
PD=1;PG=0.9997;                     %检测概率与门概率
for k=10:99
    X（:k+1）=F*X_temp;                          %一步状态预测
    ZZ（:k+1）=H*X（:k+1）;
    P（:（4*k+1）:（4*k+4））=F*P0*F‘+tao*Q*tao‘;    %一步预测协方差    
    S（:（（2*k-1）:（2*k）））=H*P（:（4*k+1）:（4*k+4））*H‘+R;              %新息
    s=S（:（（2*k-1）:（2*k）））;
    K=P（:（4*k+1）:（4*k+4））*H‘*s^（-1）;           %增益    
    Av=pi*gama*sqrt（det（s））;        %二维正方形波门面积
    nc=round（10*Av*lamda+1）;        %虚假量测总数
    j=0;
    q=sqrt（10*Av）/2;
    z_temp=Z（:k）;
    for i=1:nc
        a=Z（1k）-q;b=Z（1k）+q;
        c=Z（2k）-q;d=Z（2k）+q;
        xx（i）=a+（b-a）*rand;
        y（i）=c+（d-c）*rand;
        zz_temp=[xx（i） y（i）]‘;      %虚假量测
        z_temp=[z_temp zz_temp];    %虚假量测和真实量测在内的所有量测    
    end
    for i=1:nc+1
        v_temp（:i）=z_temp（:i）-ZZ（:k+1）;
        %判断落入波门内的量测
        if （v_temp（:i）‘*s^（-1）*v_temp（:i））<=gama
            j=j+1;
            z（:j）=z_temp（:i）;
            v（:j）=z（:j）-ZZ（:k+1）;
        end
    end
    sum=0;b0=lamda*sqrt（det（2*pi*s））*（1-PD*PG）/PD;
    for i=1:j
        e（i）=exp（-0.5*v（:i）‘*s^（-1）*v（:i））;        
        sum=sum+e（i）;
    end
    beta0=b0/（b0+sum）;    sum1=[0;0];sum2=[0 0;0 0];
    for i=1:j
        beta（i）=e（i）/（b0+sum）;
        sum1=sum1+beta（i）*v（:i）;               %组合新息
        sum2=sum2+beta（i）*v（:i）*v（:i）‘;
    end
    X_temp=X（:k+1）+K*sum1;   %状态更新
    P0=beta0*P（:（4*k+1）:（4*k+4））+（1-beta0）*（eye（4）-K*H）*P（:（4*k+1）:（4*k+4））+K*（sum2-sum1*sum1‘）*K‘;      %协方差更新
end
figure（l）;plot（X（1:）X（3:）‘--r‘）;grid on; hold on;
plot（x（1:）x（3:）‘.k‘）;hold on;
plot（Z（1:）Z（2:）‘xb‘）;xlabel（‘X‘）;ylabel（‘Y‘）;hold on;
clear;
T=2;x（:1）=[10 1 10 -15]‘;       %初始状态
F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];H=[1 0 0 0;0 0 1 0];
R=[400 0;0 400];
for k=1:99
    x（:k+1）=F*x（:k）;
    Z（:k）=H*x（:

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5707  2014-05-06 07:54  NNSF.m

----------- ---------  ---------- -----  ----

                 5707                    1