cmkf.m

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

% %主程序，返回

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


function [Z1]=CMKF;

%丛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))';
 
%-----------------------------------------------------------------------------------------------
  

%----------------------使用CMKF直接滤波方法－－－－－－－－－－－------------------------------
X1=zeros(6,1,times);
P1=zeros(6,6,times);

X1(:,:,1)=XTrue(:,:,1);
X1(:,:,2)=x;
P1(:,:,2)=P0;
for i=2:times-1
    Xest=phi*X1(:,:,i);
    Ppre=phi*P1(:,:,i)*phi'+G*Q*G';
    
    %--------计算观测噪声方差－－－－－－
    rm=Z(1,1,i+1);   sitam=Z(2,1,i+1);   betam=Z(3,1,i+1);
    
    R=GetR(rm,sitam,betam,DeltaR,DeltaSita,DeltaBeta);
    
    if det(H*Ppre*H') < det(R)
        rm=(Xest(1,1)^2+Xest(3,1)^2+Xest(5,1)^2)^0.5;
        sitam=atan2(Xest(3,1),Xest(1,1));
        betam=atan(Xest(5,1)/sqrt(Xest(1,1)^2+Xest(3,1)^2));
        
        R=GetR(rm,sitam,betam,DeltaR,DeltaSita,DeltaBeta);
    end;    
    %------------------------------------
    
    K=Ppre*H'*(H*Ppre*H'+R)^(-1);
    P1(:,:,i+1)=Ppre-K*H*Ppre;
    X1(:,:,i+1)=Xest+K*(Zdc(:,:,i+1)-H*Xest);
end;


% Z 1代表   滤波后经转换的测量
Z1=zeros(3,1,times);
for ii=1:times
    %模型观测距离
    range=(X1(1,1,ii)^2+X1(3,1,ii)^2+X1(5,1,ii)^2)^0.5;
    %模拟观测方位角度 
    azimuth=atan2(X1(3,1,ii),X1(1,1,ii));
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    
    %观测俯仰角度
    pitching=atan(X1(5,1,ii)/(X1(1,1,ii)^2+X1(3,1,ii)^2)^0.5);
    
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z1(:,:,ii)=[range;
              azimuth;
              pitching];
        
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');