To do this elective task you need access to Matlab with the control system toolbox.

You submit your solution here.

There are three subparts.

C.1 illustrates typical control system analysis and design, using a simplified model of the height control system we studied in the mandatory task.
C.2 illustrates so called Linear Quadratic controller design, an optimization based approach for which there exists an analytic solution of the optimal controller, a reason for why it is a quite popular choice. It is also the basis for the nowadays quite popular Model Predictive Control (MPC) method.
C.3 illustrates controller design by a method based on convex optimization. This part requires you to download and install a matlab package (CVX) for convex optimization. On Mac the installation has unfortunately recently become a little of a struggle, but should be possible with the advice given below.

Alternative to C: If you think these exercises concern material you already know well and want some more challenging task instead, let us know and we will find something together.

C.1 Model-based analysis of drone control (height control).

We will now analyze the behavior of the flying robot that we experimented with in the ROS simulation environment. The mathematical model we will use will be an approximation of the dynamics, where we neglect effects such as

There are limitations on the thrust that can be generated from the motors, both an upper and lower limit exists in reality
The amount of physical thrust generated for a certain command signal varies over time, due to e.g. of variations in battery charge level
The physical height h can not be negative ("crashing into ground")
Air resistance will generate a force in the opposite direction to movement; this force typically increases quadratically with speed
The force generated by the motors is impacted by robot speed (during movement upwards, the propellers generate less force; during movement downwards the propellers generate more force)
Wind and air turbulence effects as well as effects of air pressure variations. One such effect if especially noticeable also indoors, when operating a small drone close to a floor, where a "hovercraft" effect arises making the drone extra hard to control.

Even though we neglect many important effects in our mathematical model of the true system, it will often capture the behavior well enough to produce valuable intuition into how to tune your control system.

State-space model of the height dynamics

If we assume the drone can be modeled as an object with mass m influenced by a vertical force LaTeX: f(t) generated by the propellers we get the differential equation $LaTeX: m\ddot h(t) = f(t) - mg$ . To analyze the system we can rewrite this in so called state-space form. Lets choose state vector as $x = [\begin{matrix} h \\ \dot{h} \end{matrix}]$ and also assume that we use a thrust signal LaTeX: f(t) which combines a feed-forward term equal to LaTeX: +mg with a control signal that we will call LaTeX: u(t) , i.e LaTeX: f(t) = mg + u(t) . We then have

$\begin{aligned} \dot{x} (t) & = A x (t) + B u (t) = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] x (t) + [\begin{matrix} 0 \\ k \end{matrix}] u (t) \\ y (t) & = C x (t) + D u (t) = [\begin{matrix} 1 & 0 \end{matrix}] x (t) \end{aligned}$

where we put $k := 1 / m .$ The choice of output signal $y$ reflects that we are interested in the height. We assume here that the height can be measured somehow, for instance by a pressure sensor or by a positioning system.

Start matlab and define the state-space process (with unit mass $k = 1$ ) by

k=1; A = [0 1 ; 0 0]; B=[0 ; k]; C=[1 0]; D=0;
P = ss(A,B,C,D);

Transfer function representation of the process

The process LaTeX: P can also be represented as a so called transfer function: LaTeX: P(s) . This uses notation where the symbol $s$ corresponds to derivation (and its inverse $\frac{1}{s}$ corresponds to integration). In matlab the transfer function is calculated by writing

>> tf(P)
ans =
   1
  ---
  s^2

The transfer function of the process is hence $P (s) = \frac{1}{s^{2}}$ and the relation between output and input is given by LaTeX: Y(s) = P(s) U(s) (if the initial state of the system is zero LaTeX: x(0)=0 ).

The process $P (s)$ above is therefore often called a double-integrator since its effect is to integrate the input signal $u (t)$ twice to produce the output $y (t)$ . (Knowledge of transfer functions, based on so called Laplace transforms, will not be required to follow what we do below.)

