Module2 - mtrl - Control

Tools

We will use Matlab and its Control System Toolbox so make sure you have it available (it is not available in the virtual machine). If you already have a good knowledge of control, you can skip through the background material quickly and go directly to the material on convex optimization below.

Background

If you have no or only little experience of control you might want to start with watching the introduction videos 1 and 2 (out of 46) in this series of control lectures. Links to an external site.

Make sure you understand the fundamental difference between open loop control and feedback control. The main need for the use of feedback control is the presence of uncertainty (disturbances, imperfect models etc...). Without uncertainty there would be little need for feedback. In the safe world of simulations, where you can assume perfect knowledge of system dynamics and where there are no disturbances you can easily miss the importance. You however quickly notice if you, for some strange reason, want to move from theory to practice.

If you want to start by trying out things yourself, then spend some time (remember your time budget so do not get stuck in all the fun in there) on some of these control challenges Links to an external site.. You could check out the "Cruise Control", "Inverted Pendulum" and "Rocket Landing" problems, we will return some of these later. (Don't get stuck here though, even if you think it is fun...)

Watching the videos above is enough to continue, but if you want even more background before going further, we recommend reading Chapter 1 in the book Feedback Systems, by Åström and Murray. Links to an external site. (Karl Johan Åström is professor emeritus in Lund, Richard Murray is professor at Caltech, and member of the advisory board of WASP)

A control design is typically a trade-off between competing objectives. To do a good design one needs to understand the existing trade-offs and then find a good compromise. The conclusion can be that the objectives are impossible to achieve because of fundamental limitations, and that the only solution is to redesign the system, for instance adding new sensors getting a stronger motor. A good design method should provide the user with appropriate "tuning knobs" for investigating and understanding the trade-offs.

We will start by doing a control design using the most popular method there is: the PID controller, which has three tuning knobs, that often are tuned by hand.

If you want more background on PID control before we start, you can watch these videos

PID intro Links to an external site.PID examples Links to an external site.

Design example 1 - Height control of a helicopter (with PID control)

Download the Matlab code height_PID.m Download height_PID.m

and step through it in matlab, while going through the following text.

We will start by going through a control design of a simple system. A dynamical model of the height control of e.g. the Crazyflie is given by

$LaTeX: m\ddot h(t) = f(t)-mg$ $m\ddot h(t) = f(t)-mg$

Here $h (t)$ $h(t)$ denotes the height, which we want to follow a certain goal $r (t)$ $r(t)$ (also called reference), for instance $r (t)$ $r(t)$ =1 means we want the height to be 1 meter. The signal $f (t)$ $f(t)$ is the vertical force which we can control by adjusting the rotor speed. The parameter $g$ $g$ is the constant of gravity. To hover at a constant height we clearly need LaTeX: f(t)=mg $f(t)=mg$ .

To analyze the system we will write it in so called state-space form. The state vector is chosen as $x = [\begin{matrix} h \\ \dot{h} \end{matrix}]$ $x = \begin{bmatrix} h \\ \dot h \end{bmatrix}$ and the resulting system is

$\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}$ $\begin{align*} \dot x(t) &= Ax(t)+ Bu(t) = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}x(t) + \begin{bmatrix} 0 \\ k \end{bmatrix} u(t)\\ y(t)&=Cx(t) + Du(t) = \begin{bmatrix} 1 & 0 \end{bmatrix} x(t) \end{align*}$

where we have defined a new control signal LaTeX: u(t) := f(t)-mg $u(t) := f(t)-mg$ and put $k := 1 / m .$ $k:=1/m.$ The choice of output signal $y$ $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$ $k=1$ ) by

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

The process P can also be represented as a so called transfer function. In matlab it is calculated by writing

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

The transfer function of the process is hence $P (s) = \frac{1}{s^{2}}$ $P(s) = \frac{1}{s^2}$ .

