trajectory_matlab.asv

利用角椎體原理來做影像導航 只需要三個共同地標點即可以定位

clc
clear all
close all
randn('seed',1245);

dt = 1 ;
step=100
e(1)=5000;
n(1)=0;
ve(1)=0;
vn(1)=100;
h_box=[];
for i = 1:100
h=8000;
h_box=[h_box,h];
end
for t=1:step
    if t<10,
     ae(t)=0;
     an(t)=0;
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    
    elseif t>=10&& t<20
    ae(t)=1;
    an(t)=1;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
        
    elseif t>=20 && t<30
    ae(t)=1;
    an(t)=1;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    elseif t>=30&& t<50
    ae(t)=0;
    an(t)=0;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    elseif t>=50&& t<60
    ae(t)=-6;
    an(t)=-6;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    elseif t>=60&& t<70
    ae(t)=4;
    an(t)=4;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    elseif t>=70&& t<90
    ae(t)=2;
    an(t)=2;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    
    elseif t>=90&& t<100
    ae(t)=0;
    an(t)=0;  
    e(t+1)=e(t)+ve(t)*dt;
    n(t+1)=n(t)+vn(t)*dt;
    ve(t+1)=ve(t)+ae(t)*dt;
    vn(t+1)=vn(t)+an(t)*dt;
    

       end
end



%