pda.m

根据《雷达数据处理及应用》一书中的场景编写的PDA算法

clc;
clear;
close all;
T=1;
N=100;
A=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
C=[1 0 0 0;0 0 1 0];
I=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
Q=0;
R11=200;R22=200;R12=0;R21=0;
R=[R11 R12;R21 R22];
X0=[200;0;10000;-15];
%**************
for i=1:N 
    Vk=[sqrt(R11)*randn(1,1);sqrt(R22)*randn(1,1)];
    Zk=C*X0+Vk;                        %***********生成量测***************%
    x(i)=X0(1);                           %***********理想x方向位置**********%
    vx(i)=X0(2);                          %***********理想x方向速度**********%
    y(i)=X0(3);                           %***********理想y方向位置**********%
    vy(i)=X0(4);                          %***********理想y方向速度**********%
    zx(i)=Zk(1);                          %***********x方向为量测位移********%
    zy(i)=Zk(2);                          %***********y方向为量侧位移********%   
    X0=A*X0;
end
    gama=16;
    lamda=0.0004;
    Pd=1;Pg=0.9997;
    %**************滤波初值***************%
Xt=[zx(2);(zx(2)-zx(1))/T;zy(2);(zy(2)-zy(1))/T];
Pkk=[R11 R11/T R12 R12/T;R11/T 2*R11/T^2 R12/T 2*R12/T^2;R21 R21/T R22 R22/T;R21/T 2*R21/T^2 R22/T 2*R22/T^2];
for i=1:10
    X_k=A*Xt;
    P_k=A*Pkk*A';
    K=P_k*C'*inv(C*P_k*C'+R);
    Xt=X_k+K*([zx(i);zy(i)]-C*X_k);
    Pkk=P_k-P_k*C'*inv(C*P_k*C'+R)*C*P_k;
    
    
    Ex(i)=Xt(1);
Evx(i)=Xt(2);
Ey(i)=Xt(3);
Evy(i)=Xt(4);
error(i)=Pkk(1);
error2(i)=Pkk(6);
error3(i)=Pkk(11);
error4(i)=Pkk(16);
end
for i=11:N

    P_k=A*Pkk*A';                      %P_k为P（K+1|K）,Pkk为P(K|K)
    X_k=A*Xt;                                 %X_k为一步预测X(k+1|k)
    Z_k=C*X_k;                                 %Z_k为一步预测Z(k+1|k)
    Sk=C*P_k*C'+ R;
    %量测确认区域面积
    Av=pi*gama*sqrt(det(Sk));
    %准备生成杂波数目
    nc=floor(10*Av*lamda+1);
    disp(nc);
    q=sqrt(10*Av)/2;

    a=x(i)-q;
    b=x(i)+q;
    c=y(i)-q;
    d=y(i)+q;

    %*************生成包含杂波的nc个虚假量测********************%
    xi=a+(b-a)*rand(1,nc); 
    yi=c+(d-c)*rand(1,nc);
    %*********************************************************%
    
    vis=[];
    mk=0;
    for j=1:nc
        zi=[xi(j);yi(j)];
        vi=zi-Z_k;
         
        if vi'*inv(Sk)*vi<=gama
            vis=[vis,vi];
            mk=mk+1;
        end
      
    end
      real=[zx(i)-Z_k(1);zy(i)-Z_k(2)];
        vis=[vis,real];
        mk=mk+1;
   %disp(mk);
    if mk==0
        Xt=X_k;
        Pkk=P_k;
    else 
    bk=lamda*(2*pi)*det(Sk)*(1-Pd*Pg)/Pd;
    %***********通过波门选取候选回波**************%
    e=[];
    for j=1:mk
        vi=vis(:,j);
        e(j)=exp((-1/2)*vi'*inv(Sk)*vi);
    end
    %*******************************************
    beta_0=bk/sum(e);
    beta_i=e./sum(e);

%     disp(beta_i);
    zkk1=beta_i.*vis(1,:);
    zkk2=beta_i.*vis(2,:);
    zkk=[sum(zkk1);sum(zkk2)];
    op=[0 0;0 0];
    for u=1:mk
        op1=vis(:,u)*(vis(:,u))';
        op=op1*beta_i(u)+op;
    end
    K=P_k*C'*inv(Sk);
    Xt=X_k+K*zkk;
    PK=K*(op-zkk*zkk')*K';
    Pkk=beta_0*P_k+(1-beta_0)*(I-K*C)*P_k+PK;

end
Ex(i)=Xt(1);
Evx(i)=Xt(2);
Ey(i)=Xt(3);
Evy(i)=Xt(4);
error(i)=Pkk(1);
error2(i)=Pkk(6);
error3(i)=Pkk(11);
error4(i)=Pkk(16);
end

%****************画图*******************
i=1:N;
plot(i,Ex(i),'r:',i,x(i),'m--',i,zx(i),'b');
legend('最优估计值','真值','测量值');
xlabel('x方向位移    迭代次数');

figure,plot(i,Evx(i),'r:',i,vx(i),'m--');
legend('最优估计值','真值');
xlabel('x方向速度     迭代次数');

figure,plot(i,Ey(i),'r:',i,y(i),'m--',i,zy(i),'b');
legend('最优估计值','真值','测量值');
xlabel('y方向位移    迭代次数');

figure,plot(i,Evy(i),'r:',i,vy(i),'m--');
legend('最优估计值','真值');
xlabel('y方向速度   迭代次数');
figure,plot(i,error(i),'r');
xlabel('x方向位移误差斜方差');
figure,plot(i,error2(i),'r');
xlabel('x方向速度误差斜方差');
figure,plot(i,error3(i),'r');
xlabel('y方向位移误差斜方差');
figure,plot(i,error4(i),'r');
xlabel('y方向速度误差斜方差');