The operator $s$ $s$ corresponds to derivation and its inverse $\frac{1}{s}$ $\frac{1}{s}$ to integration. The process $P (s)$ $P(s)$ above is therefore often called a double-integrator since its effect is to integrate the input signal $u (t)$ $u(t)$ twice to produce the output $y (t)$ $y(t)$ . We will avoid using transfer functions in the presentation below and understanding of these will not be required.

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}$ $u(t) = k_pe(t) + k_i \int_0^t e(\tau)\,d\tau + k_d \frac{de(t)}{dt}$

Here $e (t) = r (t) - y (t)$ $e(t)=r(t)-y(t)$ is the control error and $r (t)$ $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})$ $(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$ $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}}$ $\frac{k_ds}{1+sT_f}$ . Define the controller by

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

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

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

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}$ $k_p$ and see how the result is changed.

kp = 10; C=kp   % try both increasing and decreasing kp
Gcl = feedback(P*C,1);
step(Gcl, Tsim);

(Remember your conclusions here, since it will be asked for in the quiz.)

We get better results by using the following 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
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 3g. We also notice an overshoot of 25 percent.

Further experiments 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

control signal saturation (rotor speed is limited and negative rotor speed might be impossible),
measurement errors (the height measurement might be inaccurate),
uncertainty in process parameters (we assumed we knew exactly the amount of thrust the motors generate, we also assumed the mass was known),
unmodelled dynamics (we assumed the thrust could be changed immediately without any lag).

The simulation results are therefore optimistic. Experience (read "repeated failures") teaches you what effects you can safely neglect in controller design.

Optimization based controller 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 60s 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)$ $\dot x(t) = Ax(t) + Bu(t)$ describes the system
all elements in the state-space vector $x$ $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$ $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}$ $\begin{align} \min_{u}\int_0^\infty (x^TQx + u^TRu + 2x^TNu)\,dt \end{align}$

Here $Q, R, N$ $Q,R,N$ are matrices with design parameters giving different weights to combinations of elements in x and u. The criterion reflects a situation where $x = 0$ $x=0$ (and $u = 0$ $u=0$ ) is the ideal situation.

Often the cross-term is left out, i.e. $N = 0$ $N=0$ is used. Also it is popular to use diagonal matrices for $Q$ $Q$ and $R$ $R$

If we have a good guess of typical acceptable sizes of the different components of $x$ $x$ and $u$ $u$ , then a good starting point is to guess diagonal elements of Q and R as

$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}}$ $Q_{ii} \sim \frac{1}{x_{i,typ}^2}, \quad \textrm{and } \quad R_{jj}\sim \frac{1}{u_{j,typ}^2}$

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

The optimal controller has the form

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

where the feedback matrix K is calculated in matlab by the command

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

Design Example 1 continued - height control (with LQR)

Download the file height_lqr.m Download height_lqr.m

that does LQR design for the same double integrator example as before.

Using a diagonal $Q$ $Q$ matrix we have two tuning knobs in our design, $Q_{11}$ $Q_{11}$ and $Q_{22}$ $Q_{22}$ (if you only have one control signal you can always assume $R = 1.$ $R=1.$ ) If we try equal values $Q_{11} = Q_{22} = q$ $Q_{11}=Q_{22}=q$ and set $q = 0.1, 10, 1000$ $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 $u (t) = k_{r} r (t) - K x (t)$ $u(t) = k_r r(t) - Kx(t)$ with the scalar k_r chosen to get the correct output level in stationarity.

The results for $q = 1000$ $q=1000$ are similar to what we had with the PID-controller earlier (without the overshoot).

Exercise: Try improving this design by changing Q using the file height_lqr.m Download height_lqr.m

. (Hint: Increasing $q$ $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 $Q$ $Q$ giving results such as in the following figure. This is asked for in the quiz.

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.

LQG design

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

A common approach is to design a state estimator that takes information about historical known values of $y (t)$ $y(t)$ and $u (t)$ $u(t)$ and constructs a state-estimate $\hat{x} (t)$ $\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}$ $\begin{align*} \dot x(t) &= Ax(t) + Bu(t) + Gw(t) \\ y(t) &= Cx(t) + v(t) \end{align*}$

where noise covariance matrices are

