ekf_radar.m

扩展卡尔曼滤波 信息同步 数据融合算法实现

% %  子程序
% %  采用扩展卡尔曼滤波算法
% %  时间  2003 6 17
% %  秦玉亮
% % %-----------------------------------------------------------------------------------------------


function [X2,Zfilt,P2,ZRadar]=EKF_Radar;

%丛SourceRadar.m中调用所有用到的参数
[ZTrueRadar,ZRadar,XTrueRadar,totaltimeRadar,T0Radar,Tao0_Radar,t_Radar]=sourceRadar;
%采样点数：总运动时间/采样周期＋1
T=T0Radar;
times=totaltimeRadar/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];


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

DeltaR=3000;               %观测距离误差标准差
DeltaSita=1*pi/180;      %观测方位角角度误差标准差    此例中方位角和俯仰角的标准差应相等
DeltaBeta=1*pi/180;        %观测俯仰角角度误差标准差   


DeltaSita_w=.2*pi/180;    %观测方位角角速度误差标准差    此例中方位角和俯仰角角速度的标准差应相等
DeltaBeta_w=.2*pi/180;    %观测俯仰角角速度误差标准差   


q=0.01;                     % q是系统噪声标准差
Q=q^2*eye(3);              % 系统各方向的状态噪声方差

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

 X2=zeros(6,1,times);
 P2=zeros(6,6,times);
 
 P0=zeros(6,6,times);
 nCount=0; %计数 每个融合周期内被动雷达滤波次数的计数
 j=0;     %融合周期计数
%--------------------EKF滤波方法，结果为X2－－－－－－－－－－－－－－
X2(:,:,1)=XTrueRadar(:,:,1);

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

r2=12000;
r1=r2;
% r1=Z1(1,1,1);  
sita1=ZRadar(1,1,1);   beta1=ZRadar(2,1,1);
% r2=ZRadar(1,1,2);  
sita2=ZRadar(1,1,2);   beta2=ZRadar(2,1,2);

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

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

P2(:,:,2)=P0;
P2(:,:,1)=P0;
for i=2:times-1
    
    Xest=phi*X2(:,:,i);
    Q1=G*Q*G';
    Ppre=phi*P2(:,:,i)*phi'+Q1';
    
    %计算  H'[X(k+1|k)]
    %临时变量xk,yk,zk; xkf, ykf ,zkf; RR, Rz, rrf
    
   xk=Xest(1,1);   yk=Xest(3,1);   zk=Xest(5,1); xkf=Xest(2,1); ykf=Xest(4,1); zkf=Xest(6,1);
  
   RR=(xk^2+yk^2+zk^2)^0.5;
   Rz=(xk^2+yk^2)^0.5;
   rrf= (xk*xkf+yk*ykf+zk*zkf)/RR;
    
    h11=xk/RR;
    h13=yk/RR;
    h15=zk/RR;
   
    h21=yk/Rz^2;
    h23=- xk/Rz^2;
    
    h31=-xk*zk/(RR^ 2*Rz);
    h33=-yk*zk/(RR^2*Rz);
    h35=Rz/RR^2;
    
    h41=(2*xk*(xk*ykf-yk*xkf)-ykf*Rz^2)/Rz^4;
    h42=yk/Rz^2;
    h43=(xkf*Rz^2-2*yk*(yk*xkf-xk*ykf))/Rz^4;
    h44=-xk/Rz^2;
    
    h51=((xk*zkf-zk*xkf)*RR*Rz^2-(zkf*RR-rrf*zk)*(2*xk*Rz^2+zk^2*xk))/(RR^3*Rz^3);
    h52=-zk*xk/(RR^2*Rz);
    h53=((yk*zkf-zk*ykf)*Rz^2*RR-(zkf*RR-rrf*zk)*(2*yk*Rz^2+yk*zk^2))/(RR^3*Rz^3);
    h54=-zk*yk/(RR^2*Rz);
    h55=(-rrf*RR^2*Rz-2*(zkf*RR-zk*rrf)*zk*Rz)/(RR^3*Rz^3);
    h56=RR/Rz^2;
    
    H2=[%h11  0   h13   0   h15 0;
        h21  0   h23   0    0  0;
        h31  0   h33   0   h35 0;
        h41  h42 h43  h44   0  0;
        h51  h52 h53  h54  h55 h56];


    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(xk,yk);
          atan(zk/(xk^2+yk^2)^0.5);
         (yk*xkf-xk*ykf)/(xk^2+yk^2);
%          (zkf*Rz^2-zk*(xkf+ykf))/(RR*Rz)];    
     (zkf*RR-zk*rrf)/(RR*Rz)];
%  
 
%-------------对数据进行判断--------------
     if (ZRadar(1,1,i+1)==0)&(ZRadar(2,1,i+1)==0)&(ZRadar(3,1,i+1)==0)&(ZRadar(4,1,i+1)==0)                  %如果数据不存在  则进行推测
         X2(:,:,i+1)=Xest;
     else
      X2(:,:,i+1)=Xest+K*(ZRadar(:,:,i+1)-Hest);
     end
 %------------------------



end

Zfilt=zeros(4,1,times);                       
for i=1:times
    %模型观测距离
   
%     range=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;

      
    %模拟观测方位角度 
    azimuth=atan(X2(1,1,i)/X2(3,1,i));      %%%%%   这里采用的是孙忠康的雷达数据处理中323页的公式  注意其方位角的公式与一般不同!!!!!!!      

%      
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(X2(5,1,i)/(X2(1,1,i)^2+X2(3,1,i)^2)^0.5);
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    %观测方位角速度
    azimuth_w=-(X2(3,1,i)*X2(2,1,i)-X2(1,1,i)*X2(4,1,i))/(X2(1,1,i)^2+X2(3,1,i)^2)  ;
    
    %观测俯仰角角速度
    R_Rs=(X2(1,1,i)^2+X2(3,1,i)^2)^0.5;
    R_R=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;
    R_V=( X2(1,1,i)*X2(2,1,i)+X2(3,1,i)*X2(4,1,i)+X2(5,1,i)*X2(6,1,i) )/R_R;
    
   pitching_w=( X2(6,1,i)*R_R-X2(5,1,i)*R_V)/(R_R*R_Rs) ;
    
    Zfilt(:,:,i)=[%range;
              azimuth;
              pitching;
              azimuth_w;
              pitching_w];
  end;