## Design an LQR Servo Controller in Simulink

The following figure shows a Simulink® block diagram shows a tracking problem in aircraft autopilot design. To open this diagram, type `lqrpilot` at the MATLAB® prompt.

Key features of this diagram to note are the following:

• The Linearized Dynamics block contains the linearized airframe.

• `sf_aerodyn` is an S-Function block that contains the nonlinear equations for $\left(\theta ,\varphi \right)=\left(0,{15}^{\circ }\right)$.

• The error signal between $\varphi$ and the ${\varphi }_{ref}$ is passed through an integrator. This aids in driving the error to zero.

### State-Space Equations for an Airframe

Beginning with the standard state-space equation

`$\stackrel{˙}{x}=Ax+Bu$`

where

`$x={\left[u,v,w,p,q,r,\theta ,\varphi \right]}^{T}$`

The variables u, v, and w are the three velocities with respect to the body frame, shown as follows.

Body Coordinate Frame for an Aircraft

The variables $\varphi$ and $\theta$ are roll and pitch, and p, q, and r are the roll, pitch, and yaw rates, respectively.

The airframe dynamics are nonlinear. The following equation shows the nonlinear components added to the state space equation.

Nonlinear Component of the State-Space Equation

`$\stackrel{˙}{x}=Ax+Bu+\left[\begin{array}{c}-g\mathrm{sin}\theta \\ g\mathrm{cos}\theta \mathrm{sin}\varphi \\ g\mathrm{cos}\theta \mathrm{cos}\varphi \\ 0\\ 0\\ 0\\ q\mathrm{cos}\varphi -r\mathrm{sin}\varphi \\ \left(q\mathrm{sin}\varphi +r\mathrm{cos}\varphi \right)\cdot \mathrm{tan}\theta \end{array}\right]$`

To see the numerical values for A and B, type

```load lqrpilot A, B ```

at the MATLAB prompt.

### Trimming

For LQG design purposes, the nonlinear dynamics are trimmed at $\varphi ={15}^{\circ }$ and p, q, r, and θ set to zero. Since u, v, and w do not enter into the nonlinear term in the preceding figure, this amounts to linearizing around $\left(\theta ,\varphi \right)=\left(0,{15}^{\circ }\right)$ with all remaining states set to zero. The resulting state matrix of the linearized model is called `A15`.

### Problem Definition

The goal to perform a steady coordinated turn, as shown in this figure.

Aircraft Making a 60° Turn

To achieve this goal, you must design a controller that commands a steady turn by going through a 60° roll. In addition, assume that θ, the pitch angle, is required to stay as close to zero as possible.

### Results

To calculate the LQG gain matrix, `K`, type

```lqrdes ```

at the MATLAB prompt. Then, start the `lqrpilot` model with the nonlinear model, `sf_aerodyn`, selected.

This figure shows the response of $\varphi$ to the 60° step command.

Tracking the Roll Step Command

As you can see, the system tracks the commanded 60° roll in about 60 seconds.

Another goal was to keep θ, the pitch angle, relatively small. This figure shows how well the LQG controller did.

Minimizing the Displacement in the Pitch Angle, Theta

Finally, this figure shows the control inputs.

Control Inputs for the LQG Tracking Problem

Try adjusting the `Q` and `R` matrices in `lqrdes.m` and inspecting the control inputs and the system states, making sure to rerun `lqrdes` to update the LQG gain matrix `K`. Through trial and error, you may improve the response time of this design. Also, compare the linear and nonlinear designs to see the effects of the nonlinearities on the system performance.