realtrack.m

this code is an example for kalman filter

%******************************************************************
%     仿真环境的生成....
%     目标起始X0=[40000 -40 25000 -50]',t=45s时发生机动,ax=-20,ay=-2
%     时间隔T=1s,仿真步数step=60,仿真次数num=50.
%****************************************************************

%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
%
%
%        环境1
%        目标起始X0=[40000 -40 25000 -50]',t=45s时发生机动,ax=-20,ay=-25;
%        t=80s时:ax=0,ay=20;t=120s时,ax=0,ay=-20.时间隔T=1s,仿真步数step=160,
%        仿真次数num=50.
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
%
function [N,Xr,Yr]=realTrack(flag,T)



 
xr0     =  40000;
xv0     =  -40;
yr0     =  25000;
yv0     =  -50;

xr(1)   =  xr0;
yr(1)   =  yr0;
xv(1)   =  xv0;
yv(1)   =  yv0;
%%%%%%%%%%%%%%%%%%%%%%%%%%   RealTrack   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch(flag)
   
    %目标真实轨迹0
    
    case 0
        
        for     t  = 1:44
            xr(t+1)   =  xr0  +  xv0*t;
            yr(t+1)    =  yr0  +  yv0*t;
        end
       xa     = -20;
       ya     = -2;

     for t=1:15
       xr(t+45)=xr(45)+xv*t+xa*t*t/2;
       yr(t+45)=yr(45)+yv*t+ya*t*t/2;
     end
     xv    =xv + xa*15 ;
     yv    =yv + ya*15 ;
     for n=0:T:59
      Xr(n/T+1)=xr(n+1);
      Yr(n/T+1)=yr(n+1);
     end
    N=60/T;

  %目标真实轨迹1
    case 1
        for     t  = 1:44
            xr(t+1)   =   xr0  +  xv0*t;
            yr(t+1)    =  yr0  +  yv0*t;
        end
        xa     = -20;
        ya     = -25;


     for t=1:35
       xr(t+45)=xr(45)+xv*t+xa*t*t/2;
       yr(t+45)=yr(45)+yv*t+ya*t*t/2;
     end
     xv    =  xv + xa*35 ;
     yv    =  yv + ya*35 ;
     xa    =  0;
     ya    = -20;
     for t=1:40
       xr(t+80)=xr(80)+xv*t+xa*t*t/2;
       yr(t+80)=yr(80)+yv*t+ya*t*t/2;
     end
     
    for n=0:T:119
      Xr(n/T+1)=xr(n+1);
      Yr(n/T+1)=yr(n+1);
    end
    N=120/T;
%目标真实轨迹2
case 2
totalTime=91;
    %雷达扫描周期T
%v2.0@李伟 2007.3.7%数据初始化,X,Y表示以T为间隔采样前的数据,总长90S
x=zeros(totalTime,1);
y=zeros(totalTime,1);
%x,y表示以T为间隔采样后的数据
X=zeros(ceil(totalTime/T),1);
Y=zeros(ceil(totalTime/T),1);
%速度vx,vy(m/s)
x(1)=120000;%起始点坐标
y(1)=2000;
vx=-426;
vy=0;

for t=1:29
    x(t+1)=x(1)+vx*t;
    y(t+1)=y(1)+vy*t;
end
ax=5;
ay=-10;

for t=1:7
    x(t+30)=x(30)+vx*t+ax*t*t/2;
    y(t+30)=y(30)+vy*t+ay*t*t/2;
end
vx=vx+ax*7;
vy=vy+ay*7;
ax=-8;
ay=18;
for t=1:11
    x(t+37)=x(37)+vx*t+ax*t*t/2;
    y(t+37)=y(37)+vy*t+ay*t*t/2;
end
vx=vx+ax*11;
vy=vy+ay*11;
ax=10;
ay=-20;

for t=1:12
    x(t+48)=x(48)+vx*t+ax*t*t/2;
    y(t+48)=y(48)+vy*t+ay*t*t/2;
end
vx=vx+ax*12;
vy=vy+ay*12;
ax=0;
ay=30;
for t=1:4
    x(t+60)=x(60)+vx*t+ax*t*t/2;
    y(t+60)=y(60)+vy*t+ay*t*t/2;
end
vx=vx+ax*4;
vy=vy+ay*4;
ax=-10;
ay=-8;
t=1;
    x(t+64)=x(64)+vx*t+ax*t*t/2;
    y(t+64)=y(64)+vy*t+ay*t*t/2;

vx=vx+ax*1;
vy=vy+ay*1;
ax=-5;
ay=0;
for t=1:15
    x(t+65)=x(65)+vx*t+ax*t*t/2;
    y(t+65)=y(65)+vy*t+ay*t*t/2;
end
vx=vx+ax*15;
vy=vy+ay*15;
ax=5;
ay=0;
for t=1:11
    x(t+80)=x(80)+vx*t+ax*t*t/2;
    y(t+80)=y(80)+vy*t+ay*t*t/2;
end
%以T为间隔采样
for n=0:T:totalTime-1
    Xr(n/T+1)=x(n+1);
    Yr(n/T+1)=y(n+1);
end
%显示真实轨迹
%plot(X,Y,'r');


N=totalTime/T;
%目标真实轨迹3
    case 3
        
%雷达扫描周期T
%数据初始化,X,Y表示以T为间隔采样前的数据
totalTime=1000;T=1;
x=zeros(totalTime,1);
y=zeros(totalTime,1);
%x,y表示以T为间隔采样后的数据
X=zeros(ceil(totalTime/T),1);
Y=zeros(ceil(totalTime/T),1);
%非机动段：t=0:400s;速度vx,vy(m/s)
x0=2000;%起始点坐标
y0=10000;
vx=15;
vy=0;

for t=0:400
    x(t+1)=x0+vx*t;
    y(t+1)=y0+vy*t;
end
%开始机动1:t=400:600s
ax=-0.075;
ay=0.075;

for t=0:200
    x(t+401)=x(401)+vx*t+ax*t*t/2;
    y(t+401)=y(401)+vy*t+ay*t*t/2;
end
vx=vx+ax*(600-400);%600s末的速度
vy=vy+ay*(600-400);

for t=0:10
    x(t+601)=x(601)+vx*t;
    y(t+601)=y(601)+vy*t;
end

ax=-0.3;
ay=-0.3;

for t=0:50
    x(t+611)=x(611)+vx*t+ax*t*t/2;
    y(t+611)=y(611)+vy*t+ay*t*t/2;
end
vx=vx+ax*(660-610);%660s末速度
vy=vy+ay*(660-610);
%660s以后匀速运动
for t=0:(totalTime-660)
    x(t+661)=x(661)+vx*t;
    y(t+661)=y(661)+vy*t;
end
%以T为间隔采样
for n=0:T:totalTime
    Xr(n/T+1)=x(n+1);
    Yr(n/T+1)=y(n+1);
end
%显示真实轨迹
%plot(X,Y,'r');
N=totalTime/T;
%显示真实轨迹
%plot(X,Y,'r');
end

%显示真实轨迹
%plot(Xr,Yr,'r');