% Script fig13_25a.m; Navion localizer capture; tracking a circular arc
% and then a straight line; x=[v r p phi psi yct]'; u=[da dr]'; xdot=
% Fx+Gu; yout=[yct ay]'=Hx+Lu; units ft, sec, crad; 7/91, 6/96, 2/5/01
%
F=[-.254 -1.76 0 .322 0 0; 2.55 -.76 -.35 0 0 0; -9.08 2.19 -8.40 0 0 ...
0; 0 0 1 0 0 0; 0 1 0 0 0 0; 1 0 0 0 1.76 0]; G=[0 -.222 29 0 0 0; ...
.1246 -4.6 2.55 0 0 0]'; H=[0 0 0 0 0 1; -.254 0 0 0 0 0]; L=[0 0; ...
0 .1246]; A=diag([.04 4]); B=eye(2); K=lqr(F,G,H'*A*H,B+L'*A*L,H'*A*L);
%
% Simulate flight; s=[v r p phi psi rho]:
R=5378; s0=[0 0 0 0 -100*pi/6 R]'; [t,s]=ode23('loc_cap',[0 35],...
s0,[],F,G,K); N=length(t); un=ones(N,1); er=s(:,6)-R*un;
%
figure(1); clf; subplot(411), plot(t,er); grid; axis tight
ylabel('e_y (ft)'); subplot(412); plot(t,s(:,4:5)); grid
legend('\phi','\psi',4); ylabel('crad'); axis tight
subplot(413), plot(t,s(:,2:3)); grid; axis tight; legend('r','p')
ylabel('crad/sec'); subplot(414), plot(t,s(:,1)); grid; axis tight
ylabel('v (ft/sec)'); xlabel('Time (sec)')
