%% LQ-optimal control design for F16 lateral regulator 
%
% Example 5.3-1 from Stevens & Lewis 2003, page 413.
%
% Linear model of lateral-directional dynamics of F16 
% trimmed at: VT=502ft/s, 302psf dynamic pressure, cg @ 0.35cbar. 
% Includes dynamics of ailerons and rudders and washout filter.
%
% beta ... sideslip angle
% phi  ... bank angle
% p    ... roll rate
% r    ... yaw rate
%
% delta_a ... aileron deflection
% delta_r ... rudder deflection

A = [-0.3220,   0.0640,     0.0364,     -0.9917,    0.0003,     0.0008  0;
    0,          0,          1,          0.0037,     0,          0,      0;
    -30.6492,   0,          -3.6784,    0.6646,     -0.7333,    0.1315, 0;
    8.5396,     0,          -0.0254,    -0.4764,    -0.0319,    -0.062, 0;
    0,          0,          0,          0,          -20.2,      0,      0;
    0,          0,          0,          0,          0,         -20.2,   0;
    0,          0,          0,          57.2958,    0,          0,      -1];

B = [0,     0;
    0,      0;
    0,      0;
    0,      0;
    20.2,   0;
    0,      20.2;
    0,      0];

C = [0,         0,          0,          57.2958,    0,      0,      -1;
    0,          0,          57.2958,    0,          0,      0,      0;
    57.2958,    0,          0,          0,          0,      0,      0;
    0,          57.2958,    0,          0,          0,      0,      0];


G = ss(A,B,C,zeros(4,2));

set(G,'StateName',{'beta','phi','p','r','delta_a','delta_r','r_w'});
set(G,'OutputName',{'r_w','p','beta','phi'});
set(G,'InputName',{'u_a','u_r'});

Ts = 0.1;          % sampling period
Gd = c2d(G,Ts);     % discretized system

[A,B,C,D] = ssdata(Gd);

q_beta = 0.1;
q_phi = 1;
q_p = 10;
q_r = 1000;
q_rw = 100;

r_a = 1;
r_r = 1;

Q = diag([q_beta q_phi q_p q_r 0 0 q_rw]);
R = diag([r_a r_r]);

Cq = sqrt(Q);
rank(obsv(A,Cq)) % check the observability condition

[S,E,K] = dare(A,B,Q,R);

G_closed = ss(A-B*K,B,eye(7),zeros(7,2));
set(G_closed,'Ts',Ts)
set(G_closed,'OutputName',{'\beta','\phi','p','r','\delta_a','\delta_r','r_w'});
initial(G_closed,[1 0 0 0 0 0 0],10)


% print -depsc2 ../lectures/figures/lqr_f16_initial.eps
% !epstopdf ../lectures/figures/lqr_f16_initial.eps