www.pudn.com > data_fusion.rar > sourceRadar.m, change:2009-06-04,size:4078b

 
%产生雷达数据，考虑异步，即被动雷达数据到达时间与融合时间之间不同 
function [Z0,ZRadar,XTrue,totaltime,TRadar,Tao0_Radar,t_Radar]=sourceRadar; 
TRadar=0.005;               %采样周期 1 
totaltime=10;         %匀速运动阶段时间 
Tao0_Radar=0.002;      %被动雷达第一个数据的延时 
%初始距离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方向速度 
 
%采样率为2HZ的雷达的初始误差 
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);              %系统各方向的状态噪声方差 
 
%－－－－－－－－计算目标在雷达1(采样间隔为10HZ)中的真实轨迹,初始化XTrue1－－－－－－－－－－－－－－－－－－－－ 
times = totaltime/TRadar+1;          %采样点数 
 
XTrue=zeros(6,1,times); 
t_Radar=zeros(1,1,times); 
%真实轨迹作直线运动 
for i=1:times 
    XTrue(:,:,i)=[Xstart + vx*((i-1)*TRadar+Tao0_Radar); 
                      vx; 
                  Ystart + vy*((i-1)*TRadar+Tao0_Radar); 
                      vy; 
                  Zstart + vz*((i-1)*TRadar+Tao0_Radar); 
                      vz;];                  
    t_Radar(1,1,i)=(i-1)*TRadar+Tao0_Radar; 
end; 
 
 
%－－－－－－－－极坐标下的模拟观测值，观测噪声距离方向上标准差DeltaR米，角度方位差为DeltaSita,DeltaBeta－－－－－－－－－－ 
ZRadar=zeros(4,1,times); 
for i=1:times 
 
     
    azimuth=atan(XTrue(1,1,i)/XTrue(3,1,i))+randn(1)*DeltaSita; 
 
      
    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)+randn(1)*DeltaBeta; 
    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)  + randn(1)*DeltaSita_w; 
     
    %观测俯仰角角速度 
    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)  + randn(1)*DeltaBeta_w; 
    ZRadar(:,:,i)=[ azimuth; 
              pitching; 
              azimuth_w; 
              pitching_w]; 
           
           
 
     
end; 
%－－－－－－－－极坐标下的模拟观测值理想状态下的值－－－－－－－－－－ 
Z0=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) ; 
     
    Z0(:,:,i)=[azimuth; 
              pitching; 
              azimuth_w; 
              pitching_w]; 
 
     
end;