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

%% Analyzing the model
damp(G)

%% Discretizing the linear state space model
Ts = 0.1;          % sampling period
Gd = c2d(G,Ts);     % discretized system

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

%% Setting the weights for the LQ optimization
q_beta = 1;
q_phi = 1;
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 discrete-time ARE
[S,E,K] = dare(A,B,Q,R);

%% Finding the feedforward matrices
Cr = [0 0 0 1 0 0 0];               % picks the fourth variable - the pitch rate p
H = [A-eye(7,7) B; Cr zeros(1,2)]
Z = vertcat(zeros(7,1),eye(1,1))

NxNy = H\Z

Nx = NxNy(1:7)
Nu = NxNy(8:end)

Nbar = Nu + K*Nx

%% Forming a closed-loop system
G_closed = ss(A-B*K,B*Nbar,Cr,zeros(1,1));
set(G_closed,'Ts',Ts)
set(G_closed,'OutputName',{'r'});

%% Simulating a response to a step
step(G_closed,10)


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