Analysis of the PID control system

We will here control the process with a PID controller of the form

$u (t) = k_{p} e (t) + k_{i} \int_{0}^{t} e (τ) d τ + k_{d} \frac{d e (t)}{d t}$

Here $e (t) = r (t) - y (t)$ is the control error and $r (t)$ is the wanted height, it is called a reference signal.

The PID controller consists of three parts:

a proportional part corresponding to the present error,
an integral part describing error history,
a derivative part describing the current error trend.

Reasons for the popularity of PID control is that the three tuning knobs $(k_{p}, k_{i}, k_{d})$ are easy to understand and can often be tuned manually even without deep process knowledge, and that the control structure is often sufficient for acceptable performance.

The transfer function of the PID controller is

$C (s) = k_{p} + \frac{k_{i}}{s} + k_{d} s$

In practice one often uses a filter on the derivative part, to reduce the impact of noise, therefore we will change the last term to $\frac{k_{d} s}{1 + s T_{f}}$ , which corresponds to using a first order low pass filter with time constant LaTeX: T_f to reduce the impact of noise on the derivative term .

Let us start with a P-controller and define the controller in matlab by

s = tf('s');                   % defines the operator s
kp=1; ki=0; kd=0; Tf=0;        % here we only use the P-part
C = kp + ki/s + kd*s/(1+s*Tf); % general PID with filter

We will now use the controller $U = C (s) (R - Y)$ on the process $Y = P (s) U$ , as illustrated in the figure

Let's simulate a step response, assuming the reference $r (t)$ goes from 0 to 1 at time t=0.

(If you want details about what the "feedback" and "step" command to, write "help feedback" etc)

Gcl = feedback(P*C,1); % y = Gcl r
Tsim = 10;             % simulation time
step(Gcl, Tsim);       % step response

which gives the following disappointing result

The height fluctuates between 0 and 2 instead of approaching the reference height 1 :-(

Try other values of $k_{p}$ and see how the result is changed.

kp = 9; C=kp   % try both increasing and decreasing kp
Gcl = feedback(P*C,1);
hold on        % keep the old plot 
step(Gcl, Tsim);

You will find that the oscillations are un-damped, have the same amplitude as before, but have 3 times higher frequency.

(Remark: The reason that the double integrator controlled with a P-controller behaves like an undamped "mass hanging in a spring"-system is that the resulting dynamical equations are exactly the same. A force is generated which is proportional to a spatial displacement; and changing the kp value has the same effect as changing the spring constant.)

Let us now try the PID controller parameters

kp=3; ki=1; kd=3; Tf=0.1;      % 
C = kp + ki/s + kd*s/(1+s*Tf); %
Gcl = feedback(P*C,1);         % calculate y
Gu = feedback(C,P);            % calculate u
figure
subplot(211); step(Gcl, Tsim); % output y
subplot(212); step(Gu,Tsim)    % input u

The height now converges in 3 seconds, with a maximum input corresponding to an acceleration of $LaTeX: 33 \,\textrm{m}/{s}^2$ . We also notice an overshoot of 25 percent.

Further experiments with varying control parameters would show that there is a compromise between speed of the height controller and the control signal amplitude. (Decreasing settling time to half increases the control signal amplitude a factor 4.)

We should also remember that our simple model does not capture imperfections such as those mentioned above. The simulation results are therefore optimistic. Experience (read "repeated failures") teaches you what effects you can safely neglect in controller design.

Q1: Assume that the force generated from the propellers is influenced by the drone's vertical speed and results in that the force equation is replaced by $LaTeX: f(t) = mg - c\,\dot h+ u(t)$ with .

a) What is the transfer function for this system?

b) Replace with and redo the step response simulations with the P-controller, using the same parameters as you used above. What major difference do you notice with the oscillations ?