$E (w w^{T}) = W, E (v v^{T}) = V$ $E(ww^T) = W, \quad E(vv^T)=V$

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

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

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

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

In reality the W and V matrices are not known (and the noise might not be Gaussian either). The matrices W and V (and perhaps also 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)$ $\hat x(t)$ in the previous formula for the LQR controller, giving

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

Design Example - Double Inverted Pendulums (with LQG)

The file pend01.m Download pend01.m

illustrates LQG-design on a slightly more advanced problem than before. Two pendulums are mounted on a common cart that can move horizontally as illustrated in the picture

Additional: If you are interested, a derivation of the dynamics is available in this note Download in this note

(which you need some math to understand).

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

Q =  C'*C;
R = 1e-4;
[K,S]=lqr(A,B,Q,R);
G = B;
H = 0*C*B;
QN = 1;
RN = diag([1e-3 1e-3 1e-3]);
syse = ss(A,[B G],C,[D H]);
[kest,L]=kalman(syse,QN,RN);
reg = -lqgreg(kest,K);

Remark: The file pend01.m Download pend01.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.

The following movie (generated by the file plotit.m Download 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

$\theta_1(0) = 1 \textrm{ degree}, \quad \theta_2(0) = -2 \textrm{ degrees}$ ).

Play media comment.

It is interesting to note 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.

Controller design by Convex Optimization

Download the CVX addon to Matlab available at this home page Links to an external site.(choose the "standard bundle" and follow the installation instructions.)

In the LQG method we formulated the control design problem as a minimization of an integral of a quadratic function of states and control. As long as the goal function is a convex function of the states and control one can find the control signal by convex optimization.

We will demonstrate the convex optimization method on the Rocket Landing Problem (that was part of the control challenges Links to an external site. which we linked at the top of this page).

If you are interested, a derivation of the used Rocket dynamics is given in this note Download this note

The CVX software defines a language for "disciplined convex programming". Following the language syntax guarantees that the resulting optimization problem can be solved by convex optimization.

After installing CVX, download the code rocketcvx.m Download rocketcvx.m

that 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)

$u(t)$ for the last T=10 seconds of operation that lands the rocket on the platform, i.e. giving the correct final state

x (T)

$x(T)$ . There are two control signal

u_{1}

$u_1$ and

u_{2}

$u_2$ describing sideways and upwards thrust respectively.

The optimization criterion is chosen, somewhat randomly, as minimization of $max_{t} (| u_{1} (t) |) + {(\int_{0}^{T} | u_{2} (t) |^{2} d t)}^{1 / 2}$ $\max_t \left(|u_1(t)|\right) + \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}$ $u_{1,min}\leq u_1(t) \leq u_{1,max}$

Furthermore we enforce $u_{1} (t) = 0$ $u_1(t) = 0$ during the last 3 seconds of operation, to encourage the rocket to do a final vertical approach.

cvx_begin
    n = length(x1);
    variables x(n,T) u(2,T-1);
    minimize(max(abs(u(1,:))) + norm(u(2,:)));
    subject to
      x(:,1) == x1;
      for t = 1 : T-1
         x(:,t+1) == Phi*x(:,t) + Gamma*u(:,t);
         u1min <= u(1,t) <= u1max 
      end
      for t = T-30 : T-1   % forces the rocket do a purely vertical approach
         u(1,t) == 0       
      end
      x(:,T) == xfinal;
cvx_end

The result should look something like the following movie

Play media comment.

Spend some time studying the code so you roughly understand how it works. Also experiment with changing some parameters, such as the initial position, the bounds on lateral thrust or the design of the final vertical approach.

There are many other uses of convex optimization and the CVX software you just installed, for instance in signal processing, statistics, machine learning, .... For further study see the CVX example page Links to an external site.and consult the CVX documentation. Links to an external site.

Additional: CVXGEN - generate efficient C-code for Quadratic Programming problems

We will demonstrate how to generate fast C-code that solves convex QP-problems with some hundred or so optimization variables within a millisecond. We will use a web-service that transforms a CVX-like description of your optimization problem and automatically generates C-code that can be used from within matlab, or can be embedded into your realtime production code.

