compareeandc.asv

文件包含有5项内容： 一、扩展卡尔曼滤波EKF 二、去偏转换卡尔曼滤波CMKF 三、最小二乘拟和的方法 四、最小二乘、EKF、CMKF的比较 五、野值剔除算法 用MATLAB实现了这些具体

clear;
%调用初始值function [XTrue,Z,Z2,T,Q,DeltaR,DeltaSita,DeltaBeta,DeltaR2,DeltaSita2,DeltaBeta2,Q2,totaltime,montimes]=MYInit;
 [XTrue,Z,Z0,T,Q,DeltaR,DeltaSita,DeltaBeta,totaltime,montimes]=MYInit;
%总采样点数
times=fix(totaltime/T+1);
%定义求和变量
% 
SumErrSquareP1=zeros(times,1);    
SumErrSquareP2=zeros(times,1);
SumErrSquareV1=zeros(times,1);  

sump1=zeros(times,1);
sump2=zeros(times,1);
sump3=zeros(times,1);

sumpr1=zeros(times,1);
sumpr2=zeros(times,1);
sumpr3=zeros(times,1);
for i=1:montimes
   [Z1]=EKF;
   [Z2]=CMKF;
   for h=1:times
       if h<=20
           B(h)=Z(1,1,h);   % 极坐标下的量测值
           C(h)=Z1(1,1,h);   %滤波
           D(h)=Z0(1,1,h);    %理想
           E(h)=Z2(1,1,h);
           
           B1(h)=Z(2,1,h);   % 极坐标下的量测值
           C1(h)=Z1(2,1,h);   %滤波
           D1(h)=Z0(2,1,h);    %理想
           E1(h)=Z2(2,1,h);
           
           B2(h)=Z(3,1,h);   % 极坐标下的量测值
           C2(h)=Z1(3,1,h);   %滤波
           D2(h)=Z0(3,1,h);    %理想
           E2(h)=Z2(3,1,h);
       else
          B(h)=Z(1,1,h);   
          C(h)=Z1(1,1,h);   %滤波
          D(h)=Z0(1,1,h);    %理想
          E(h)=Z2(1,1,h);
       
           B1(h)=Z(2,1,h);   % 极坐标下的量测值
           C1(h)=Z1(2,1,h);   %滤波
           D1(h)=Z0(2,1,h);    %理想
           E1(h)=Z2(2,1,h);
           
           B2(h)=Z(3,1,h);   % 极坐标下的量测值
           C2(h)=Z1(3,1,h);   %滤波
           D2(h)=Z0(3,1,h);    %理想
           E2(h)=Z2(3,1,h);
          hh1=h-10; hh2=h-9; hh3=h-8; hh4=h-7; hh5=h-6; hh6=h-5; hh7=h-4; hh8=h-3; hh9=h-2; hh10=h-1; 
          bb=[B(hh1),B(hh2),B(hh3),B(hh4),B(hh5),B(hh6),B(hh7),B(hh8),B(hh9),B(hh10),B(h)];
          x=h-10:h;
          p=polyfit(x,bb,2);
          B(h)=polyval(p,h);    %拟合
       end
   end
  for h=1:times
       SumErrSquareP1(h)=SumErrSquareP1(h)+(C(h)-D(h)).^2;%EKF
       SumErrSquareP2(h)=SumErrSquareP2(h)+(E(h)-D(h)).^2; %CMKF
       SumErrSquareV1(h)=SumErrSquareV1(h)+(B(h)-D(h)).^2; %最小二乘
       
        sump1(h)=sump1(h)+(C1(h)-D1(h)).^2;%EKF
       sump2(h)=sump2(h)+(E1(h)-D1(h)).^2; %CMKF
     
        sumr1(h)=sumr1(h)+(C2(h)-D2(h)).^2;%EKF
       sumr2(h)=sumr2(h)+(E2(h)-D2(h)).^2; %CMKF
  end
end;
%蒙特卡罗方法的距离、速度误差均方根值
RmsErrP1=(180/pi)*(SumErrSquareP1(:)/montimes).^0.5;
RmsErrP2=(180/pi)*(SumErrSquareP2(:)/montimes).^0.5;
RmsErrV1=(180/pi)*SumErrSquareV1(:)/montimes.^0.5;


sump1=(180/pi)*(sump1(:)/montimes).^0.5;
sump2=(180/pi)*(sump2(:)/montimes).^0.5;

sumr1=(180/pi)*(sumr1(:)/montimes).^0.5;
sumr2=(180/pi)*(sumr2(:)/montimes).^0.5;

i=1:times;
plot(i,RmsErrP1','k-',i,RmsErrP2','k:',i,RmsErrV1','k.');
legend('CMKF','EKF','最小二乘');
title('俯仰角误差均方根');
ylabel('度');
xlabel('k');

figure;


% figure
% 
% i=1:times;
% 
% plot(i,EV1,'k-',i,EV2,'k:');
% 
% legend('CMKF','EKF');
% 
% title('速度误差均方根');
% 
% ylabel('速度(m/s)');
% 
% xlabel('k');