crlbcompute.m

文件夹中NPFMain.m为滤波算法主运行程序

%运行此程序文件的前提是先载入由NPFMain.m运行所得到的mat文件
%本程序功能是画出CRLB、EKF、NPF滤波误差均方差
clc
Q1=q*[T^3/3, T^2/2, 0,       0;
      T^2/2, T,     0,       0;
      0,     0,     T^3/3,   T^2/2;
      0,     0,     T^2/2,   T];
RealState(5,:)=40000;
Phi2=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1];
G2=[T^2/2, T, 0, 0; 0, 0, T^2/2, T]'; 
H2=[1, 0, 0, 0; 0, 0, 1, 0];

%求克拉美-罗下限(CRLB)
SigmaD2=SigmaR^2*RealState(1,2)^2/(RealState(1,2)^2+RealState(3,2)^2)+SigmaA^2*RealState(3,2)^2;
SigmaH2=SigmaR^2*RealState(3,2)^2/(RealState(1,2)^2+RealState(3,2)^2)+SigmaA^2*RealState(1,2)^2;
SigmaDH2=(SigmaR^2/(RealState(1,2)^2+RealState(3,2)^2)-SigmaA^2)*RealState(1,2)*RealState(3,2);
P00   =  [SigmaD2,    SigmaD2/T,      SigmaDH2,    SigmaDH2/T;
          SigmaD2/T, 2*SigmaD2/(T^2), SigmaDH2/T, 2*SigmaDH2/(T^2);
          SigmaDH2,   SigmaDH2/T,     SigmaH2,    SigmaH2/T;
          SigmaDH2/T,2*SigmaDH2/(T^2),SigmaH2/T, 2*SigmaH2/(T^2) ];
J(:,:,2)=inv(P00);
CRLB(1,2)=P00(1,1);
CRLB(2,2)=P00(2,2);
CRLB(3,2)=P00(3,3);
CRLB(4,2)=P00(4,4);

for PtNum=3:PtAcc
    EstBeta=40000+normrnd(0,SigmaBeta);
    SigmaDSquare=SigmaR^2*RealState(1,PtNum)^2/(RealState(1,PtNum)^2+RealState(3,PtNum)^2)+SigmaA^2*RealState(3,PtNum)^2;
    SigmaHSquare=SigmaR^2*RealState(3,PtNum)^2/(RealState(1,PtNum)^2+RealState(3,PtNum)^2)+SigmaA^2*RealState(1,PtNum)^2;
    SigmaDH=(SigmaR^2/(RealState(1,PtNum)^2+RealState(3,PtNum)^2)-SigmaA^2)*RealState(1,PtNum)*RealState(3,PtNum);
    R=[SigmaDSquare, SigmaDH;
           SigmaDH,     SigmaHSquare];
    F=Phi2+G2*fDiv(RealState(:,PtNum-1));
    J(:,:,PtNum)=inv(Q1)+H'*inv(R)*H-inv(Q1)*F*inv(J(:,:,PtNum-1)+F'*inv(Q1)*F)*F'*inv(Q1);
    k=inv(J(:,:,PtNum));
    CRLB(1,PtNum)=k(1,1);
    CRLB(2,PtNum)=k(2,2);
    CRLB(3,PtNum)=k(3,3);
    CRLB(4,PtNum)=k(4,4);
end


figure(1),
subplot(2,2,1);
plot(time,EKFFilErrorMean(1,3:PtAcc),'r*--',time,NPFFilErrorMean(1,3:PtAcc),'kx:',...
    time,MeaErrorMean(1,3:PtAcc),'bo-');
xlabel('跟踪时间/s');
ylabel('X方向距离误差均方差/m');
legend('EKF滤波','NPF滤波','观测');
grid on;
subplot(2,2,2);
plot(time,EKFFilErrorMean(2,3:PtAcc),'r*--',time,NPFFilErrorMean(2,3:PtAcc),'kx:');
xlabel('跟踪时间/s');
ylabel('X方向速度误差均方差/m');
legend('EKF滤波','NPF滤波');
grid on;
subplot(2,2,3);
plot(time,EKFFilErrorMean(3,3:PtAcc),'r*--',time,NPFFilErrorMean(3,3:PtAcc),'kx:',...
    time,MeaErrorMean(2,3:PtAcc),'bo-');
xlabel('跟踪时间/s');
ylabel('Y方向距离误差均方差/m');
legend('EKF滤波','NPF滤波','观测');
grid on;
subplot(2,2,4);
plot(time,EKFFilErrorMean(4,3:PtAcc),'r*--',time,NPFFilErrorMean(4,3:PtAcc),'kx:');
xlabel('跟踪时间/s');
ylabel('Y方向速度误差均方差/m');
legend('EKF滤波','NPF滤波');
grid on;
figure(2),
subplot(2,2,1);
plot(time,EKFFilErrorVar(1,3:PtAcc),'r*-',time,NPFFilErrorVar(1,3:PtAcc),'kx:',...
    time,MeaErrorVar(1,3:PtAcc),'bo-',time,sqrt(CRLB(1,3:PtAcc)),'gd-.');
xlabel('跟踪时间/s');
ylabel('X方向距离误差均方差/m');
legend('EKF滤波','NPF滤波','观测','CRLB');
grid on;
subplot(2,2,2);
plot(time,EKFFilErrorVar(2,3:PtAcc),'r*--',time,NPFFilErrorVar(2,3:PtAcc),'kx:',...
    time,sqrt(CRLB(2,3:PtAcc)),'gd-.');
xlabel('跟踪时间/s');
ylabel('X方向速度误差均方差/m');
legend('EKF滤波','NPF滤波','CRLB');
grid on;
subplot(2,2,3);
plot(time,EKFFilErrorVar(3,3:PtAcc),'r*--',time,NPFFilErrorVar(3,3:PtAcc),'kx:',...
    time,MeaErrorVar(2,3:PtAcc),'bo-',time,sqrt(CRLB(3,3:PtAcc)),'gd-.');
xlabel('跟踪时间/s');
ylabel('Y方向距离误差均方差/m');
legend('EKF滤波','NPF滤波','观测','CRLB');
grid on;
subplot(2,2,4);
plot(time,EKFFilErrorVar(4,3:PtAcc),'r*--',time,NPFFilErrorVar(4,3:PtAcc),'kx:',...
    time,sqrt(CRLB(4,3:PtAcc)),'gd-.');
xlabel('跟踪时间/s');
ylabel('Y方向速度误差均方差/m');
legend('EKF滤波','NPF滤波','CRLB');
grid on;