c) Also redo the experiments using the same PID controller as before: Compare the step responses when PID control is used on with those for . Do you see a large difference in the resulting step response between the two systems?

C2. Optimization based controller design - Linear Quadratic Design

Many design methods are formulated as optimization of certain goal criteria. It is however not possible to capture all requirements on the controlled system in a single criterion. The goal criterion should therefore not be viewed as something given beforehand. The parameters in the goal criterion give you design knobs you can change to investigate the design space and to understand the system and the design trade-offs.

The main design time is typically spent on iterating

trying different parameters in the goal criterion
calculating the optimal controller (done by a tool)
evaluating the resulting performance in various aspects

We will not be interested here in the mathematical details on how 2. is performed.

Linear Quadratic Control

This design method has been popular since the 60's and is covered in many control courses. The basic variant is called LQR, and is based on a number of optimistic assumptions

a linear system model $\dot{x} (t) = A x (t) + B u (t)$ describes the system
all elements in the state-space vector $x$ can be measured
all these measurements are available without noise to the same computational unit without communication delay
there are no limitations on the control signal $u$
... some more

The goal criterion is in LQR given by

$\begin{aligned} min_{u} \int_{0}^{\infty} (x^{T} Q x + u^{T} R u + 2 x^{T} N u) d t \end{aligned}$

Here $Q, R, N$ are matrices with design parameters giving different weights to combinations of elements in LaTeX: x and LaTeX: u . The criterion reflects a situation where $x = 0$ and $u = 0$ is the ideal situation ("the regulation problem"). The resulting controller will not include any integral action, and will typically not be robust against constant disturbances. (But there are extensions of LQR that fixes this issue, we will however not discuss this here.)

Often the cross-term is left out, i.e. $N = 0$ is used. Also it is popular to restrain the parameter tuning to diagonal matrices for $Q$ and $R$ (but the true experts will often like to use non-diagonal matrices for best performance.)

If we have a good guess of typical acceptable sizes of the different components of $x$ and $u$ , then a good starting point is to guess diagonal elements of LaTeX: Q and LaTeX: R being inversely proportional to the squared sizes of expected amplitudes.

$Q_{i i} \sim \frac{1}{x_{i, t y p}^{2}}, and R_{j j} \sim \frac{1}{u_{j, t y p}^{2}}$

Increasing $Q_{i i}$ will result in a controller that will try to decrease the error in component $x_{i}$ . Increasing $R_{j j}$ will give a controller that doesn't use control signal $j$ so much.

The optimal controller which minimizes the criterion above can be found (from the famous "Riccati equation"). The optimal controller has the form of a simple state-feedback controller

$u (t) = - K x (t)$

The feedback matrix K can be calculated in matlab by the command

K=lqr(sys,Q,R,N)

where "sys" is the system you want to control.

One can easily convince oneself that multiplying both Q and R with the same constant factor, say 10, will not change the resulting controller (the only thing that happens is that the goal criterion is multiplied with 10).

Example continued - height control with LQR

Download the file height_lqr.m that does LQR design for the same double integrator example as before.

Using a diagonal $Q$ matrix we have two tuning knobs in our design: $Q_{11}$ and $Q_{22}$ . (For this system we only have one input signal so the LaTeX: R -matrix will be a scalar, and because of what we discussed above we can assume LaTeX: R=1 ). If we start with investigating equal values $Q_{11} = Q_{22} = q$ and try the values $q = 0.1, 10, 1000$ we get the following results

To follow the change in reference value we have here added a term to the control signal LaTeX: u(t) = l_r r(t) - Kx(t) with the scalar LaTeX: l_r chosen to get the correct output level LaTeX: y=r in stationarity (if you have not take a course in control theory, you do not have to understand the expressions for LaTeX: l_r, Gcl, Gu in the code).

The results for $q = 1000$ are similar to what we had with the PID-controller earlier, but without an overshoot, which is good.

