% %  子程序
% %  采用擴(kuò)展卡爾曼濾波算法
% %  時(shí)間  2003 6 17
% %  秦玉亮
% % %-----------------------------------------------------------------------------------------------


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

%叢SourceRadar.m中調(diào)用所有用到的參數(shù)
[ZTrueRadar,ZRadar,XTrueRadar,totaltimeRadar,T0Radar,Tao0_Radar,t_Radar]=sourceRadar;
%采樣點(diǎn)數(shù)：總運(yùn)動(dòng)時(shí)間/采樣周期＋1
T=T0Radar;
times=totaltimeRadar/T+1;

%計(jì)算參數(shù)準(zhǔn)備:
%狀態(tài)方程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;               %觀測距離誤差標(biāo)準(zhǔn)差
DeltaSita=1*pi/180;      %觀測方位角角度誤差標(biāo)準(zhǔn)差    此例中方位角和俯仰角的標(biāo)準(zhǔn)差應(yīng)相等
DeltaBeta=1*pi/180;        %觀測俯仰角角度誤差標(biāo)準(zhǔn)差   


DeltaSita_w=.2*pi/180;    %觀測方位角角速度誤差標(biāo)準(zhǔn)差    此例中方位角和俯仰角角速度的標(biāo)準(zhǔn)差應(yīng)相等
DeltaBeta_w=.2*pi/180;    %觀測俯仰角角速度誤差標(biāo)準(zhǔn)差   


q=0.01;                     % q是系統(tǒng)噪聲標(biāo)準(zhǔn)差
Q=q^2*eye(3);              % 系統(tǒng)各方向的狀態(tài)噪聲方差

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

 X2=zeros(6,1,times);
 P2=zeros(6,6,times);
 
 P0=zeros(6,6,times);
 nCount=0; %計(jì)數(shù) 每個(gè)融合周期內(nèi)被動(dòng)雷達(dá)濾波次數(shù)的計(jì)數(shù)
 j=0;     %融合周期計(jì)數(shù)
%--------------------EKF濾波方法，結(jié)果為X2－－－－－－－－－－－－－－
X2(:,:,1)=XTrueRadar(:,:,1);

%極坐標(biāo)觀測噪聲協(xié)方差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)(極坐標(biāo)系的測量值），建立模型的初始估計(jì)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);

%設(shè)定初始值和初始狀態(tài)
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];   

%使用真值計(jì)算初值估計(jì)誤差方差
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';
    
    %計(jì)算  H'[X(k+1|k)]
    %臨時(shí)變量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)];
%  
 
%-------------對數(shù)據(jù)進(jìn)行判斷--------------
     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)                  %如果數(shù)據(jù)不存在  則進(jìn)行推測
         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));      %%%%%   這里采用的是孫忠康的雷達(dá)數(shù)據(jù)處理中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;