# LQR Control

This document describes the MATLAB and Simulink implementation
for designing an LQR controller for our quadcopter platform.
This work is almost entirely based on Matt Rich's graduate thesis ([Model development, system identification, and control of a quadrotor helicopter][model_dev]),
specifically sections 4.1, 4.2 (for the nonlinear model), 7.3 (initial LQR control),
and 7.4 (improved LQR control).

## 1 Theory

### 1.1 Nonlinear Model

The nonlinear model for the quadcopter is described at a high level in section 4.1,
with individual components further separated and defined in section 4.2. The definition
is given in the form of $`\dot \Lambda = f(\Lambda)`$ where $`\Lambda`$ is the state vector.
Technically, this is also a function of rotor force and torque.

There are 12 state variables: three linear velocities ($`u`$, $`v`$, $`w`$),
three rotational velocities ($`p`$, $`q`$, $`r`$), hree dimesions for location ($`x`$, $`y`$, $`z`$),
and three Euler angles ($`\phi`$, $`\theta`$, $`\psi`$).

### 1.2 Linearization

The linearization for this system is performed around the only equilibrium we have, which is hovering in a constant
position. This means that every state variable is $`0`$ except for possibly the $`z`$ position, but we set that to
$`0`$ as well because it does not affect the linearization. The equilibrium command inputs are also zero except for
$`u_T`$ (as some non-zero throttle is required to maintain altitude).

### 1.3 LQR Controller

LQR (Linear Quadratic Regulator) control is designed to minimize a given cost function, which is given as
```math
J = \int_0^\infty x^T Q x + u^T R u dt
```
where $`x`$ is the error between the state and the equilibrium state, and $`u`$ is the difference between the command
at time $`t`$ and the equilibrium input. 

## 2 Implementation

### 2.1 Nonlinear Model and Linearization

The nonlinear model, as described in section 4.1, is implemented in MATLAB in `model/linearization.m` with symbolic variables for
states and commands. Setting all other values to zero, the equilibrium throttle input is found using this model with a binary search
for the correct command value. The $`A`$ and $`B`$ state-space matrices are determined with symbolic differentiation, so the linearization
is automatically updated if the model is improved (or simplified) or if the equilibrium point is manually changed.

### 2.2 LQR Controller

Matt Rich describes two different weighting schemes for the matrices $`Q`$ and $`R`$. The first makes each of these matrices diagonal
(and $`N = 0`$), so the controller tries to separately minimize each of the state variables. The MATLAB function in `model/initial_lqr.m`
computes this controller when given the state-space matrices $`A`$ and $`B`$, as well as vectors for state values and command values.
These vectors should correpsond to approximately the maximum allowable value for each state or command (for example, you might set
the 10th value of the state weights to $`\frac \pi 6`$ if you don't want the quad to pitch more than about $`30^o`$).  

Rich also gives a weighting scheme broken down by what he refers to as subsystems, of which there are four: altitude, lateral,
longitudinal, and direction. The matrix $`Q`$ then ends up block diagonal (after permutation to make subsystems adjacent).
The logic behind this is that, for example, if the quad is too far to the right, rolling leftward should reduce the cost rather than
increase it, because that is the action required to return to equilibrium. Given the $`c`$ and $`d`$ matrices Rich defines in 7.4.2,
the matlab function in `improved_lqr.m` will construct the corresponding controller for this weighting style. Unfortunately, this design
has never flown successfully in simulation.

### 2.3 Automated Optimization of LQR Weights

Due to a lack of controls knowledge and intuition in our team, we implemented a MATLAB script to pick weights for the diagonally-weighted
LQR controller design. The script `repeated_opt.m` picks a random set of weights, then uses MATLAB's included contstrained minimization function
(`fmincon()`) to search for weights that reduce the actual error when run through the Simulink simulation of the nonlinear model (recall that
the controller itself was built with only the linearization, so it doesn't represent extreme angles and speeds well). This process is repeated,
and each set of weights is stored in a file along with a error score (lower implies a better controller).  
 
The function passed to `fmincon()` is `lqr_err()`, which transforms inputs from a subset of the weights to the full weight list and calls the function
to create the LQR $`K`$ matrix. We assume the lateral and longitudinal weights should be the same, which reduces the dimension of the space for the
minimization to search to 11 instead of 16. Internally, `lqr_err()` calls `run_once()`, which simply runs the Simulink model and returns the
error between state and setpoint as the output.  

Warning: The LQR controller designed as above (or in any way) has not been tested on our quadcopter. The simulation is a good place to verify that
the control conceptually works, but don't assume that it will necessarily act the same way on the quad.

### 2.4 C Implementation

The existing PID controller on the quad is run on the [computation graph][ctrl_graph], which only currently handles scalar values. An implementation
for matrix constants and matrix multiplication blocks is available on the branch `53-add-lqr-controller-to-c`, but the controller itself is not
finished or connected. To implement the LQR controller (or any sort of state-feedback style controller) on the quad, either this implementation
needs to be finished or an entirely new implementation needs to replace the computation graph.

## 3 Errata to "Model Development..."

Like any 150-page technical work, Rich's thesis contains its share of mistakes and errors. Most are minor typos or inconsistencies that largely
have no impact on the readability or usability of the work. There are a few points, however, that deserve note here to clarify discrepencies
should anyone want to replicate his work again.  

In section 4.3.1, gravity in the body reference frame should depend on $`\phi`$ rather than $`\psi`$ (roll affects the direction of gravity, but yaw shouldn't).
The actual vector of graviational force should be
```math
^BG = \begin{bmatrix}
-mg\sin(\theta)\\
mg\cos(\theta)\sin(\phi)\\
mg\cos(\theta)\cos(\phi)\\
\end{bmatrix}
```  

In section 4.6, the outputs of the camera model should be defined as
```math
Y_c[n] = \begin{bmatrix}
x(nT_c - \tau_c)\\
\vdots\\
\psi(nT_c - \tau_c)\\
\end{bmatrix}
```
As written in the original, the actual period was dependent on both the period variable $`T_c`$ and the latency $`\tau_c`$.
This model is not required for designing a controller, but is necessary if creating a simulation that includes inputs from the camera system.  

In section 7.3.1, the list of weights that Rich chose for his test are missing for $`u`$, $`v`$, $`w`$, $`p`$, $`q`$, and $`r`$, and aren't
exactly clear for the rest. This was one of the reason for the automated weight testing MATLAB script.

In sections 7.4.2.3 and 7.4.2.4, the $`c`$ matrices should be
```math
c_{\text{alt}} = c_{\text{dir}} = \begin{bmatrix}
1 & 0\\
0 & \sqrt{10}\\
0 & 0
\end{bmatrix}
```
in order to create the block-diagonal $`Q`$ given in 7.4.2.4. Also note that this $`Q`$ should be symmetric. The third
value in the top row of the lateral block is $`0.02`$, and the element symmetric from this one should be $`0.02`$ as
well, not $`0.2`$. As written, $`Q`$ cannot be used in MATLAB to genenerate a LQR controller because it isn't
positive semidefinite.  

[model_dev]:   http://lib.dr.iastate.edu/cgi/viewcontent.cgi?article=3777&context=etd
[ctrl_graph]:  ../../quad/src/computation_graph/README.md