Q2 Use the file height_lqr.mr and try to improve the tuning, making the responses faster, by changing Q. (Hint: Increasing $q$ further does not give a significantly faster step response, it only increases the size of the control signal.) Try to find a tuning of the $Q$ matrix giving results such as in the following figure.

Notice that we with the fastest of the three designs now have a settling time of about 1 second, no overshoot, but still the same size of control signal as before. We have hence made the response 3 times faster without any major drawbacks.

The Kalman filter and LQG control

It is only in small toy examples that the full state space vector $x (t)$ is known. Instead only some partial measurements of $x (t)$ , often corrupted by noise, are available. With linear measurements we have $y (t) = C x (t) + v (t)$ , where $C$ is a known matrix and $v$ is measurement noise.

A common approach is to design a state estimator that takes information about historical known values of $y (t)$ and $u (t)$ and constructs a state-estimate $\hat{x} (t)$ from a model of the system.

In linear quadratic Gaussian (LQG) control an optimal such state estimate is calculated under the assumption that all disturbance and error signals are Gaussian with known variance matrices. If our system is given by

$\begin{aligned} \dot{x} (t) & = A x (t) + B u (t) + G w (t) \\ y (t) & = C x (t) + v (t) \end{aligned}$

where noise covariance matrices are

$E (w w^{T}) = W, E (v v^{T}) = V$

then the optimal filter to obtain $\hat{x} (t)$ is given by the Kalman filter

$\dot{\hat{x}} = A \hat{x} + B u + L (y - C \hat{x})$

where the Kalman gain matrix $L$ can be calculated by the Matlab command

L = lqe(A,G,C,W,V)

An alternative is to used the command kalman (see help kalman).

In reality the LaTeX: W and LaTeX: V matrices are not known (and the noise might not be Gaussian either). The matrices and (and perhaps also LaTeX: G ) are instead treated as tuning knobs used to investigate different properties of the resulting controller and the closed loop system.

The controller that minimizes the expected value of the LQR goal criterion is obtained by inserting the state estimator $\hat{x} (t)$ in the previous formula for the LQR controller, giving

$u (t) = - K \hat{x} (t)$

The LQG controller can in matlab be found by combining the commands lqr, kalman and lqgreg as illustrated below.

Note: The resulting controller will in this "vanilla version" not include any integral action. It will therefore often need to be extended to give good performance.

(If you want more information here is a short intro to the Kalman filter. For more details you can check out the Kalman filter wikipedia page.)

LQG Design - Controlling Two Inverted Pendulums on a cart.

Download and run the file pendulaoncart.m which illustrates LQG-design on a slightly more advanced problem than the drone we studied above. Also download the file plotit.m which visualises the results. Two pendulums are mounted on a common cart that can move horizontally as illustrated in the picture

(Bonus material: If you are interested, a derivation of the dynamics is available in this note, which you need some math to understand).

The task is to balance both pendulums, i.e. get $θ_{1} = θ_{2} = 0$ , by moving the position $x (t)$ of the cart. It is assumed the position of the cart and the angles of the pendulums can be measured and used for feedback control, but not the cart velocity or the angular velocities. The input is the force $u (t)$ on the cart. An LQG controller is calculated by the following lines of code in the file

Q =  C'*C;
R = secretscalarvalue;        % tune here !
[K,S]=lqr(A,B,Q,R);
G = B;
H = 0*C*B;
QN = 1;
RN = diag(secretparameter * [1 1 1]);  % tune here !
syse = ss(A,[B G],C,[D H]);
[kest,L]=kalman(syse,QN,RN);
reg = -lqgreg(kest,K);

Remark: The file pendulaoncart.m generates several additional plots that you shouldn't try to understand, unless you are a control student. These plots illustrate the behavior of the system at different frequencies and can be used to understand the design better, such as the robustness against disturbances and modeling errors. If you are a control student, you should make sure you understand these plots.

