lmukfvsekf_1.m

ekf和ukf的程序的对比

%目标匀速直线运动（速度受噪声驱动）下（距离+方位角)观测的UKF滤波跟踪算法
clear all
N=120;  %观测次数
T=1;    %观测周期
F=[1 0 T 0;               %目标运动状态转移矩阵
   0 1 0 T;
   0 0 1 0;
   0 0 0 1];
v1=normrnd(0,0.1,[1,N]);   %目标在x速度方向上的噪声
v2=normrnd(0,0.1,[1,N]);   %目标在y速度方向上的噪声
Q=[0 0 0      0     ;  %状态噪声协方差
   0 0 0      0     ;
   0 0 0.1^2 0     ;
   0 0 0      0.1^2];
Obj{1}=[300 -5 -2+v1(1) 2+v2(1)]'; %目标初始状态：[x坐标 y坐标 x速度 y速度]'
for k=2:N
    Obj{k}=F*Obj{k-1}+[0 0 v1(k) v2(k)]';
end
w1=normrnd(0,sqrt(0.1),[1,N]);  %产生随即观测噪声:距离
w2=normrnd(0,0.1,[1,N]);        %产生随即观测噪声:角度
C=[0.1 0   ;    %观测噪声协方差
   0   0.01];
%UKF
Sobj{1}=[320 15 0 0]';  %初始状态估计量
Mobj{1}=100*eye(4);     %初始协方差
for k=1:N
    Zreal{k}=[sqrt(Obj{k}(1,1)^2+Obj{k}(2,1)^2),atan(Obj{k}(2,1)/Obj{k}(1,1))]';%真实测量值
    Zmsr{k}=Zreal{k}+[w1(k),w2(k)]';   %实际测量值
end
%滤波算法
for k=1:N
    SobjF{k}=F*Sobj{k};       %k时刻状态预测值
    MobjF{k}=F*Mobj{k}*F'+Q;  %k时刻协方差预测
    %UT变换求量测方程对状态的传播
    [ZmsrF{k},ZZcov{k},XZcov{k}]=UnscentedT(SobjF{k},MobjF{k});
    ZZcov{k}=ZZcov{k}+C;    
    W{k}=XZcov{k}*inv(ZZcov{k});     %滤波增益
    Sobj{k+1}=SobjF{k}+W{k}*(Zmsr{k}-ZmsrF{k});  %状态更新
    Mobj{k+1}=MobjF{k}-W{k}*ZZcov{k}*W{k}'; %协方差更新
end
%绘制航迹图
hold on
for k=1:N
    Xobj(k)=Obj{k}(1,1);Yobj(k)=Obj{k}(2,1);          %目标真实轨迹
    XobjP(k)=Sobj{k+1}(1,1);YobjP(k)=Sobj{k+1}(2,1);  %跟踪轨迹
end
plot(XobjP,YobjP,'-r.',Xobj,Yobj,'b.',0,0,'o');
grid on


%EKF
ESobj{1}=[320 15 0 0]';  %初始状态估计量
EMobj{1}=100*eye(4); %初始协方差
for k=1:N
    EZreal{k}=[sqrt(Obj{k}(1,1)^2+Obj{k}(2,1)^2),atan(Obj{k}(2,1)/Obj{k}(1,1))]';%真实测量值
    EZmsr{k}=EZreal{k}+[w1(k),w2(k)]';   %实际测量值
end
%滤波算法
for k=1:N
    ESobjF{k}=F*ESobj{k};   %k时刻状态预测值
    EZmsrF{k}=[sqrt(ESobjF{k}(1,1)^2+ESobjF{k}(2,1)^2),atan(ESobjF{k}(2,1)/ESobjF{k}(1,1))]';%测量预测
    EZres{k}=EZmsr{k}-EZmsrF{k};  %测量residual
    EMobjF{k}=F*EMobj{k}*F'+Q;  %预测协方差
    %求观测函数的雅可比矩阵
    EH{k}=zeros(2,4);
    EH{k}(1,1)=ESobjF{k}(1,1)/sqrt(ESobjF{k}(1,1)^2+ESobjF{k}(2,1)^2);
    EH{k}(1,2)=ESobjF{k}(2,1)/sqrt(ESobjF{k}(1,1)^2+ESobjF{k}(2,1)^2);
    EH{k}(2,1)=-ESobjF{k}(2,1)/(ESobjF{k}(1,1)^2+ESobjF{k}(2,1)^2);
    EH{k}(2,2)=ESobjF{k}(1,1)/(ESobjF{k}(1,1)^2+ESobjF{k}(2,1)^2);
    ES{k}=C+EH{k}*EMobjF{k}*EH{k}';  %残差协方差
    EW{k}=EMobjF{k}*EH{k}'*inv(ES{k});  %滤波增益
    ESobj{k+1}=ESobjF{k}+EW{k}*EZres{k}; %状态更新
    EMobj{k+1}=EMobjF{k}-EW{k}*ES{k}*EW{k}'; %协方差更新
end
%绘制航迹图
for k=1:N
%    Xobj(k)=Obj{k}(1,1);  Yobj(k)=Obj{k}(2,1);  %目标真实轨迹
    EXobjP(k)=ESobj{k+1}(1,1);  EYobjP(k)=ESobj{k+1}(2,1); %跟踪轨迹
end
plot(EXobjP,EYobjP,'-g.'),grid on