source.m

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

%产生雷达数据，考虑异步，即被动雷达数据到达时间与融合时间之间不同
% 2003,6,16   秦玉亮
function [ZTrue,XTrue]=source;
totaltime=10;         %匀速运动阶段时间
Tf=0.020;
%初始距离r0,sita0,beta0
r0=12000;            %目标初始斜距
sita0=pi/4;          %初始方位角
beta0=10*pi/180;          %初始俯仰角

Xstart=r0*cos(beta0)*cos(sita0);       %目标起始X坐标
Ystart=r0*cos(beta0)*sin(sita0);      %目标起始Y坐标
Zstart=r0*sin(beta0);                %目标起始Z坐标

vx=-800;             %X方向速度
vy=-230;          %Y方向速度
vz=0;            %Z方向速度


%－－－－－－－－计算目标在雷达1(采样间隔为10HZ)中的真实轨迹,初始化XTrue1－－－－－－－－－－－－－－－－－－－－
times = fix(totaltime/Tf);          %采样点数

XTrue=zeros(6,1,times);
%真实轨迹作直线运动
for i=1:times
    XTrue(:,:,i)=[Xstart + vx*(i*Tf);
                      vx;
                  Ystart + vy*(i*Tf);
                      vy;
                  Zstart + vz*(i*Tf);
                      vz;];                 
end;


%－－－－－－－－极坐标下的模拟观测值理想状态下的值－－－－－－－－－－
ZTrue=zeros(4,1,times);
for i=1:times
      
    azimuth=atan(XTrue(1,1,i)/XTrue(3,1,i));

   
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(XTrue(5,1,i)/(XTrue(1,1,i)^2+XTrue(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=(XTrue(3,1,i)*XTrue(2,1,i)-XTrue(1,1,i)*XTrue(4,1,i))/(XTrue(1,1,i)^2+XTrue(3,1,i)^2)  ;
    
    %观测俯仰角角速度
    R_Rs=(XTrue(1,1,i)^2+XTrue(3,1,i)^2)^0.5;
    R_R=(XTrue(1,1,i)^2+XTrue(3,1,i)^2+XTrue(5,1,i)^2)^0.5;
    R_V=( XTrue(1,1,i)*XTrue(2,1,i)+XTrue(3,1,i)*XTrue(4,1,i)+XTrue(5,1,i)*XTrue(6,1,i) )/R_R;
    
    pitching_w=( XTrue(6,1,i)*R_R-XTrue(5,1,i)*R_V )/(R_R*R_Rs) ;
    
    ZTrue(:,:,i)=[azimuth;
              pitching;
              azimuth_w;
              pitching_w];

    
end;