ekf.m

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

% %主程序，返回

% 
% %  
% % %-----------------------------------------------------------------------------------------------


function [Z2]=EKF;

%丛Ini.m中调用所有用到的参数
[XTrue,Z,Z0,T,Q,DeltaR,DeltaSita,DeltaBeta,totaltime,montimes]=MYInit;

%采样点数：总运动时间/采样周期＋1
times=totaltime/T+1;

%计算参数准备:
%状态方程X(k+1)=phi*X(k)+G*W(k)  ;E[W(k)W(k)']=Q
%X(k)=[x volx y voly z volz]';
phi=[1 T 0 0 0 0;
     0 1 0 0 0 0;
     0 0 1 T 0 0;
     0 0 0 1 0 0;
     0 0 0 0 1 T;
     0 0 0 0 0 1];
 
 G=[T^2/2  0   0;
      T    0   0;
      0  T^2/2 0;
      0    T   0;
      0    0  T^2/2;
      0    0   T];


%观测方程Zdc(k)=H*X(k)+V(k),其中Zdc是已转换为直角坐标系下的观测值
H=[1 0 0 0 0 0;
   0 0 1 0 0 0;
   0 0 0 0 1 0]; 

%Zdc为转换到直角坐标系下的观测值,公式见刘春玲《现代雷达》的p50式18-24

Zdc=zeros(3,1,times);        %Zdc是转换后的观测值

%临时变量DeltaR2,DeltaSita2,DeltaBeta2,eSita1,eBeta1;

DeltaR2=DeltaR^2;
DeltaSita2=DeltaSita^2;
DeltaBeta2=DeltaBeta^2;

eSita1=exp(-DeltaSita2);

eBeta1=exp(-DeltaBeta2);

%－－－－－－－－－－计算测量转换纵坐标的直角坐标观测值－－－－－－－－－－－－－－－－----------
for i=1:times
    %临时变量rm,sitam,betam
    rm=Z(1,1,i);
    sitam=Z(2,1,i);
    betam=Z(3,1,i);
    
    Zdc(:,:,i)=[rm*cos(betam)*cos(sitam)*(1-eSita1*eBeta1+(eSita1*eBeta1)^0.5);
                rm*cos(betam)*sin(sitam)*(1-eSita1*eBeta1+(eSita1*eBeta1)^0.5);
                rm*sin(betam)*(1-eBeta1+eBeta1^0.5)];        
end;
%-----------------------------------------------------------------------------------------------

%--------计算初始估计，首先以Zdc(:,:,1),Zdc(:,:,2)，建立模型的初始估计x,即X(2)----------------------，

x=zeros(6,1);
 x=[        Zdc(1,1,2);
     (Zdc(1,1,2)-Zdc(1,1,1))/T;
            Zdc(2,1,2);
     (Zdc(2,1,2)-Zdc(2,1,1))/T;
            Zdc(3,1,2);
     (Zdc(3,1,2)-Zdc(3,1,1))/T];
 
 
 %使用真值来计算初始估计误差协方差,程序中三个算法都采用使用真值计算的初始估计误差协方差
 P0=(x-XTrue(:,:,2))*(x-XTrue(:,:,2))';

%--------------------用来对比的EKF滤波方法，结果为X2－－－－－－－－－－－－－－
X2(:,:,1)=XTrue(:,:,1);

%极坐标观测噪声协方差R2
R2=[DeltaR^2    0         0;
      0      DeltaSita^2  0;
      0         0        DeltaBeta^2];
  
%以Z(:,:,1),Z(:,:,2)(极坐标系的测量值），建立模型的初始估计xx，
r1=Z(1,1,1);   sita1=Z(2,1,1);   beta1=Z(3,1,1);
r2=Z(1,1,2);   sita2=Z(2,1,2);   beta2=Z(3,1,2);

%设定初始值和初始状态
X2(:,:,2)=[r2*cos(beta2)*cos(sita2); 
          (r2*cos(beta2)*cos(sita2)-r1*cos(beta1)*cos(sita1))/T;
             r2*cos(beta2)*sin(sita2);
           (r2*cos(beta2)*sin(sita2)-r1*cos(beta1)*sin(sita1))/T;
         r2*sin(beta2);
          (r2*sin(beta2)-r1*sin(beta1))/T];   

%使用真值计算初值估计误差方差
P0=(XTrue(:,:,2)-X2(:,:,2))*(XTrue(:,:,2)-X2(:,:,2))';

P2(:,:,2)=P0;

for i=2:times-1
    
    Xest=phi*X2(:,:,i);
    Ppre=phi*P2(:,:,i)*phi'+G*Q*G';
    
    %计算  H'[X(k+1|k)]
    %临时变量xk,yk,zk
    xk=Xest(1,1);   yk=Xest(3,1);   zk=Xest(5,1);
    
    h11=xk/(xk^2+yk^2+zk^2)^0.5;
    h13=yk/(xk^2+yk^2+zk^2)^0.5;
    h15=zk/(xk^2+yk^2+zk^2)^0.5;
    h21=-yk/(xk^2+yk^2);
    h23= xk/(xk^2+yk^2);
    h31=-xk*zk/(xk^2+yk^2+zk^2)/(xk^2+yk^2)^0.5;
    h33=-yk*zk/(xk^2+yk^2+zk^2)/(xk^2+yk^2)^0.5;
    h35=(xk^2+yk^2)/(xk^2+yk^2+zk^2)/(xk^2+yk^2)^0.5;
    
    H2=[h11 0 h13 0 h15 0;
       h21 0 h23 0  0  0;
       h31 0 h33 0 h35 0];
    
    
    K=Ppre*H2'*(H2*Ppre*H2'+R2)^(-1);
    P2(:,:,i+1)=Ppre-K*H2*Ppre;
    
    Hest=[(xk^2+yk^2+zk^2)^0.5;
          atan2(yk,xk);
          atan(zk/(xk^2+yk^2)^0.5)];    
      
    X2(:,:,i+1)=Xest+K*(Z(:,:,i+1)-Hest);
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Z2代表   滤波后经转换的测量
Z2=zeros(3,1,times);
for ii=1:times
    %模型观测距离
 
    range2=(X2(1,1,ii)^2+X2(3,1,ii)^2+X2(5,1,ii)^2)^0.5;
    %模拟观测方位角度 
  
    azimuth2=atan2(X2(3,1,ii),X2(1,1,ii));
    if azimuth2>2*pi
        azimuth2=azimuth2-2*pi;
    else if azimuth2 < 0
            azimuth2=azimuth2+2*pi;
        end;
    end;
    %观测俯仰角度
   
    pitching2=atan(X2(5,1,ii)/(X2(1,1,ii)^2+X2(3,1,ii)^2)^0.5);
    
    if pitching2>pi/2
        pitching2=pitching2-pi;
    else if pitching2<-pi/2
            pitching2=pitching2+pi;
        end;
    end;
   
          Z2(:,:,ii)=[range2;
              azimuth2;
              pitching2];
end;


%    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    %Z2  代表经最小二乘拟合所获得的测量
%    x=1:times;
%    for h=1:times
%        B(h)=Z(3,1,h);
%        C(h)=Z1(3,1,h);
%        D(h)=Z0(3,1,h);
%        E(h)=Z2(3,1,h);
%    end
%     p=polyfit(x,B,2);
% 
%    x2=1:times;
%    y2=polyval(p,x2);
%    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot(x,D,'k-',x,y2,'k:');
% title('最小二乘估计——俯仰角');
% legend('真实数据','拟和数据');
% xlabel('k');
% ylabel('俯仰角');
% figure;
% plot(x,D,'k-',x,C,'k:');
% title('最小二乘估计——俯仰角');
% legend('真实数据','CMKF滤波数据');
% xlabel('k');
% ylabel('俯仰角');
% figure;
% plot(x,D,'k-',x,E,'k:');
% title('最小二乘估计——俯仰角');
% legend('真实数据','EKF滤波数据');
% xlabel('k');
% ylabel('俯仰角');
%  plot(x,y2,'r',x,C,'k',x,D);
% plot(x,C,'r');