% newr.m % Analyze tracking algorithm by Park et al % AIAA GNC 2004 % % Assumes that ac3.m has been run to generate syscl % Jonathan How % MIT 16.333 Fall 2004 % % close all dt=1; % time step for the simulation U0=235.9; path=[]; jcase=1; % 2 path cases considered if jcase==1 t=[0:5*dt:2500]'; omp=.0025; path=24000*[sin(omp*t) (1-cos(omp*t))]; xe=0;ye=1500; X=[.1 0 0 0*pi/180 0*pi/180 0 0 0]'; else t=[0:dt:1350]'; path(:,1)=U0*t; omp=.005; path(:,2)=500*(-cos(2*pi*omp*t)+1).*exp(.002*t); xe=0;ye=-1000; X=[.1 0 0 -15*pi/180 -15*pi/180 0 0 0]'; end % Discretize the dynamics with time step dt % system has the inner yaw and roll loops closed [A,B,C,D]=ssdata(syscl); syscld=c2d(ss(A,B,C,D),dt); [Ad,Bd,Cd,Dd]=ssdata(syscld); Bd=Bd(:,1);Dd=Dd(:,1); % only need first input % bank angle limit philim=30; % %inputs are phi_d and 0 %state x=[v p r phi Psi xx xx xx] L1=2000; % look ahead distance store=[]; % find the point on the path L1 ahead ii=find((xe-path(:,1)).^2+(ye-path(:,2)).^2 < L1^2); iii=max(ii); % % % kk=1; while (~isempty(iii)) & (kk< length(t)-1) kk=kk+1; aim_point=path(iii,:); xedot=U0*cos(X(5))-X(1)*cos(X(4))*sin(X(5)); yedot=U0*sin(X(5))+X(1)*cos(X(4))*cos(X(5)); v1=[xedot yedot]'; v2=[aim_point(1)-xe aim_point(2)-ye]'; v1=v1/norm(v1); v2=v2/norm(v2); [v1 v2]; temp=cross([v1;0],[v2;0]); eta=acos(v1'*v2)*sign(temp(3)); phi_d=atan(2*U0^2/L1*sin(eta)/9.81); phi_d=max(min(phi_d,philim*pi/180),-philim*pi/180); store=[store;[t(kk) X' xe ye phi_d v2']]; % propagate forward a step X=Ad*X+Bd*phi_d; xe=xe+xedot*dt; ye=ye+yedot*dt; ii=find((xe-path(:,1)).^2+(ye-path(:,2)).^2 < L1^2); iii=max(ii); end figure(1);clf plot(store(:,11),store(:,10),'m'); hold on;plot(path(:,2),path(:,1),'g'); legend('veh','Path');xlabel('Y_e'); ylabel('x_e');setlines(2);hold off if jcase==1 axis('square');axis('equal') print -depsc park_1; jpdf('park_1') else orient tall print -depsc park_1a; jpdf('park_1a') end figure(2);clf plot(store(:,1),store(:,[5 12])*180/pi); axis([0 t(kk) -philim*1.1 philim*1.1]) xlabel('time');ylabel('\phi and \phi_d'); legend('\phi','\phi_d');setlines(2) if jcase==1 print -depsc park_2; jpdf('park_2') else print -depsc park_2a; jpdf('park_2a') end return