The following movie (generated by the file plotit.m) illustrates how the resulting control system handles a situation where the initial condition is close to the goal, but with the two pendulums leaning slightly in different directions ( $θ_{1} (0) = 1 degree, θ_{2} (0) = - 2 degrees$ ).

It is perhaps unintuitive that both pendulums can be balanced by the same cart. Using control theory it can be proved that this impossible if the pendulums have the same length. It will also be practically impossible if the lengths of the pendula are close to each other (Theoretically possible, but in practice requiring very good sensors, fast actuators and no model mismatches, like friction etc).

Q3 Tune the two parameters (secretscalarvalue and secretparameter) so that the controller manages to stabilize the cart-pendulum system. You should see a "PASS" sign when the simulations has finished. It is assumed that the cart fails by hitting a wall if its x-position exceeds +- 0.08m.

C3. Optimization based controller design by Convex Optimization using CVX

Background: Many recent algorithms for controller design, as well as in machine learning, signal processing and other areas, involve formulating convex optimization problems (see wikipedia for more background). We will use a toolbox for matlab that allows you to define problems using a natural mathematical syntax. The CVX software defines a language for "disciplined convex programming". The user specifies an objective and set of constraints by combining constants, variables, and parameters using a library of functions with known mathematical properties. Following the language syntax guarantees that the resulting optimization problem can be solved by convex optimization.

Installation: Download the CVX package suitable for your computer from this page (choose the "free distribution" and follow the installation instructions. Execute the command cvx_setup in matlab from the directory where you downloaded cvx. On Mac you will unfortunately nowadays have to follow these instructions to get your operating system to allow you to run the installation code. Expect the installation to take about 8 iterations of "cvx_setup" before finally succeeding.)

Rocket Landing Problem: We will demonstrate the convex optimization method on a Rocket Landing Problem.

(If you want an illustration, or maybe try out the rocket landing problem yourself first, go to control challenges and choose "Rocket Landing", and

After installing CVX, download the code rocketcvx.m which uses convex optimization to find a control signal that takes the rocket from a (known) initial condition to landing at the platform. At the start the rocket is 200 meter above the landing platform and has a horizontal position error of 20 meter. It finds a control signal trajectory $u (t)$ for the last LaTeX: T=10 seconds of operation that lands the rocket on the platform, i.e. giving the correct final state $x (T)$ . There are two control signal $u_{1}$ and $u_{2}$ describing sideways and upwards thrust respectively.

Read the code and try to understand how the problem is formulated. (if you are interested, a derivation of the used Rocket dynamics is given in this note.) Even if the code manages to land the rocket, some improvement is possible. For a nice landing you want low final velocity in the vertical direction and very low velocity in the horizontal direction.

The optimization criterion is chosen, somewhat randomly, as minimization of $LaTeX: \left(\int_0^T |u_1(t)|^2\right)^{1/2} + \left(\int_0^T |u_2(t)|^2 dt\right)^{1/2}$ and we also enforce hard limits on sideways thrust $u_{1, m i n} \leq u_{1} (t) \leq u_{1, m a x}$

Here are some ideas to investigate

Buy larger lateral thrust rockets (increase the allowed range of $LaTeX: [u_{1,min}, u_{1,max}]$ ). Not appreciated by higher management
Try other optimization criteria to be minimized
Try other restrictions on the trajectory, such as to enforce $u_{1} (t) = 0$ during final part of the trajectory
Try other final constraints on the state

Also experiment with changing the initial position of the rocket making sure you solutions remains good.

Try to achieve something that looks similar to this:

For further study you see the CVX example page and consult the CVX documentation.

Q4 Describe briefly your final CVX design and a movie illustrating your landing. (In the given file there are outcommented code for how to generate such a movie).

Extra: control challenges for those who feel understimulated...

A more serious alternative is to try out embedded code generation using e.g. CVXPYGEN: https://github.com/cvxgrp/cvxpygen