%% LQR Control using State-Space Matrices
% aircraftPitchModel.mat contains the state-space matrices of an aircraft where the input is the elevator deflection angle \delta and the output is the aircraft pitch angle \theta.
% \left[\begin{array}{c}
% \dot{\alpha}\\
% \dot{q}\\
% \dot{\theta}\\
% \end{array}\right]\;=\;\left[\begin{array}{ccc}
% -0.313 & 56.7& 0\\
% -0.0139 & -0.426 & 0\\
% 0 & 56.7 & 0\\
% \end{array}\right] \left[\begin{array}{c}
% \alpha\\
% q\\
% \theta\\
% \end{array}\right]+\left[\begin{array}{c}
% 0.232\\
% 0.0203\\
% 0\\
% \end{array}\right] \left[\delta\right]\\
% \;\;\;\;\;y\;=\;\left[\begin{array}{ccc}
% 0 & 0 & 1
% \end{array}\right] \left[\begin{array}{c}
% \alpha\\
% q\\
% \theta\\
% \end{array}\right]+\left[0\right]\left[\delta\right]
% For a step reference of 0.2 radians, consider the following design criteria:
% Rise time less than 2 seconds
% Settling time less than 10 seconds
% Steady-state error less than 2%
% Load the model data to the workspace.

load('aircraftPitchModel.mat')

% Define the state-cost weighted matrix Q and the control weighted matrix R. Generally, you can use Bryson's Rule to define your initial weighted matrices Q and R. For this example, consider the output vector C along with a scaling factor of 5 for matrix Q and choose R as 1. R is a scalar since the system has only one input.

R = 1
Q1 = 2*C'*C

% Compute the gain matrix using lqr.

[K1,S1,P1] = lqr(A,B,Q1,R);

% Check the closed-loop step response with the generated gain matrix K1.

sys1 = ss(A-B*K1,B,C,D);
step(sys1)

% Since this response does not meet the design goals, increase the scaling factor to 25, compute the gain matrix K2, and check the closed-loop step response for gain matrix K2.

Q2 = 25*C'*C
[K2,S2,P2] = lqr(A,B,Q2,R);
sys2 = ss(A-B*K2,B,C,D);
step(sys2)

% In the closed-loop step response plot, the rise time, settling time, and steady-state error meet the design goals.
% Copyright 2019 The MathWorks, Inc.