true_trajectory1.m

在TDOA定位系统中,利用平滑的IMM算法对机动目标进行被动跟踪

clear

All_time=300;
Velocity=300;
an=pi/4;
Begin_posit_x=60*10^3;
Begin_posit_y=40*10^3;
True_x(1)=Begin_posit_x;
True_y(1)=Begin_posit_y;
Velocity_x(1)=-Velocity*cos(an);
Velocity_y(1)=Velocity*sin(an);
Velocity_angle(1)=0;
for k=2:60
    Velocity_angle(k)=0;
    Velocity_x(k)=-Velocity*cos(an);
    Velocity_y(k)=Velocity*sin(an);
    True_x(k)=True_x(k-1)+Velocity_x(k);
    True_y(k)=True_y(k-1)+Velocity_y(k);
    
end

%%% Step2: the 1th maneuvering flight of 180" turns with angular rates, w = 1.87  for 180/1.87=95 s of time %%%


angle(60)=an+pi/2;
for k=61:155
    Velocity_angle(k)=pi*1.87/180;
    angle(k)=angle(k-1)+Velocity_angle(k);
    Velocity_x(k)=Velocity*cos(angle(k));
    Velocity_y(k)=Velocity*sin(angle(k));
        
    True_x(k)=True_x(k-1)+Velocity_x(k);
    True_y(k)=True_y(k-1)+Velocity_y(k);   
end

%%% Step3 nonmaneuvering flight for 60 s of time  %%%


for k=156:180
    Velocity_angle(k)=0;
    Velocity_x(k)=Velocity*cos(an);
    Velocity_y(k)=-Velocity*sin(an);
    True_x(k)=True_x(k-1)+Velocity_x(k);
    True_y(k)=True_y(k-1)+Velocity_y(k);   
end

%%% Step4: the 2th maneuvering flight of 180" turns with angular rates, w = -2.8  for 180/2.8=65 s of time %%%


angle(180)=-pi/2+atan(an);
for k=181:245
    Velocity_angle(k)=-pi*2.8/180;
    angle(k)=angle(k-1)+Velocity_angle(k);
    Velocity_x(k)=Velocity*cos(angle(k));
    Velocity_y(k)=Velocity*sin(angle(k));
        
    True_x(k)=True_x(k-1)+Velocity_x(k);
    True_y(k)=True_y(k-1)+Velocity_y(k);
   
end

%%% Step5 nonmaneuvering flight for 55 s of time  %%%

for k=246:300
    Velocity_angle(k)=0;
    Velocity_x(k)=-Velocity*cos(an);
    Velocity_y(k)=Velocity*sin(an);
    True_x(k)=True_x(k-1)+Velocity_x(k);
    True_y(k)=True_y(k-1)+Velocity_y(k); 
    
end
subplot(211)

plot(True_x,True_y)
axis([1.5 6.5 1.5 6.5]*10^4)
xlabel('x (m)')
ylabel('x (m)')
xlabel('x')
ylabel('y')
k=1:All_time;
subplot(212)
plot(k,Velocity_x,k,Velocity_y)
xlabel('t (s)')
ylabel('velocity (m/s)')