If you want to generate code yourself, you will need to follow the instructions at cvxgen.com Links to an external site.. Note that CVXGEN is only available for free to academic, non-commercial users with an academic email address, and you will need that to request for an academic license.

The work flow is shown in the series of cvxgen screenshots at this page. Links to an external site.

The tool can be used to generate support vector machines, solve so called Lasso problems used in statistical modeling, etc. Here we will demonstrate it by implementing a so called Model Predictive Controller (MPC).

Model predictive control is based on solving repeated optimization problems using so called receding horizon control. The MPC method is explained in this video Links to an external site.

To generate code for an MPC controller for the double integrator we took the lines of code in the file inputtocvxgen.m Download inputtocvxgen.m

and pasted it into the cvxgen webservice (after requesting an academic license). Following the workflow on the cvxgen home page we ended up with a number of files, including the solver codes cvxsolve (matlab-code), csolve.c (C-code) and csolve.mexmaci64 (mex-code to use the C code within matlab on a Mac computer).

The matlab solver was then bench-marked against the generated C-solver in the file test_cvxgen.m Download test_cvxgen.m

which implements a simple MPC controller for the double integrator. The running time for solving the optimization problem in matlab with cvxsolve.m was about 3 seconds per time step and using the csolve.mexmaci64 it was 50 microseconds (a speedup of about 60000 !).

If you think it might be useful to be able to generate fast embedded C-code for quadratic optimization problems you might want to redo the example presented above. (You will not have to use this method in the control of the Crazyflie.)

Additional: Evaluation of control design (Gang of Four)

After designing the control system (or better, while doing it) we should try to understand its performance. A good complement to time-domain simulation is to study the systems also in the frequency domain. We will describe the so called Gang of Four figure.

The GoF contain four different amplitude curves as a function of frequency. For a process P(s) and a controller C(s) one puts $s = i ω$ $s=i\omega$ and plots the amplitude of the following transfer functions as a function of frequency $ω$ $\omega$

$\begin{aligned} Sensitivity function S = \frac{1}{1 + P C} \\ Load Sensitivity function P S = \frac{1}{1 + P C} \\ Noise Sensitivity function C S = \frac{C}{1 + P C} \\ Complementary Sensitivity function T = \frac{P C}{1 + P C} \end{aligned}$ $\begin{align*} &\textrm{Sensitivity function } S = \frac{1}{1+PC} \\ &\textrm{Load Sensitivity function } PS = \frac{1}{1+PC} \\ &\textrm{Noise Sensitivity function } CS = \frac{C}{1+PC} \\ &\textrm{Complementary Sensitivity function } T= \frac{PC}{1+PC} \end{align*}$

Read the first 3 pages of Chapter 12 in Åström Murray Links to an external site. to get an explanation of what these functions illustrate. For instance a low value of the sensitivity function $S$ $S$ means that the closed loop system attenuates disturbances well, and a high amplitude on $C S (i ω)$ $CS(i\omega)$ at a certain frequency $ω$ $\omega$ means that measurement noise at this frequency will have a large effect on the control signal (which is bad).

Transformation to discrete time

Transformation of a continuous time system sysc = ss(A,B,C,D)

$\begin{aligned} \dot{x} (t) & = A x (t) + B u (t) \\ y (t) & = C x (t) + D u (t) \end{aligned}$ $\begin{align*} \dot x(t) &= Ax(t) + Bu(t) \\ y(t)&= Cx(t) + Du(t) \end{align*}$

to discrete time with sample time $h$ $h$

$\begin{aligned} x (k h + h) & = Φ x (k h) + Γ u (k h) \\ y (k h) & = C x (k h) + D u (k h) \end{aligned}$ $\begin{align*} x(kh+h) &= \Phi x(kh) + \Gamma u(kh) \\ y(kh) &= Cx(kh) +Du(kh) \end{align*}$

can be done by the commands

sysd = c2d(sys,h)
[Phi, Gamma,C,D] = ssdata(sysd)

Words of wisdom from the master

Play media comment.