%% 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
%% Building the state-space matrices and the model

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 [rad]','phi [rad]','p [rad/s]','r [rad/s]','delta_a [deg]','delta_r [deg]','r_w [deg/s]'});
set(G,'OutputName',{'r_w [deg/s]','p [deg/s]','beta [deg]','phi [deg]'});
set(G,'InputName',{'u_a [deg]','u_r [deg]'});

%
%% Analyzing the model

damp(G)

%
%% Setting the weights for the LQ optimization

q_beta = 1;
q_phi = 100;
q_p = 1;
q_r = 1;
q_rw = 1;

rho = 1
r_a = 1*rho;
r_r = 1*rho;

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

%
%% Checking the satisfaction of the existence and uniqueness conditions

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

%
%% Solving continuous-time ARE

[S,K,p] = icare(A,B,Q,R);

%
%% Forming a closed-loop system

G_closed = ss(A-B*K,B,eye(7),zeros(7,2));
set(G_closed,'OutputName',{'\beta [rad]','\phi [rad]','p [rad/s]','r [rad/s]','\delta_a','\delta_r','r_w [deg/s]'});

%
%% Simulating a response to some nonzero states

initial(G_closed,[1/10 0 0 0 0 0 0],10)
G_closed = ss(A-B*K,B,C,zeros(4,2));
set(G_closed,'OutputName',{'r_w [deg/s]','p [deg/s]','beta [deg]','phi [deg]'});
initial(G_closed,[1/10 0 0 0 0 0 0],10)