Contents

• PID
  • How to compute the proportional feedback term
  • How to compute the integral feedback term
  • How to compute the derivative feedback term
• State space models
  • What is a state space model?
  • How do I put a system in state space form?
    • Example (first-order)
    • Example (second-order)
• The matrix exponential function
  • What is the matrix exponential?
  • How do I use the matrix exponential to solve a linear system?
  • How do I use the matrix exponential to solve state space models?
• Asymptotic stability
  • What are eigenvalues and eigenvectors?
  • How do I diagonalize a square matrix?
  • What is the matrix exponential of a diagonal matrix?
  • What is the solution to a linear system that is diagonalizable?
  • When is a linear system asymptotically stable?
• Eigenvalue placement by controllable canonical form
  • Eigenvalues are invariant to coordinate transformation
  • Controllable canonical form
  • Putting a system in controllable canonical form
  • A systematic process for control design
  • How to decide when eigenvalue placement is possible
• Optimization and optimal control
  • Optimization
    • Minimum vs. Minimizer
  • Optimal Control
    • Statement of the problem
    • The HJB equation (our new “first-derivative test”)
    • Solution approach
  • LQR
    • Statement of the problem
    • Do the matrices $Q$, $R$, and $M$ really have to be symmetric?
    • Solution to the problem
    • LQR Code
• State estimation
  • What is a state-space model with output?
  • How do I linearize a sensor model?
    • Example (linear sensor model)
    • Example (nonlinear sensor model)
• Optimal observer design
  • Statement of the problem
  • Solution to the problem
  • Summary
  • Comparison between LQR and KF (i.e., why you can use “LQR” in Python to compute an optimal observer)
• Reference tracking
  • Reference tracking with full state feedback
  • Reference tracking with partial state feedback
  • Tracking more than one element of the state
  • Choosing the desired state to keep errors small

---

PID

The proportional-integral-derivative or PID controller is a simple, widely-used, and often very effective way to make a dynamic system do what you want.

Suppose this dynamic system has exactly one real-valued input $u(t)$ and one real-valued output $y(t)$. Suppose our goal is to choose the input $u(t)$ so that the output $y(t)$ converges to a desired reference value $r$. Define the error as

$$e(t)=y(t)-r.$$

The PID controller is

$$u(t)=-k_{p}e(t)-k_i{\int }_0^{t}e(\tau )d\tau -k_d\frac{de(t)}{dt}$$

for some choice of constant gains $k_p$, $k_i$, and $k_d$. The first term in this controller is “proportional feedback,” the second term is “integral feedback,” and the third term is “derivative feedback.”

How to compute the proportional feedback term

We assume the controller is given $r$ and has access to a measurement of $y(t)$, so computing the term

$$-k_{p}e(t)$$

is just arithmetic.

How to compute the integral feedback term

To compute the term

$$-k_i{\int }_0^{t}e(\tau )d\tau$$

we need to find the integral

$${\int }_0^{t}e(\tau )d\tau$$

at the current time $t$. In most cases, we cannot find this integral exactly even if we wanted to, because we do not know $y(t)$ — and, therefore, $e(t)$ — at all times $t\in \mathbb{R}$, but rather only at sampled times

$$t\in \{0,\mathrm{\Delta }t,2\mathrm{\Delta }t,3\mathrm{\Delta }t,\dots \}$$

for some time step $\mathrm{\Delta }t>0$. For this reason, it is common to use the right-hand rectangle method of numerical integration:

$${\int }_0^{t}e(\tau )d\tau \approx {\int }_0^{t-\mathrm{\Delta }t}e(\tau )d\tau +\mathrm{\Delta }t\cdot e(t).$$

Here is another way of thinking about this. Suppose we define a new variable

$$z(t)={\int }_0^{t}e(\tau )d\tau$$

to contain the integral of the error. It is equivalent to say that $z(t)$ is described by the first-order ordinary differential equation

$$\frac{dz(t)}{dt}=e(t)$$

with the initial condition

$$z(0)=0.$$

Then, it is common to use the backward Euler method

$$z(t)\approx z(t-\mathrm{\Delta }t)+\mathrm{\Delta }t\cdot e(t)$$

to compute an approximation to $z(t)$. Applying the backward Euler method to find $z(t)$ is exactly the same as applying the right-hand rectangle method to find the integral that $z(t)$ represents.

To implement the backward Euler method with code, you need to define a variable to store the prior value $z(t-\mathrm{\Delta }t)$ of the error integral, and you need to update the value of this variable at each time step. You will have to initialize the error integral to something when you start your controller — it is common to start with a value of zero.

How to compute the derivative feedback term

To compute the term

$$-k_d\frac{de(t)}{dt}$$

we need to find the derivative

$$\frac{de(t)}{dt}$$

at the current time $t$. Just like for the integral term, we usually cannot find this derivative exactly. So, just like it is common to use the backward Euler method to find the integral of the error, it is common to use the backward finite difference method to find the derivative of the error:

$$\frac{de(t)}{dt}\approx \frac{e(t)-e(t-\mathrm{\Delta }t)}{\mathrm{\Delta }t}.$$

To implement the backward finite difference method with code, you need to define a variable to store the prior value $e(t-\mathrm{\Delta }t)$ of the error, and you need to update the value of this variable at each time step. Again, you will have to initialize this prior error to something when starting your controller — it is common to start with a value of zero.

State space models

What is a state space model?

A state-space model is a set of ordinary differential equations that can be written in this form:

$$\dot{x}=Ax+Bu$$

There are two variables:

• $x$ is the state
• $u$ is the input

Both variables are functions of time, so we could have written $x(t)$ and $u(t)$ — but in general, we won’t use that notation unless we need it. We use dots to indicate time derivatives, so $\dot{x}$ means $dx(t)/dt$.

The state and input may have more than one element — that is to say, they may be vectors (represented by a matrix of numbers) and not scalars (represented by a single number). Suppose, in particular, that:

• the state $x$ has $n_x$ elements
• the input $u$ has $n_u$ elements

Then, we would represent them as column matrices:

$$x=\left[\begin{array}{c}{x}_1\\ ⋮\\ x_{n_x}\end{array}\right]u=\left[\begin{array}{c}{u}_1\\ ⋮\\ u_{n_u}\end{array}\right]$$

Taking the time derivative of a matrix is equivalent to taking the time derivative of each element of that matrix, so we would write

$$\dot{x}=\left[\begin{array}{c}{\dot{x}}_1\\ ⋮\\ {\dot{x}}_{n_x}\end{array}\right]$$

The state-space model also has two constants: $A$ and $B$. If $x$ and $u$ are column matrices with (possibly) more than one element, then these constants have to be matrices as well:

• $A$ is a constant matrix of size $n_x\times n_x$
• $B$ is a constant matrix of size $n_x\times n_u$

The state space model has two key properties:

• it is linear because both $\dot{x}$ is a linear function of $x$ and $u$
• it is time-invariant because $A$ and $B$ are constant

Other people use other symbols for both the variables and constants in a state space model. Indeed, we will sometimes use other symbols for these things as well. For example: $$\dot{z}=Ez+Fv$$ This is also a "state space model," in which the state is $z$ and the input is $v$.

How do I put a system in state space form?

Suppose we are given a description of a dynamic system. We would like to describe this same system with a state space model.

Remember that every state space model is linear. The equations of motion that describe a dynamic system are often nonlinear. So apparently, we will have to accept that state space models can only approximate some of the systems we want to describe.

We will use linearization to arrive at this approximation, in four steps.

Step 1. Rewrite the system as a set of first-order ordinary differential equations:

$$\dot{m}=f(m,n)$$

In this expression, the variables $m$ and $n$ are functions of time and can have more than one element — in general, you should represent them as column matrices. As we said before, the function $f(\cdot )$ will often be nonlinear.

Step 2. Find an equilibrium point $m_e,n_e$ of the system by solving this equation:

$$0=f(m_e,n_e)$$

A solution to this equation is called an equilibrium point because if

$$m=m_{e}n=n_e$$

then

$$\dot{m}=f(m_e,n_e)=0$$

and so $m$ remains constant. That is to say, if the system reaches an equilibrium point, then it stays there. This is an important property! The goal of almost every controller we design in this course will be to make a system reach an equilibrium point quickly and reliably.

This equation may have no solutions, in which case no equilibrium point exists for the system. This is bad. We will ignore this situation for now.

This equation also may have many solutions, in which case you have to make a choice. A good choice is whatever equilibrium point you would like your system to achieve.

Step 3. Define the state and input as follows:

$$x=m-m_{e}u=n-n_e$$

Note that $x$ measures error — the difference between $m$ and its equilibrium value. When error is zero, the system has reached equilibrium and is doing what we want. Apparently, with this way of defining the state and the input, “control design” means choosing $u$ so that $x$ goes to zero.

Step 4. Compute $A$ and $B$ as follows:

$$A=\frac{\partial f}{\partial m}{|}_{(m_e,n_e)}B=\frac{\partial f}{\partial n}{|}_{(m_e,n_e)}$$

Recall that $$\frac{\partial f}{\partial m}{|}_{(m_e,n_e)}$$ is the Jacobian (i.e., matrix of partial derivatives) of $f$ with respect to $m$, evaluated at the equilibrium point.

Why does this make any sense? Look again at the ODEs that describe the original system:

$$\dot{m}=f(m,n)$$

First, because

$$x=m-m_e$$

then

$$\dot{x}=\dot{m}-0=\dot{m}$$

So the left-hand side of the ODEs can simply be replaced with $\dot{x}$.

Now, suppose we want a linear approximation to the right-hand side of the ODEs — the function $f(m,n)$. One way to find such an approximation is to take a Taylor’s series expansion about $m_e,n_e$ up to first order:

$$\begin{array}{rl}f(m,n)& \approx f(m_e,n_e)+\frac{\partial f}{\partial m}{|}_{(m_e,n_e)}(m-m_e)+\frac{\partial f}{\partial n}{|}_{(m_e,n_e)}(n-n_e)\\ & =0+Ax+Bu\\ & =Ax+Bu\end{array}$$

There you have it: “$\dot{x}=Ax+Bu$” is a first-order (i.e., linear) approximation to “$\dot{m}=f(m,n)$”.

Example (first-order)

Consider the system with dynamics that are described by the following equation of motion:

$$\dot{\omega }+2\omega =\tau$$

Let’s apply our method to put this system in state space form.

We begin by rewriting it as a set of first-order ODEs. Lucky us, the system is already described by just one first-order ODE, so all we need to do is solve for $\dot{w}$:

$$\dot{\omega }=f(\omega ,\tau )=-2\omega +\tau$$

Next, we find an equilibrium point by solving

$$0=-2{\omega }_e+{\tau }_e$$

In this case, there are many solutions. Suppose we pick this one:

$${\omega }_e=10{\tau }_e=20$$

Then, we define the state and input based on this choice of equilibrium point:

$$x=\omega -{\omega }_{e}u=\tau -{\tau }_e$$

Finally, we compute $A$ and $B$ by taking Jacobians (easy in this case because all the variables are scalars):

$$\begin{array}{rl}A& =\frac{\partial (-2\omega +\tau )}{\partial \omega }{|}_{({\omega }_e,{\tau }_e)}=-2\\ B& =\frac{\partial (-2\omega +\tau )}{\partial \tau }{|}_{({\omega }_e,{\tau }_e)}=1\end{array}$$

The resulting state space model is

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ & =-2x+u\end{array}$$

Note that the original system was linear, so there was no approximation here. We could probably have skipped most of these steps and written the system in state-space form by inspection. On the other hand, it is nice to know that the process of “linearization” still works even in this simple case.

Example (second-order)

Consider the system with dynamics that are described by the following equation of motion:

$$\ddot{q}+3\sin q=\tau$$

Let’s apply our method to put this system in state-space form.

We begin by rewriting it as a set of first-order ODEs:

• We find the time-derivative of $q$ with highest order — in this case, $\ddot{q}$, of order 2.
• We define new variables for each time-derivative of $q$ with lower order — in this case, $\dot{q}$, the only time-derivative with order between 0 and 2. We might choose the following name for this time-derivative:

$$v=\dot{q}$$

• We rewrite the original ODEs (in this case, just one) in terms of these new variables:

$$\dot{v}+3\sin q=\tau$$

• We add one extra ODE for each new variable (in this case, just one extra) — this is trivial, coming from the way we defined these new variables:

$$\dot{q}=v$$

• We collect the original ODEs and the extra ODEs together, if necessary solving for all of the time-derivatives (that’s not necessary here):

$$\begin{array}{rl}\dot{q}& =v\\ \dot{v}& =-3\sin q+\tau \end{array}$$

• Finally, we rewrite our result in the form $\dot{m}=f(m,n)$ by collecting things in column matrices as follows:

$$\left[\begin{array}{c}\dot{q}\\ \dot{v}\end{array}\right]=\left[\begin{array}{c}v\\ -3\sin q+\tau \end{array}\right]$$

Note that, as desired, these rewritten ODEs have time derivatives that are at most of first order. Also note that all of these time-derivatives are on the left-hand side of the equations — none appear on the right-hand side.

Next, we find an equilibrium point by solving

$$\left[\begin{array}{c}0\\ 0\end{array}\right]=\left[\begin{array}{c}v\\ -3\sin q+\tau \end{array}\right]$$

There are many solutions. Suppose we pick this one:

In this case, there are many solutions. Suppose we pick this one:

$$q_e=\pi /2v_e=0{\tau }_e=3$$

Then, we define the state and input based on this choice of equilibrium point:

$$x=\left[\begin{array}{c}q-q_e\\ v-v_e\end{array}\right]u=\left[\begin{array}{c}\tau -{\tau }_e\end{array}\right]$$

Finally, we compute $A$ and $B$ by taking Jacobians.

$$\begin{array}{rl}A& =\frac{\partial f}{\partial \left[\begin{array}{c}q\\ v\end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}{\displaystyle \frac{\partial (v)}{\partial q}}& {\displaystyle \frac{\partial (v)}{\partial v}}\\ {\displaystyle \frac{\partial (-3\sin q+\tau )}{\partial q}}& {\displaystyle \frac{\partial (-3\sin q+\tau )}{\partial v}}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}0& 1\\ -3\cos q& 0\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\\ B& =\frac{\partial f}{\partial \left[\begin{array}{c}\tau \end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}{\displaystyle \frac{\partial (v)}{\partial \tau }}\\ {\displaystyle \frac{\partial (-3\sin q+\tau )}{\partial \tau }}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}0\\ 1\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{c}0\\ 1\end{array}\right]\end{array}$$

The resulting state space model is

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ & =\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]x+\left[\begin{array}{c}0\\ 1\end{array}\right]u\end{array}$$

The original system was nonlinear and the state space model is linear, so there must be some approximation here! As we will see, this approximation is good near the equilibrium point but can be very bad elsewhere.

The matrix exponential function

What is the matrix exponential?

The matrix exponential of a square matrix $M$ is the infinite series

$$e^M=I+M+\frac{1}{2}{M}^2+\frac{1}{6}{M}^3+\cdots =\sum _{k=0}^{\mathrm{\infty }}{\displaystyle \frac{1}{k!}}{M}^k$$

where $I$ is the identity matrix. This series converges for any square matrix $M$.

How do I use the matrix exponential to solve a linear system?

The solution to the set of linear ODEs

$$\dot{x}=Fx$$

with the initial condition

$$x(t_0)=x_0$$

is

$$x(t)=e^{F(t-t_0)}{x}_0.$$

How do we know that this solution is correct? First, let’s check that this solution satisfies the ODEs:

$$\begin{array}{rl}\dot{x}& =\frac{d}{dt}\left(e^{F(t-t_0)}{x}_0\right)\\ & =\frac{d}{dt}\left((I+F(t-t_0)+\frac{1}{2}{F}^2(t-t_0{)}^2+\frac{1}{6}{F}^2(t-t_0{)}^3+\cdots )x_0\right)\\ & =\frac{d}{dt}(I+F(t-t_0)+\frac{1}{2}{F}^2(t-t_0{)}^2+\frac{1}{6}{F}^3(t-t_0{)}^3+\cdots )x_0\\ & =(\frac{d}{dt}\left(I\right)+\frac{d}{dt}(F(t-t_0))+\frac{d}{dt}(\frac{1}{2}{F}^2(t-t_0{)}^2)+\frac{d}{dt}(\frac{1}{6}{F}^3(t-t_0{)}^3)\cdots )x_0\\ & =(0+F+F^2(t-t_0)+\frac{1}{2}{F}^3(t-t_0)+\cdots )x_0\\ & =F(I+F(t-t_0)+\frac{1}{2}{F}^2(t-t_0{)}^2+\cdots )x_0\\ & =F{e}^{F(t-t_0)}{x}_0\end{array}$$

Apparently, it does. Second, let’s check that this solution satisfies the initial condition:

$$\begin{array}{rl}x(t_0)& =e^{F(t_0-t_0)}{x}_0\\ & =e^0{x}_0\\ & =I{x}_0\\ & =x_0\end{array}$$

Again, it does. (We might wonder if this is the only solution to the original ODEs — it is, although a proof would require more work.)

How do I use the matrix exponential to solve state space models?

Consider the state space model

$$\dot{x}=Ax+Bu$$

This model does not look the same as

$$\dot{x}=Fx$$

Indeed, without specifying $u$, we cannot solve for $x$ as a function of time. However, particular choices of $u$ allow us to simplify the state space model. For example, if we choose $u=0$, then we can write

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ & =Ax+B\cdot (0)\\ & =Ax+0\\ & =Ax\end{array}$$

and so we are right back at a linear system that can be solved with the matrix exponential. Another common choice of $u$ is

$$u=-Kx$$

for some constant matrix $K$. (What would the size of $K$ have to be for us to define $u$ in this way?) This choice of $u$ is called state feedback, since the input depends on the state. If we plug this choice into our state space model, then we can write

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ & =Ax+B(-Kx)\\ & =(A-BK)x\end{array}$$

and so — just like before — we are right back at a linear system that can be solved with the matrix exponential. Although this result will get us a long way, we will see how to solve state space models for other choices of input later on.

Given the state-space model $$\dot{x}=Ax+Bu$$ it is standard to call the system $$\dot{x}=Ax$$ that results from the application of zero input $u=0$ the open-loop system. Similarly, it is standard to call the system $$\dot{x}=(A-BK)x$$ that results from the application of linear state feedback $u=-Kx$ the closed-loop system. Remember that "zero input" is not necessarily the same as "zero actuator commands." When linearizing equations of motion to derive a state space model, we defined $$u=n-n_e$$ where $n$ was the set of actuator commands and $n_e$ was the value of these commands at equilibrium. So, $$u=0$$ actually means $$n=n_e.$$ Similarly, $$u=-Kx$$ actually means $$n=n_e-Kx=n_e-K(m-m_e).$$ The term $n_e$ is typically referred to as feedforward and the term $-K(m-m_e)$ is typically referred to as feedback.

Asymptotic stability

What are eigenvalues and eigenvectors?

Consider a square matrix $F\in {\mathbb{R}}^{n\times n}$. If we can find a complex number $s\in \mathbb{C}$ and a non-zero, complex-valued vector $v\in {\mathbb{C}}^n$ that satisfy

$$(sI-F)v=0$$

then we call $s$ an eigenvalue of $F$ and $v$ the corresponding eigenvector of $F$. If we wanted, we could rearrange this equation to put it in a form that might be more familiar:

$$0=(sI-F)v=sv-Fv\Rightarrow Fv=sv.$$

One way to find eigenvalues is to solve the equation

$$det(sI-F)=0$$

where “$det(\cdot )$” means taking a determinant. In general, this equation will be an $n$th-order polynomial in $s$, and so will have $n$ solutions — we might call them $s_1,\dots ,s_n$. To find an eigenvector that corresponds to each eigenvalue $s_i$, we solve

$$F{v}_i=s_i{v}_i$$

for $v_i$. Note that there are many possible solutions to this equation and that eigenvectors are only unique up to a scale factor. In particular, for any real number $k\in \mathbb{R}$, we have

$$\begin{array}{rl}F(k{v}_i)& =k(F{v}_i)\\ & =k(s{v}_i)\\ & =s(k{v}_i).\end{array}$$

Apparently, if $v_i$ is an eigenvector corresponding to $s_i$, then so is $k{v}_i$ for any $k\ne 0$. For this reason, algorithms to find eigenvectors typically normalize them to have unit length.

How do I diagonalize a square matrix?

Suppose we have found the eigenvalues $s_1,\dots ,s_n$ and eigenvectors $v_1,\dots ,v_n$ of a square matrix $F\in {\mathbb{R}}^{n\times n}$. Define the matrix

$$V=\left[\begin{array}{ccc}{v}_1& \cdots & v_n\end{array}\right]$$

with an eigenvector in each column, and also the matrix

$$\text{diag}(s_1,\dots ,s_n)=\left[\begin{array}{cccc}{s}_1& 0& \cdots & 0\\ 0& s_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & s_n\end{array}\right]$$

with the eigenvalues along the diagonal.

Two things are true.

First, the following equality holds:

$$FV=V\text{diag}(s_1,\dots ,s_n)$$

You could easily verify this result for yourself.

Second, if $s_1,\dots ,s_n$ are all distinct (i.e., if no two eigenvalues are the same), then the matrix $V$ is invertible. This result is harder to verify — it has to do with the fact that if the eigenvalues are distinct then the eigenvectors are linearly independent.

The key consequence of $V$ being invertible is that we can solve the above equality to write:

$$\text{diag}(s_1,\dots ,s_n)=V^{-1}FV.$$

In this case — if all eigenvalues are distinct and so the matrix of eigenvectors is invertible — we say that $F$ is diagonalizable. The process of “diagonalizing $F$” is finding the matrix $V$.

What is the matrix exponential of a diagonal matrix?

It is easy to find the matrix exponential of a diagonal matrix, starting from the definition:

$$\begin{array}{rl}{e}^{\text{diag}(s_1,\dots ,s_n)t}& =I+\text{diag}(s_1,\dots ,s_n)t+\frac{1}{2}{(\text{diag}(s_1,\dots ,s_n)t)}^2+\cdots \\ & =\left[\begin{array}{cccc}1& 0& \cdots & 0\\ 0& 1& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 1\end{array}\right]+\left[\begin{array}{cccc}{s}_{1}t& 0& \cdots & 0\\ 0& s_{2}t& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & s_{n}t\end{array}\right]+\left[\begin{array}{cccc}(s_{1}t{)}^2/2& 0& \cdots & 0\\ 0& (s_{2}t{)}^2/2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & (s_{n}t{)}^2/2\end{array}\right]+\cdots \\ & =\left[\begin{array}{cccc}1+s_{1}t+(s_{1}t{)}^2/2+\cdots & 0& \cdots & 0\\ 0& 1+s_{2}t+(s_{2}t{)}^2/2+\cdots & \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 1+s_{n}t+(s_{n}t{)}^2/2+\cdots \end{array}\right]\\ & =\left[\begin{array}{cccc}{e}^{s_{1}t}& 0& \cdots & 0\\ 0& e^{s_{2}t}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & e^{s_{n}t}\end{array}\right]\end{array}$$

What is the solution to a linear system that is diagonalizable?

We have seen that the solution to

$$\dot{x}=Fx$$

with the initial condition

$$x(0)=x_0$$

is

$$x(t)=e^{Ft}{x}_0.$$

Suppose $F$ is a diagonalizable matrix, so that

$$\text{diag}(s_1,\dots ,s_n)=V^{-1}FV$$

where

$$\text{diag}(s_1,\dots ,s_n)$$

is a diagonal matrix that contains the eigenvalues of $F$ and where

$$V=\left[\begin{array}{ccc}{v}_1& \cdots & v_n\end{array}\right]$$

is a matrix of the corresponding eigenvectors. Then, applying the definition of matrix exponential again, we have

$$\begin{array}{rl}{e}^{Ft}{x}_0& =e^{V\text{diag}(s_1,\dots ,s_n)V^{-1}t}{x}_0\\ & =(I+V\text{diag}(s_1,\dots ,s_n)V^{-1}t+\frac{1}{2}{(V\text{diag}(s_1,\dots ,s_n)V^{-1}t)}^2+\cdots )x_0\\ & =V(I+\text{diag}(s_1,\dots ,s_n)t+\frac{1}{2}{(\text{diag}(s_1,\dots ,s_n)t)}^2+\cdots )V^{-1}{x}_0\\ & =V{e}^{\text{diag}(s_1,\dots ,s_n)t}{V}^{-1}{x}_0\\ & =V\left[\begin{array}{cccc}{e}^{s_{1}t}& 0& \cdots & 0\\ 0& e^{s_{2}t}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & e^{s_{n}t}\end{array}\right]V^{-1}{x}_0\end{array}$$

where the last step comes from what we just found out about the matrix exponential of a diagonal matrix. In this expression, the terms $V$, $V^{-1}$, and $x_0$ are constant. The only terms that depend on $t$, in fact, are the scalar exponential functions

$$e^{s_{1}t},\dots ,e^{s_{n}t}$$

that appear in the diagonal of

$$e^{\text{diag}(s_1,\dots ,s_n)t}.$$

Therefore, we can infer the behavior of $x(t)$ based entirely on these scalar exponential functions. In particular, suppose that each eigenvalue $s_i$ — a complex number — has real part $a_i$ and imaginary part $b_i$, or in other words that

$$s_i=a_i+j{b}_i.$$

Euler’s formula tells us that

$$e^{s_{i}t}=e^{(a_i+j{b}_i)t}=e^{a_{i}t}(\cos (b_{i}t)+j\sin (b_{i}t)).$$

Apparently, as time gets large, one of three things is true about each of these terms:

• if $a_i>0$, then $e^{(a_i+j{b}_i)t}$ grows quickly
• if $a_i=0$, then $e^{(a_i+j{b}_i)t}$ is constant ($b_i=0$) or is sinusoidal with constant magnitude ($b_i\ne 0$)
• if $a_i<0$, then $e^{(a_i+j{b}_i)t}$ decays quickly to zero

It is possible to show that (more or less) the same result holds for any system $\dot{x}=Fx$, not only ones for which $F$ is diagonalizable. This takes more work, and involves the transformation of $F$ into Jordan normal form rather than into a diagonal matrix. We would discover that the terms that depend on $t$ all look like

$$t^m{e}^{s_{i}t}$$

where $m$ is an integer that is at most the multiplicity of the eigenvalue $s_i$. Since $e^{a_{i}t}$ increases or decreases a lot faster than $t^m$, then the same three things we listed above would be true of each term in $x(t)$, just like before.

See the reference textbook for details.

When is a linear system asymptotically stable?

The system

$$\dot{x}=Fx$$

is called asymptotically stable if $x(t)\to 0$ as $t\to \mathrm{\infty }$, starting from any initial condition $x(t_0)=x_0.$

Based on our observations about the solution to linear systems that are diagonalizable, we can state the following important result:

The system $$\dot{x}=Fx$$ is asymptotically stable if and only if all eigenvalues of $F$ have negative real part.

In particular, we now have a test for whether or not a controller “works.” Suppose we apply linear state feedback

$$u=-Kx$$

to the state space system

$$\dot{x}=Ax+Bu$$

so that

$$\dot{x}=(A-BK)x.$$

The controller “works” when this system is asymptotically stable, i.e., when $x$ goes to zero as time gets large. We now know, therefore, that the controller “works” when all eigenvalues of $A-BK$ have negative real part.

We may not have a systematic way of finding a matrix $K$ to make the closed-loop system stable yet, but we certainly do have a systematic way now of deciding whether or not a given matrix $K$ makes the closed-loop system stable.

Eigenvalue placement by controllable canonical form

Apply the input

$$u=-Kx$$

to the open-loop system

$$\dot{x}=Ax+Bu$$

and you get the closed-loop system

$$\dot{x}=(A-BK)x.$$

Suppose we want to choose $K$ to put the eigenvalues of the closed-loop system, i.e., the eigenvalues of the matrix $A-BK$, at given locations. We will derive a formula called Ackermann’s method that allows us to do this when possible, and will show how to decide when doing so is impossible.

This entire discussion will be based on the assumption that there is exactly one input (i.e., that $u$ is a vector of length $1$, or that $n_u=1$). Ackermann's method, in particular, cannot be used for eigenvalue placement when there is more than one input.

However, other similar methods — for example, the method of Tits and Yang ("Globally convergent algorithms for robust pole assignment by state feedback," IEEE Transactions on Automatic Control, 41:1432-1452, 1996), as implemented by scipy.signal.place_poles in python — can be used when there are multiple inputs. Our result about when eigenvalue placement is possible — i.e., about when a system is "controllable" — also generalizes to systems with multiple inputs, although it becomes harder to prove.

Eigenvalues are invariant to coordinate transformation

Consider the system

$$\dot{x}=Ax+Bu.$$

Suppose we define a new state variable $z$ so that

$$x=Vz$$

for some invertible matrix $V$, and so

$$\dot{x}=V\dot{z}$$

by differentiation. We have called this process “coordinate transformation” — it is exactly the same process we used for diagonalization when establishing our result about asymptotic stability. Plug these two things into our original state-space model and we get

$$V\dot{z}=AVz+Bu.$$

Solve for $\dot{z}$ and we get the equivalent state-space model

$$\dot{z}=V^{-1}AVz+V^{-1}Bu.$$

Finding a solution $z(t)$ to this transformed system allows us to recover a solution

$$x(t)=Vz(t)$$

to the original system. We would like to know if these two solutions “behave” the same way. In particular, we would like to know if the eigenvalues of $A$ are the same as the eigenvalues of $V^{-1}AV$.

First, let’s look at the eigenvalues of $A$. We know that they are the roots of

$$det(sI-A).$$

Second, let’s look at the eigenvalues of $V^{-1}AV$. We know that they are the roots of

$$det(sI-V^{-1}AV)$$

We can play a trick. Notice that

$$V^{-1}(sI)V=s{V}^{-1}V=sI$$

and so

$$\begin{array}{rlrl}det(sI-V^{-1}AV)& =det(V^{-1}(sI)V-V^{-1}AV)& & \text{because of our trick}\\ & =det(V^{-1}(sI-A)V).\end{array}$$

It is a fact that

$$det(MN)=det(M)det(N)$$

for any square matrices $M$ and $N$. Applying this fact, we find

$$det(V^{-1}(sI-A)V)=det(V^{-1})det(sI-A)det(V).$$

It is another fact that

$$det(M^{-1})=(det(M){)}^{-1}={\displaystyle \frac{1}{det(M)}}$$

Applying this other fact, we find

$$det(V^{-1})det(sI-A)det(V)={\displaystyle \frac{det(sI-A)det(V)}{det(V)}}=det(sI-A)$$

In summary, we have established that

$$det(sI-A)=det(sI-V^{-1}AV)$$

and so the eigenvalues of $A$ and $V^{-1}AV$ are the same. The consequence is, if you design state feedback for the transformed system, you’ll recover the behavior you want for the original system. In particular, suppose you apply the input

$$u=-Lz$$

to the transformed system and choose $L$ to place the eigenvalues of $V^{-1}AV$ in given locations. Applying the input

$$u=-L{V}^{-1}x$$

to the original system, i.e., choosing

$$K=L{V}^{-1},$$

will result in placing the eigenvalues of $A$ at these same locations. The reason this is important is that it is often easier to choose $L$ than to choose $K$. The process of diagonalization was important for a similar reason.

Controllable canonical form

In the previous section, we showed that eigenvalues are invariant to coordinate transformation. The next question is what coordinates are useful for control design. The answer to that question turns out to be something called controllable canonical form.

A system with $n_x$ states and $1$ input is in controllable canonical form if it looks like

$$\dot{z}=A_{\text{ccf}}z+B_{\text{ccf}}u$$

where

$$A_{\text{ccf}}=\left[\begin{array}{ccccc}-a_1& -a_2& \cdots & -a_{n_x-1}& -a_{n_x}\\ 1& 0& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& 0\end{array}\right]B_{\text{ccf}}=\left[\begin{array}{c}1\\ 0\\ 0\\ ⋮\\ 0\end{array}\right].$$

Notice that

• $A_{\text{ccf}}$ is a matrix of size $n_x\times n_x$,
• $B_{\text{ccf}}$ is a matrix of size $n_x\times 1$,
• $z$ is a vector of length $n_x$, and
• $u$ is a vector of length $1$.

It is a fact that the characteristic equation of this system is given by

$$det(sI-A_{\text{ccf}})=s^{n_x}+a_1{s}^{n_x-1}+\cdots +a_{n_x-1}s+a_{n_x}.$$

It is easy to see that this formula is true for $n_x=2$ and $n_x=3$. In particular:

• If $n_x=2$, then:

$$\begin{array}{rl}det(sI-A_{\text{ccf}})& =det\left[\begin{array}{cc}s+a_1& a_2\\ -1& s\end{array}\right]\\ & =(s+a_1)s+a_2=s^2+a_{1}s+a_2.\end{array}$$

• If $n_x=3$, then:

$$\begin{array}{rl}det(sI-A_{\text{ccf}})& =det\left[\begin{array}{ccc}s+a_1& a_2& a_3\\ -1& s& 0\\ 0& -1& s\end{array}\right]\\ & =(s+a_1)s^2+a_3-(-a_{2}s)=s^3+a_1{s}^2+a_{2}s+a_3.\end{array}$$

There are a variety of ways to prove that this same formula is true in general. Applying the general formula to compute the matrix determinant, for example, we would find:

$$\begin{array}{rl}det(sI-A_{\text{ccf}})& =det\left[\begin{array}{ccccc}s+a_1& a_2& \cdots & a_{n_x-1}& a_{n_x}\\ -1& s& \cdots & 0& 0\\ 0& -1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & -1& s\end{array}\right]\\ & =(s+a_1)det(T_1)-a_{2}det(T_2)+a_{3}det(T_3)-\cdots \end{array}$$

where each matrix $T_i$ is upper-triangular with $-1$ in $i-1$ diagonal entries and $s$ in $n_x-i$ diagonal entries. Since the determinant of an upper-triangular matrix is the product of its diagonal entries, we have

$$det(T_i)=\{\begin{array}{ll}{s}^{n_x-i}& \text{when }i\text{ is odd}\\ -s^{n_x-i}& \text{when }i\text{ is even}\end{array}$$

Plug this in, and our result follows. Now, the reason that controllable canonical form is useful is that if we choose the input

$$u=-Lz$$

for some choice of gains

$$L=\left[\begin{array}{ccc}{\ell }_1& \cdots & {\ell }_{n_x}\end{array}\right]$$

then the “$A$ matrix” of the closed-loop system is

$$A_{\text{ccf}}-B_{\text{ccf}}L=\left[\begin{array}{ccccc}-a_1-{\ell }_1& -a_2-{\ell }_2& \cdots & -a_{n_x-1}-{\ell }_{n_x-1}& -a_{n_x}-{\ell }_{n_x}\\ 1& 0& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& 0\end{array}\right]$$

The characteristic equation of this closed-loop system, computed in the same way as for $A_{\text{ccf}}$, is

$$s^{n_x}+(a_1+{\ell }_1)s^{n_x-1}+\cdots +(a_{n_x-1}+{\ell }_{n_x-1})s+(a_{n_x}+{\ell }_{n_x}).$$

If you want this characteristic equation to look like

$$s^{n_x}+r_1{s}^{n_x-1}+\cdots +r_{n_x-1}s+r_{n_x}$$

then it’s obvious what gains you should choose

$${\ell }_1=r_1-a_1{\ell }_2=r_2-a_2\cdots {\ell }_{n_x}=r_{n_x}-a_{n_x}.$$

So, if you have a system in controllable canonical form, then it is easy to choose gains that make the characteristic equation of the closed-loop system look like anything you want (i.e., to put the closed-loop eigenvalues anywhere you want). In other words, it is easy to do control design.

Putting a system in controllable canonical form

We have seen that controllable canonical form is useful. Now we’ll see how to put a system in this form. Suppose we have a system

$$\dot{x}=Ax+Bu$$

and we want to choose an invertible matrix $V$ so that if we define a new state variable $z$ by

$$x=Vz$$

then we can rewrite the system as

$$\dot{z}=A_{\text{ccf}}z+B_{\text{ccf}}u$$

where

$$A_{\text{ccf}}=V^{-1}AV\text{and}{B}_{\text{ccf}}=V^{-1}B$$

are in controllable canonical form. The trick is to look at the so-called controllability matrix that is associated with the transformed system:

$$W_{\text{ccf}}=\left[\begin{array}{cccc}{B}_{\text{ccf}}& A_{\text{ccf}}{B}_{\text{ccf}}& \cdots & A_{\text{ccf}}^{n_x-1}{B}_{\text{ccf}}\end{array}\right].$$

We will talk more later about the controllability matrix — for now, notice that

$$\begin{array}{rl}{B}_{\text{ccf}}& =V^{-1}B\\ A_{\text{ccf}}{B}_{\text{ccf}}& =V^{-1}AV{V}^{-1}B=V^{-1}AB\\ A_{\text{ccf}}^2{B}_{\text{ccf}}& =A_{\text{ccf}}(A_{\text{ccf}}{B}_{\text{ccf}})=V^{-1}AV{V}^{-1}AB=V^{-1}{A}^{2}B\\ ⋮& =⋮\end{array}$$

You see the pattern here, I’m sure. The result is:

$$\begin{array}{rl}{W}_{\text{ccf}}& =\left[\begin{array}{cccc}{V}^{-1}B& V^{-1}AB& \cdots & V^{-1}{A}^{n_x-1}B\end{array}\right]\\ & =V^{-1}\left[\begin{array}{cccc}B& AB& \cdots & A^{n_x-1}B\end{array}\right]\\ & =V^{-1}W\end{array}$$

where

$$W=\left[\begin{array}{cccc}B& AB& \cdots & A^{n_x-1}B\end{array}\right]$$

is the controllability matrix associated with the original system.

There are three things to note:

• $A$ and $B$ are things that you know — you have a description of the original system, as always — so you can compute $W$.

• $A_{\text{ccf}}$ and $B_{\text{ccf}}$ are also things that you know — the values $a_1,\dots ,a_{n_x}$ in the top row of $A_{\text{ccf}}$ are the coefficients of the characteristic polynomial of the matrix $A$ — so you can compute $W_{\text{ccf}}$.

• $W$ is a square $n_x\times n_x$ matrix — it has $n_x$ columns $B$, $AB$, and so forth, all of which have size $n_x\times 1$. So, if $W$ has non-zero determinant, then you can find its inverse.

As a consequence, you can solve for the matrix $V^{-1}$:

$$V^{-1}=W_{\text{ccf}}{W}^{-1}.$$

Now, suppose you design a control policy for the transformed system:

$$u=-K_{\text{ccf}}z.$$

Remember, you can do this easily, because the transformed system is in controllable canonical form. We can compute the equivalent control policy, that would be applied to the original system:

$$u=-K_{\text{ccf}}z=-K_{\text{ccf}}{V}^{-1}x=-K_{\text{ccf}}{W}_{\text{ccf}}{W}^{-1}x.$$

In particular, if we choose

$$K=K_{\text{ccf}}{W}_{\text{ccf}}{W}^{-1}$$

then we get the behavior that we want. Again, we emphasize that this only works if $W$ is invertible, and that $W$ is only invertible if $det(W)\ne 0$.

A systematic process for control design

Apply the input

$$u=-Kx$$

to the open-loop system

$$\dot{x}=Ax+Bu$$

and you get the closed-loop system

$$\dot{x}=(A-BK)x.$$

Suppose we want to choose $K$ to put the eigenvalues of the closed-loop system at

$$p_1,\dots ,p_{n_x}.$$

Using the results of the previous sections, we know we can do this as follows:

• Compute the characteristic equation that we want:

$$(s-p_1)\cdots (s-p_{n_x})=s^{n_x}+r_1{s}^{n_x-1}+\cdots +r_{n_x-1}s+r_{n_x}.$$

• Compute the characteristic equation that we have:

$$det(sI-A)=s^{n_x}+a_1{s}^{n_x-1}+\cdots +a_{n_x-1}s+a_{n_x}$$

• Compute the controllability matrix of the original system (and check that $det(W)\ne 0$):

$$W=\left[\begin{array}{cccc}B& AB& \cdots & A^{n_x-1}B\end{array}\right]$$

• Compute the controllability matrix of the transformed system:

$$W_{\text{ccf}}=\left[\begin{array}{cccc}{B}_{\text{ccf}}& A_{\text{ccf}}{B}_{\text{ccf}}& \cdots & A_{\text{ccf}}^{n_x-1}{B}_{\text{ccf}}\end{array}\right]$$

       where

$$A_{\text{ccf}}=\left[\begin{array}{ccccc}-a_1& -a_2& \cdots & -a_{n-1}& -a_n\\ 1& 0& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& 0\end{array}\right]B_{\text{ccf}}=\left[\begin{array}{c}1\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]$$

• Compute the gains for the transformed system:

$$K_{\text{ccf}}=\left[\begin{array}{ccc}{r}_1-a_1& \cdots & r_{n_x}-a_{n_x}\end{array}\right]$$

• Compute the gains for the original system:

$$K=K_{\text{ccf}}{W}_{\text{ccf}}{W}^{-1}$$

And we’re done! This process is easy to implement, without any symbolic computation. Remember, although this method only works for systems with exactly one input (i.e., when $n_u=1$), similar methods work for systems with multiple inputs — in python, use scipy.signal.place_poles.

How to decide when eigenvalue placement is possible

We say that a system is controllable if eigenvalue placement is possible. We have seen eigenvalue placement with Ackermann’s method (for the special case when $n_u=1$) is only possible when the controllability matrix $W$ is invertible. Here is a generalization of that same result to any system:

The system $$\dot{x}=Ax+Bu$$ is controllable if and only if the controllability matrix $$W=\left[\begin{array}{cccc}B& AB& \cdots & A^{n_x-1}B\end{array}\right]$$ is full rank, where $n_x$ is the number of states.

Let’s break this statement down.

First, suppose there is only one input, so $n_u=1$. In that case, $A$ has size $n_x\times n_x$ and $B$ has size $n_x\times 1$. Therefore, $W$ is a square matrix of size $n_x\times n_x$, so $W$ being full rank simply means that it is invertible (i.e., that $det(W)\ne 0$).

Now, suppose there is more than one input, so $n_u>1$. Then, $B$ has size $n_x\times n_u$. The matrix $W$ then has size $n_x\times n_x{n}_u$, and $W$ is no longer square. Although we can’t invert a non-square matrix, we can still find its rank. In particular, it is easy to find the rank of a non-square matrix in python using numpy.linalg.matrix_rank. We say that $W$ is full rank if and only if its rank is $n_x$.

If $W$ is full rank, the system is controllable, and eigenvalue placement will work. If $W$ is not full rank, the system is not controllable, and eigenvalue placement will not work.

We can actually say a little more than this. It turns out that if the controllability matrix $W$ is not full rank, then although we cannot place all of the system’s eigenvalues, we can place some of these eigenvalues, while the others will remain in the same location no matter what gain matrix $K$ we choose. The eigenvalues we can place are often called “controllable eigenvalues” or “controllable modes,” while the eigenvalues we can not place are often called “uncontrollable eigenvalues” or “uncontrollable modes.” If the rank of $W$ is $n_x-1$, then we can place $n_x-1$ eigenvalues, and $1$ eigenvalue is uncontrollable. If the rank of $W$ is $n_x-2$, then we can place $n_x-2$ eigenvalues, and $2$ eigenvalues are uncontrollable. And so forth.

Optimization and optimal control

These notes were originally written by T. Bretl and were transcribed here by S. Bout.

Optimization

The following thing is called an optimization problem:

$$\begin{array}{r}\underset{u}{\mathrm{minimize}}{u}^2-2u+3.\end{array}$$

The solution to this problem is the value of $u$ that makes $u^2-2u+3$ as small as possible.

• We know that we are supposed to choose a value of $u$ because “$u$” appears underneath the “minimize” statement. We call $u$ the decision variable.

• We know that we are supposed to minimize $u^2-2u+3$ because “$u^2-2u+3$” appears to the right of the “minimize” statement. We call $u^2-2u+3$ the cost function.

In particular, the solution to this problem is $u=1$. There are at least two different ways to arrive at this result:

• We could plot the cost function. It is clear from the plot that the minimum is at $u=1$.

  

• We could apply the first derivative test. We compute the first derivative:

  $$\begin{array}{r}\frac{d}{du}(u^2-2u+3)=2u-2.\end{array}$$

  Then, we set the first derivative equal to zero and solve for $u$:

  $$\begin{array}{r}2u-2=0\Rightarrow u=1.\end{array}$$

  Values of $u$ that satisfy the first derivative test are only “candidates” for optimality—they could be maxima instead of minima, or could be only one of many minima. We’ll ignore this distinction for now. Here’s a plot of the cost function and of it’s derivative. Note that, clearly, the derivative is equal to zero when the cost function is minimized:

  

In general, we write optimization problems like this:

$$\begin{array}{r}\underset{u}{\mathrm{minimize}}g(u).\end{array}$$

Again, $u$ is the decision variable and $g(u)$ is the cost function. In the previous example:

$$\begin{array}{r}g(u)=u^2-2u+3.\end{array}$$

Here is another example:

$$\begin{array}{r}\underset{u_1,u_2}{\mathrm{minimize}}{u}_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6.\end{array}$$

The solution to this problem is the value of both $u_1$ and $u_2$ that, together, make $u_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6$ as small as possible. There are two differences between this optimization problem and the previous one. First, there is a different cost function:

$$\begin{array}{r}g(u_1,u_2)=u_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6.\end{array}$$

Second, there are two decision variables instead of one. But again, there are at least two ways of finding the solution to this problem:

• We could plot the cost function. The plot is now 3D—the “x” and “y” axes are $u_1$ and $u_2$, and the “z” axis is $g(u_1,u_2)$. The shape of the plot is a bowl. It’s hard to tell where the minimum is from looking at the bowl, so I’ve also plotted contours of the cost function underneath. “Contours” are like the lines on a topographic map. From the contours, it looks like the minimum is at $(u_1,u_2)=(-2,-1)$.

  

• We could apply the first derivative test. We compute the partial derivative of $g(u_1,u_2)$ with respect to both $u_1$ and $u_2$:

  $$\begin{array}{rl}\frac{\partial }{\partial u_1}g(u_1,u_2)& =2u_1-2u_2+2\\ \frac{\partial }{\partial u_2}g(u_1,u_2)& =6u_2-2u_1+2.\end{array}$$

  Then, we set both partial derivatives equal to zero and solve for $u_1$ and $u_2$:

  $$\begin{array}{r}\begin{array}{rl}2u_1-2u_2+2& =0\\ 6u_2-2u_1+2& =0\end{array}\Rightarrow (u_1,u_2)=(-2,-1).\end{array}$$

  As before, we would have to apply a further test in order to verify that this choice of $(u_1,u_2)$ is actually a minimum. But it is certainly consistent with what we observed above. Here is a plot of each partial derivative as a function of $u_1$ and $u_2$. The shape of each plot is a plane (i.e., a flat surface). Both planes are zero at $(-2,-1)$:

  

An equivalent way of stating this same optimization problem would have been as follows:

$$\begin{array}{r}\underset{u_1,u_2}{\mathrm{minimize}}{\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]}^T\left[\begin{array}{cc}1& -1\\ -1& 3\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]+{\left[\begin{array}{c}2\\ 2\end{array}\right]}^T\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]+6.\end{array}$$

You can check that the cost function shown above is the same as the cost function we saw before (e.g., by multiplying it out). We could have gone farther and stated the problem as follows:

$$\begin{array}{r}\underset{u}{\mathrm{minimize}}{u}^T\left[\begin{array}{cc}1& -1\\ -1& 3\end{array}\right]u+{\left[\begin{array}{c}2\\ 2\end{array}\right]}^{T}u+6.\end{array}$$

We have returned to having just one decision variable $u$, as in the first example, but this variable is now a $2\times 1$ matrix—i.e., it has two elements, which we would normally write as $u_1$ and $u_2$. The point here is that the “decision variable” in an optimization problem can be a variety of different things: a scalar, a vector (i.e., an $n\times 1$ matrix), and—as we will see—even a function of time. Before proceeding, however, let’s look at one more example of an optimization problem:

$$\begin{array}{rl}\underset{u,x}{\mathrm{minimize}}& u^2+3x^2-2ux+2u+2x+6\\ \text{subject to}& u+x=3.\end{array}$$

This example is exactly the same as the previous example, except that the two decision variables (now renamed $u$ and $x$) are subject to a constraint: $u+x=3$. We are no longer free to choose $u$ and $x$ arbitrarily. We are restricted to choices that satisfy the constraint. The solution to this optimization problem is the value $(u,x)$ that minimizes the cost function, chosen from among all values $(u,x)$ that satisfy the constraint. Again, there are a variety of ways to solve this problem. One way is to eliminate the constraint. First, we solve the constraint equation:

$$\begin{array}{r}u+x=3\Rightarrow x=3-u.\end{array}$$

Then, we plug this result into the cost function:

$$\begin{array}{rl}{u}^2+3x^2-2ux+2u+2x+6& =u^2+3(3-u{)}^2-2u(3-u)+2u+2(3-u)+6\\ & =6u^2-24u+39.\end{array}$$

By doing so, we have shown that solving the constrained optimization problem

$$\begin{array}{rl}\underset{u,x}{\mathrm{minimize}}& u^2+3x^2-2ux+2u+2x+6\\ \text{subject to}& u+x=3.\end{array}$$

is equivalent to solving the unconstrained optimization problem

$$\begin{array}{r}\underset{u}{\mathrm{minimize}}6u^2-24u+39.\end{array}$$

and then taking $x=3-u$. We can do so easily by taking the first derivative and setting it equal to zero, as we did in the first example:

$$\begin{array}{r}0=\frac{d}{du}(6u^2-24u+39)=12u-24\Rightarrow u=2\Rightarrow x=3-u=1.\end{array}$$

The point here was not to show how to solve constrained optimization problems in general, but rather to identify the different parts of a problem of this type. As a quick note, you will sometimes see the example optimization problem we’ve been considering written as

$$\begin{array}{rl}\underset{u}{\mathrm{minimize}}& u^2+3x^2-2ux+2u+2x+6\\ \text{subject to}& u+x=3.\end{array}$$

The meaning is exactly the same, but $x$ isn’t listed as one of the decision variables under “minimize.” The idea here is that $x$ is an “extra variable” that we don’t really care about. This optimization problem is trying to say the following:

“Among all choices of $u$ for which there exists an $x$ satisfying $u+x=3$, find the one that minimizes $u^2+3x^2-2ux+2u+2x+6$.”

Minimum vs. Minimizer

We have seen three example problems. In each case, we were looking for the minimizer, i.e., the choice of decision variable that made the cost function as small as possible:

• The solution to

  $$\begin{array}{r}\underset{u}{\mathrm{minimize}}{u}^2-2u+3\end{array}$$

  was $u=1$.

• The solution to

  $$\begin{array}{r}\underset{u_1,u_2}{\mathrm{minimize}}{u}_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6\end{array}$$

  was $(u_1,u_2)=(-2,-1)$.

• The solution to

  $$\begin{array}{rl}\underset{u}{\mathrm{minimize}}& u^2+3x^2-2ux+2u+2x+6\\ \text{subject to}& u+x=3\end{array}$$

  was $(u,x)=(2,1)$.

It is sometimes useful to focus on the minimum instead of on the minimizer, i.e., what the “smallest value” was that we were able to achieve. When focusing on the minimum, we often use the following “set notation” instead:

• The problem

  $$\begin{array}{r}\underset{u}{\mathrm{minimize}}{u}^2-2u+3\end{array}$$

  is rewritten

  $$\begin{array}{r}\underset{u}{\mathrm{minimum}}\{u^2-2u+3\}.\end{array}$$

  The meaning is—find the minimum value of $u^2-2u+3$ over all choices of $u$. The solution to this problem can be found by plugging in what we already know is the minimizer, $u=1$. In particular, we find that the solution is $2$.

• The problem

  $$\begin{array}{r}\underset{u_1,u_2}{\mathrm{minimize}}{u}_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6\end{array}$$

  is rewritten

  $$\begin{array}{r}\underset{u_1,u_2}{\mathrm{minimum}}\{u_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6\}.\end{array}$$

  Again, the meaning is—find the minimum value of $u_1^2+3u_2^2-2u_1{u}_2+2u_1+2u_2+6$ over all choices of $u_1$ and $u_2$. We plug in what we already know is the minimizer $(u_1,u_2)=(-2,-1)$ to find the solution—it is $3$.

• The problem

  $$\begin{array}{rl}\underset{u}{\mathrm{minimize}}& u^2+3x^2-2ux+2u+2x+6\\ \text{subject to}& u+x=3\end{array}$$

  is rewritten

  $$\begin{array}{r}\underset{u}{\mathrm{minimum}}\{u^2+3x^2-2ux+2u+2x+6:u+x=3\}.\end{array}$$

  And again, the meaning is—find the minimum value of $u^2+3x^2-2ux+2u+2x+6$ over all choices of $u$ for which there exists $x$ satisfying $u+x=3$. Plug in the known minimizer, $(u,x)=(2,1)$, and we find that the solution is 15.

The important thing here is to understand the notation and to understand the difference between a “minimum” and a “minimizer.”

Optimal Control

Statement of the problem

The following thing is called an optimal control problem:

$$\begin{array}{rl}\text{(1)}& \underset{u_{[t_0,t_1]}}{\mathrm{minimize}}& h(x(t_1))+{\int }_{t_0}^{t_1}g(x(t),u(t))dt\text{subject to}& \frac{dx(t)}{dt}=f(x(t),u(t)),x(t_0)=x_0.\end{array}$$

Let’s try to understand what it means.

• The statement

  $$\begin{array}{r}\underset{u_{[t_0,t_1]}}{\mathrm{minimize}}\end{array}$$

  says that we are being asked to choose an input trajectory $u$ that minimizes something. Unlike in the optimization problems we saw before, the decision variable $u$ in this problem is a function of time. The notation $u_{[t_0,t_1]}$ is one way of indicating this. Given an initial time $t_0$ and a final time $t_1$, we are being asked to choose the value of $u(t)$ at all times in between, i.e., for all $t\in [t_0,t_1]$.

• The statement

  $$\begin{array}{r}\frac{dx(t)}{dt}=f(x(t),u(t)),x(t_0)=x_0\end{array}$$

  is a constraint. It implies that we are restricted to choices of $u$ for which there exists an $x$ satisfying a given initial condition

  $$\begin{array}{r}x(t_0)=x_0\end{array}$$

  and satisfying the ordinary differential equation

  $$\begin{array}{r}\frac{dx(t)}{dt}=f(x(t),u(t)).\end{array}$$

  One example of an ordinary differential equation that looks like this is our usual description of a system in state-space form:

  $$\begin{array}{r}\dot{x}=Ax+Bu,\end{array}$$

• The statement

  $$\begin{array}{r}h(x(t_1))+{\int }_{t_0}^{t_1}g(x(t),u(t))dt\end{array}$$

  says what we are trying to minimize—it is the cost function in this problem. Notice that the cost function depends on both $x$ and $u$. Part of it—$g(\cdot )$—is integrated (i.e., “added up”) over time. Part of it—$h(\cdot )$—is applied only at the final time. One example of a cost function that looks like this is

  $$\begin{array}{r}x(t_1{)}^{T}Mx(t_1)+{\int }_{t_0}^{t_1}(x(t{)}^{T}Qx(t)+u(t{)}^{T}Ru(t))dt.\end{array}$$

The HJB equation (our new “first-derivative test”)

As usual, there are a variety of ways to solve an optimal control problem. One way is by application of what is called the Hamilton-Jacobi-Bellman Equation, or “HJB.” This equation is to optimal control what the first-derivative test is to optimization. To derive it, we will first rewrite the optimal control problem in “minimum form” (see “Minimum vs Minimizer”):

$$\begin{array}{r}\underset{u_{[t_0,t_1]}}{\mathrm{minimum}}\{h(x(t_1))+{\int }_{t_0}^{t_1}g(x(t),u(t))dt:\frac{dx(t)}{dt}=f(x(t),u(t)),x(t_0)=x_0\}.\end{array}$$

Nothing has changed here, we’re just asking for the minimum and not the minimizer. Next, rather than solve this problem outright, we will first state a slightly different problem:

$$\begin{array}{}\text{(2)}& \underset{{\overline{u}}_{[t,t_1]}}{\mathrm{minimum}}\{h(\overline{x}(t_1))+{\int }_t^{t_1}g(\overline{x}(s),\overline{u}(s))ds:\frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}.\end{array}$$

The two changes that I made to go from the original problem to this one are:

• Make the initial time arbitrary (calling it $t$ instead of $t_0$).

• Make the initial state arbitrary (calling it $x$ instead of $x_0$).

I also made three changes in notation. First, I switched from $x$ to $\overline{x}$ to avoid getting confused between $x$ as initial condition and $\overline{x}$ as state trajectory. Second, I switched from $u$ to $\overline{u}$ to be consistent with the switch from $x$ to $\overline{x}$. Third, I switched from calling time $t$ to calling time $s$ to avoid getting confused with my use of $t$ as a name for the initial time.

You should think of the problem (2) as a function itself. In goes an initial time $t$ and an initial state $x$, and out comes a minimum value. We can make this explicit by writing

$$\begin{array}{rl}\text{(3)}& v(t,x)& =\underset{{\overline{u}}_{[t,t_1]}}{\mathrm{minimum}}\{h(\overline{x}(t_1))+{\int }_t^{t_1}g(\overline{x}(s),\overline{u}(s))ds:& \frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}.\end{array}$$

We call $v(t,x)$ the value function. Notice that $v(t_0,x_0)$ is the solution to the original optimal control problem that we wanted to solve—the one where the initial time is $t_0$ and the initial state is $x_0$. More importantly, notice that $v(t,x)$ satisfies the following recursion:

$$\begin{array}{rl}\text{(4)}& v(t,x)& =\underset{{\overline{u}}_{[t,t+\mathrm{\Delta }t]}}{\mathrm{minimum}}\{v(t+\mathrm{\Delta }t,\overline{x}(t+\mathrm{\Delta }t))+{\int }_t^{t+\mathrm{\Delta }t}g(\overline{x}(s),\overline{u}(s))ds:& \frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}.\end{array}$$

The reason this equation is called a “recursion” is that it expresses the function $v$ in terms of itself. In particular, it splits the optimal control problem into two parts. The first part is from time $t$ to time $t+\mathrm{\Delta }t$. The second part is from time $t+\mathrm{\Delta }t$ to time $t_1$. The recursion says that the minimum value $v(t,x)$ is the sum of the cost

$$\begin{array}{r}\underset{{\overline{u}}_{[t,t+\mathrm{\Delta }t]}}{\mathrm{minimum}}\{{\int }_t^{t+\mathrm{\Delta }t}g(\overline{x}(s),\overline{u}(s))ds:\frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}\end{array}$$

from the first part and the cost

$$\begin{array}{r}\underset{{\overline{u}}_{[t+\mathrm{\Delta }t,t_1]}}{\mathrm{minimum}}\{h(\overline{x}(t_1))+{\int }_{t+\mathrm{\Delta }t}^{t_1}g(\overline{x}(t),\overline{u}(t))dt:\frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t+\mathrm{\Delta }t)=\text{blah}\}\end{array}$$

from the second part (where “$\text{blah}$” is whatever the state turns out to be, starting at time $t$ from start $x$ and applying the input $u_{[t,t+\mathrm{\Delta }t]}$), which we recognize as the definition of

$$\begin{array}{r}v(t+\mathrm{\Delta }t,\overline{x}(t+\mathrm{\Delta }t)).\end{array}$$

We now proceed to approximate the terms in (4) by first-order series expansions. In particular, we have

$$\begin{array}{rl}v(t+\mathrm{\Delta }t,\overline{x}(t+\mathrm{\Delta }t))& \approx v(t+\mathrm{\Delta }t,\overline{x}(t)+\frac{d\overline{x}(t)}{dt}\mathrm{\Delta }t)\\ & =v(t+\mathrm{\Delta }t,x+f(x,\overline{u}(t))\mathrm{\Delta }t)\\ & \approx v(t,x)+\frac{\partial v(t,x)}{\partial t}\mathrm{\Delta }t+\frac{\partial v(t,x)}{\partial x}f(x,\overline{u}(t))\mathrm{\Delta }t\end{array}$$

and we also have

$$\begin{array}{rl}{\int }_t^{t+\mathrm{\Delta }t}g(\overline{x}(s),\overline{u}(s))ds& \approx g(\overline{x}(t),\overline{u}(t))\mathrm{\Delta }t\\ & =g(x,\overline{u}(t))\mathrm{\Delta }t.\end{array}$$

If we plug both of these into (4), we find

$$\begin{array}{rl}v(t,x)& =\underset{{\overline{u}}_{[t,t+\mathrm{\Delta }t]}}{\mathrm{minimum}}\{v(t+\mathrm{\Delta }t,\overline{x}(t+\mathrm{\Delta }t))+{\int }_t^{t+\mathrm{\Delta }t}g(\overline{x}(s),\overline{u}(s))ds:\\ & \frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}\\ & =\underset{{\overline{u}}_{[t,t+\mathrm{\Delta }t]}}{\mathrm{minimum}}\{v(t,x)+\frac{\partial v(t,x)}{\partial t}\mathrm{\Delta }t+\frac{\partial v(t,x)}{\partial x}f(x,\overline{u}(t))\mathrm{\Delta }t+g(x,\overline{u}(t))\mathrm{\Delta }t:\\ & \frac{d\overline{x}(s)}{ds}=f(\overline{x}(s),\overline{u}(s)),\overline{x}(t)=x\}.\end{array}$$

Notice that nothing inside the minimum depends on anything other than $t$, $x$, and $\overline{u}(t)$. So we can drop the constraint and make $\overline{u}(t)$ the only decision variable. In fact, we might as well replace $\overline{u}(t)$ simply by “$u$” since we only care about the input at a single instant in time:

$$\begin{array}{r}v(t,x)=\underset{u}{\mathrm{minimum}}\{v(t,x)+\frac{\partial v(t,x)}{\partial t}\mathrm{\Delta }t+\frac{\partial v(t,x)}{\partial x}f(x,u)\mathrm{\Delta }t+g(x,u)\mathrm{\Delta }t\}.\end{array}$$

Also, notice that

$$\begin{array}{r}v(t,x)+\frac{\partial v(t,x)}{\partial t}\mathrm{\Delta }t\end{array}$$

does not depend on $u$, so it can be brought out of the minimum:

$$\begin{array}{r}v(t,x)=v(t,x)+\frac{\partial v(t,x)}{\partial t}\mathrm{\Delta }t+\underset{u}{\mathrm{minimum}}\{\frac{\partial v(t,x)}{\partial x}f(x,u)\mathrm{\Delta }t+g(x,\overline{u}(t))\mathrm{\Delta }t\}.\end{array}$$

To simplify further, we can subtract $v(t,x)$ from both sides, then divide everything by $\mathrm{\Delta }t$:

$$\begin{array}{}\text{(5)}& 0=\frac{\partial v(t,x)}{\partial t}+\underset{u}{\mathrm{minimum}}\{\frac{\partial v(t,x)}{\partial x}f(x,u)+g(x,u)\}.\end{array}$$

The equation is called the Hamilton-Jacobi-Bellman Equation, or simply the HJB equation. As you can see, it is a partial differential equation, so it needs a boundary condition. This is easy to obtain. In particular, going all the way back to the definition (3), we find that

$$\begin{array}{}\text{(6)}& v(t_1,x)=h(x).\end{array}$$

The importance of HJB is that if you can find a solution to (5)-(6))—that is, if you can find a function $v(t,x)$ that satisfies the partial differential equation (5) and the boundary condition (6)—then the minimizer $u$ in (5), evaluated at every time $t\in [t_0,t_1]$, is the solution to the optimal control problem (1). You might object that (5) “came out of nowhere.” First of all, it didn’t. We derived it just now, from scratch. Second of all, where did the first-derivative test come from? Could you derive that from scratch? (Do you, every time you need to use it? Or do you just use it?)

Solution approach

The optimal control problem

$$\begin{array}{rl}\underset{u_{[t_0,t_1]}}{\mathrm{minimize}}& h(x(t_1))+{\int }_{t_0}^{t_1}g(x(t),u(t))dt\\ \text{subject to}& \frac{dx(t)}{dt}=f(x(t),u(t)),x(t_0)=x_0\end{array}$$

can be solved in two steps:

• Find $v$:

  $$\begin{array}{r}0=\frac{\partial v(t,x)}{\partial t}+\underset{u}{\mathrm{minimum}}\{\frac{\partial v(t,x)}{\partial x}f(x,u)+g(x,u)\},v(t_1,x)=h(x).\end{array}$$

• Find $u$:

  $$\begin{array}{r}u(t)=\underset{u}{\mathrm{minimize}}\frac{\partial v(t,x)}{\partial x}f(x,u)+g(x,u)\text{for all }t\in [t_0,t_1].\end{array}$$

LQR

Statement of the problem

Here is the linear quadratic regulator (LQR) problem:

$$\begin{array}{rl}\underset{u_{[t_0,t_1]}}{\mathrm{minimize}}& x(t_1{)}^{T}Mx(t_1)+{\int }_{t_0}^{t_1}(x(t{)}^{T}Qx(t)+u(t{)}^{T}Ru(t))dt\\ \text{subject to}& \dot{x}(t)=Ax(t)+Bu(t),x(t_0)=x_0.\end{array}$$

It is an optimal control problem—if you define

$$\begin{array}{r}f(x,u)=Ax+Bug(x,u)=x^{T}Qx+u^{T}Ruh(x)=x^{T}Mx\end{array}$$

then you see that this problem has the same form as (1). It is called “linear” because the dynamics are those of a linear (state space) system. It is called “quadratic” because the cost is quadratic (i.e., polynomial of order at most two) in $x$ and $u$. It is called a “regulator” because the result of solving it is to keep $x$ close to zero (i.e., to keep errors small).

The matrices $Q$, $R$, and $M$ are parameters that can be used to trade off error (non-zero states) with effort (non-zero inputs). These matrices have to be symmetric ($Q=Q^T$, etc.), have to be the right size, and also have to satisfy the following conditions in order for the LQR problem to have a solution:

$$\begin{array}{r}Q\ge 0R>0M\ge 0.\end{array}$$

What this notation means is that $Q$ and $M$ are positive semidefinite and that $R$ is positive definite (https://en.wikipedia.org/wiki/Positive-definite_matrix). We will ignore these terms for now, noting only that this is similar to requiring (for example) that $r>0$ in order for the function $r{u}^2$ to have a minimum.

Do the matrices $Q$, $R$, and $M$ really have to be symmetric?

Suppose $Q_{\text{silly}}$ is not symmetric. Define

$$Q=\frac{1}{2}(Q_{\text{silly}}+Q_{\text{silly}}^T).$$

First, notice that $Q$ is symmetric:

$$\begin{array}{rl}{Q}^T& =\frac{1}{2}{(Q_{\text{silly}}+Q_{\text{silly}}^T)}^T\\ & =\frac{1}{2}(Q_{\text{silly}}^T+Q_{\text{silly}})& & \text{because }(M+N{)}^T=M^T+N^T\\ & =\frac{1}{2}(Q_{\text{silly}}+Q_{\text{silly}}^T)& & \text{because }M+N=N+M\\ & =Q.\end{array}$$

In fact, $Q$ — when defined in this way — is often referred to as the symmetric part of $Q_{\text{silly}}$.

Second, notice that we can replace $Q_{\text{silly}}$ with $Q$ without changing the cost:

$$\begin{array}{rl}{x}^{T}Qx& =x^T\frac{1}{2}(Q_{\text{silly}}+Q_{\text{silly}}^T)x\\ & =\frac{1}{2}{x}^T{Q}_{\text{silly}}x+\frac{1}{2}{x}^T{Q}_{\text{silly}}^{T}x\\ & =\frac{1}{2}{x}^T{Q}_{\text{silly}}x+\frac{1}{2}{\left(x^T{Q}_{\text{silly}}x\right)}^T& & \text{because }(MN{)}^T=N^T{M}^T\\ & =\frac{1}{2}{x}^T{Q}_{\text{silly}}x+\frac{1}{2}\left(x^T{Q}_{\text{silly}}x\right)& & \text{because the transpose of a scalar is itself}\\ & =\frac{1}{2}(x^T{Q}_{\text{silly}}x+x^T{Q}_{\text{silly}}x)\\ & =x^T{Q}_{\text{silly}}x.\end{array}$$

So, it is not that $Q$, $R$, and $M$ “have to be” symmetric, it’s that they “might as well be” symmetric — if they’re not, we can replace them with matrices that are symmetric without changing the cost.

Solution to the problem

Our “Solution approach” tells us to solve the LQR problem in two steps. First, we find a function $v(t,x)$ that satisfies the HJB equation. Here is that equation, with the functions $f$, $g$, and $h$ filled in from our “Statement of the problem”:

$$\begin{array}{r}0=\frac{\partial v(t,x)}{\partial t}+\underset{u}{\mathrm{minimum}}\{\frac{\partial v(t,x)}{\partial x}(Ax+Bu)+x^{T}Qx+u^{T}Ru\},v(t_1,x)=x^{T}Mx.\end{array}$$

What function $v$ might solve this equation? Look at the boundary condition. At time $t_1$,

$$\begin{array}{r}v(t_1,x)=x^{T}Mx.\end{array}$$

This function has the form

$$\begin{array}{r}v(t,x)=x^{T}P(t)x\end{array}$$

for some symmetric matrix $P(t)$ that satisfies $P(t_1)=M$. So let’s “guess” that this form is the solution we are looking for, and see if it satisfies the HJB equation. Before proceeding, we need to compute the partial derivatives of $v$:

$$\begin{array}{r}\frac{\partial v}{\partial t}=x^T\dot{P}x\frac{\partial v}{\partial x}=2x^{T}P\end{array}$$

This is matrix calculus (e.g., see https://en.wikipedia.org/wiki/Matrix_calculus or Chapter A.4.1 of http://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf). The result on the left should surprise no one. The result on the right is the matrix equivalent of

$$\partial (p{x}^2)/\partial x=2px.$$

You could check that this result is correct by considering an example. Plug these partial derivatives into HJB and we have

$$\begin{array}{rl}\text{(7)}& 0& =x^T\dot{P}x+\underset{u}{\mathrm{minimum}}\{2x^{T}P(Ax+Bu)+x^{T}Qx+u^{T}Ru\}& =x^T\dot{P}x+\underset{u}{\mathrm{minimum}}\{x^T(2PA+Q)x+2x^{T}PBu+u^{T}Ru\}\end{array}$$

To evaluate the minimum, we apply the first-derivative test (more matrix calculus!):

$$\begin{array}{rl}0& =\frac{\partial }{\partial u}(x^T(2PA+Q)x+2x^{T}PBu+u^{T}Ru)\\ & =2x^{T}PB+2u^{T}R\\ & =2{(B^{T}Px+Ru)}^T.\end{array}$$

This equation is easily solved:

$$\begin{array}{}\text{(8)}& u=-R^{-1}{B}^{T}Px.\end{array}$$

Plugging this back into (7), we have

$$\begin{array}{rl}0& =x^T\dot{P}x+\underset{u}{\mathrm{minimum}}\{x^T(2PA+Q)x+2x^{T}PBu+u^{T}Ru\}\\ & =x^T\dot{P}x+(x^T(2PA+Q)x+2x^{T}PB(-R^{-1}{B}^{T}Px)+(-R^{-1}{B}^{T}Px{)}^{T}R(-R^{-1}{B}^{T}Px))\\ & =x^T\dot{P}x+(x^T(2PA+Q)x-2x^{T}PB{R}^{-1}{B}^{T}Px+x^{T}PB{R}^{-1}{B}^{T}Px)\\ & =x^T\dot{P}x+x^T(2PA+Q)x-x^{T}PB{R}^{-1}{B}^{T}Px\\ & =x^T\dot{P}x+x^T(PA+A^{T}P+Q)x-x^{T}PB{R}^{-1}{B}^{T}Px\\ & \dots \text{because }{x}^T(N+N^T)x=2x^{T}Nx\text{ for any }N\text{ and }x\dots \\ & =x^T(\dot{P}+PA+A^{T}P+Q-PB{R}^{-1}{B}^{T}P)x.\end{array}$$

In order for this equation to be true for any $x$, it must be the case that

$$\begin{array}{r}\dot{P}=PB{R}^{-1}{B}^{T}P-PA-A^{T}P-Q.\end{array}$$

In summary, we have found that

$$\begin{array}{r}v(t,x)=x^{T}Px\end{array}$$

solves the HJB equation, where $P$ is found by integrating the matrix differential equation

$$\begin{array}{r}\dot{P}=PB{R}^{-1}{B}^{T}P-PA-A^{T}P-Q\end{array}$$

backward in time, starting from

$$\begin{array}{r}P(t_1)=M.\end{array}$$

Now that we know $v$, we can find $u$. Wait, we already did that! The minimizer in the HJB equation is (8). This choice of input has the form

$$u=-Kx$$

for

$$\begin{array}{r}K=R^{-1}{B}^{T}P.\end{array}$$

LQR Code

Here is how to find the solution to an LQR problem with Python:

import numpy as np
from scipy import linalg

def lqr(A, B, Q, R):
    # Find the cost matrix
    P = linalg.solve_continuous_are(A, B, Q, R)
    # Find the gain matrix
    K = linalg.inv(R) @ B.T @ P
    return K

State estimation

What is a state-space model with output?

All state-space models we have seen so far have had this form:

$$\dot{x}=Ax+Bu$$

From now on, we will consider state-space models with this form instead:

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ y& =Cx+Du\end{array}$$

We have added one more variable (along with the state $x$ and the input $u$):

• $y$ is the output.

Like the state and the input, the output is a function of time — we will write $y(t)$ when we want to make the time-dependence explicit — and may have more than one element, so is in general a vector (represented by a matrix of numbers). Suppose, in particular, that

• the output $y$ has $n_y$ elements.

Then, we would represent it as a column matrix:

$$y=\left[\begin{array}{c}{y}_1\\ ⋮\\ y_{n_y}\end{array}\right].$$

Like $A$ and $B$, the constants $C$ and $D$ have to be matrices:

• $C$ is a constant matrix of size $n_y\times n_x$.
• $D$ is a constant matrix of size $n_y\times n_u$.

The state-space model remains both linear and time-invariant, because $y$ is a linear function of $x$ and $u$ and because $C$ and $D$ are constant.

As usual, other people may use other symbols for both the variables and constants in a state-space model. For example: $$\begin{array}{rl}\dot{z}& =Ez+Fv\\ w& =Gz+Hv.\end{array}$$ This is also a "state-space model," in which the state is $z$, the input is $v$, and the output is $w$.

Outputs can be used to model a variety of different things. We will use them to model sensor measurements for the purpose of state estimation.

How do I linearize a sensor model?

You already know how to linearize a dynamic model:

• Rewrite the dynamic model as a set of first-order ODEs:

$$\dot{m}=f(m,n)$$

• Choose an equilibrium point $m_e,n_e$ among the solutions to this equation:

$$0=f(m_e,n_e)$$

• Define the state and input in terms of the equilibrium point:

$$x=m-m_{e}u=n-n_e$$

• Compute $A$ and $B$ as the Jacobian of $f$ with respect to $m$ and $n$ respectively, evaluated at the equilibrium point:

$$A=\frac{\partial f}{\partial m}{|}_{(m_e,n_e)}B=\frac{\partial f}{\partial n}{|}_{(m_e,n_e)}$$

The result is a linear approximation

$$\dot{x}=Ax+Bu$$

to the nonlinear dynamic model

$$\dot{m}=f(m,n)$$

that is accurate near the equilibrium point.

We can use the same process to linearize a sensor model:

Step 1. Rewrite the sensor model as follows:

$$o=g(m,n).$$

In this expression, the variable $o$ is a vector of sensor measurements. It is a function of time and can have more than one element — so, just like $m$ and $n$, it should be represented as a column matrix. Just like the function $f(m,n)$ that describes the dynamic model, the function $g(m,n)$ that describes the sensor model will often be nonlinear.

Step 2. Define the output as follows:

$$y=o-g(m_e,n_e).$$

Note that $y$ measures the difference between what the sensor measurements are and what these measurements would be if the system were at equilibrium. In particular, the output is zero when the system is at equilibrium even if the measurements are not.

Step 3. Compute $C$ and $D$ as follows:

$$C=\frac{\partial g}{\partial m}{|}_{(m_e,n_e)}D=\frac{\partial g}{\partial n}{|}_{(m_e,n_e)}.$$

Why does this make sense? Just like we took a first-order Taylor’s series expansion about $m_e,n_e$ to approximate the function $f(m,n)$ that describes the dynamic model, let’s take a first-order series expansion about $m_e,n_e$ to approximate the function $g(m,n)$ that describes the sensor model:

$$\begin{array}{rl}y& =o-g(m_e,n_e)\\ & =g(m,n)-g(m_e,n_e)\\ & \approx (g(m_e,n_e)+\frac{\partial g}{\partial m}{|}_{(m_e,n_e)}(m-m_e)+\frac{\partial g}{\partial n}{|}_{(m_e,n_e)}(n-n_e))-g(m_e,n_e)\\ & =\frac{\partial g}{\partial m}{|}_{(m_e,n_e)}(m-m_e)+\frac{\partial g}{\partial n}{|}_{(m_e,n_e)}(n-n_e)\\ & =Cx+Du\end{array}$$

So, with this choice of $C$ and $D$, we have produced a linear approximation

$$y=Cx+Du$$

to the nonlinear sensor model

$$o=g(m,n)$$

that is accurate near the equilibrium point.

Example (linear sensor model)

Consider again a system with the following dynamic model:

$$\ddot{q}+3\sin q=\tau .$$

We already showed how to rewrite this dynamic model as

$$\left[\begin{array}{c}\dot{q}\\ \dot{v}\end{array}\right]=\left[\begin{array}{c}v\\ -3\sin q+\tau \end{array}\right]$$

and how to linearize it about the equilibrium point

$$q_e=\pi /2v_e=0{\tau }_e=3$$

to produce the state-space model

$$\dot{x}=Ax+Bu$$

where

$$A=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]B=\left[\begin{array}{c}0\\ 1\end{array}\right]$$

and

$$x=\left[\begin{array}{c}q-q_e\\ v-v_e\end{array}\right]u=\left[\begin{array}{c}\tau -{\tau }_e\end{array}\right].$$

Now suppose we have sensors that allow the measurement of

$$q.$$

Let’s apply our method to put this measurement in state-space form.

First, we rewrite the measurement in the form $o=g(m,n)$, i.e., as a vector-valued function of $m$ and $n$:

$$o=\left[\begin{array}{c}q\end{array}\right].$$

Then, we define the output as the difference between the value of this function and what the value would be at equilibrium:

$$\begin{array}{rl}y& =o-g(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])\\ & =\left[\begin{array}{c}q\end{array}\right]-\left[\begin{array}{c}{q}_e\end{array}\right]\\ & =\left[\begin{array}{c}q-q_e\end{array}\right].\end{array}$$

Finally, we compute $C$ and $D$ by taking Jacobians:

$$\begin{array}{rl}C& =\frac{\partial g}{\partial \left[\begin{array}{c}q\\ v\end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}{\displaystyle \frac{\partial (q)}{\partial q}}& {\displaystyle \frac{\partial (q)}{\partial v}}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}1& 0\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{cc}1& 0\end{array}\right]\\ D& =\frac{\partial g}{\partial \left[\begin{array}{c}\tau \end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}{\displaystyle \frac{\partial (q)}{\partial \tau }}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}0\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{c}0\end{array}\right]\end{array}$$

The resulting state-space model is

$$\begin{array}{rl}y& =Cx+Du\\ & =\left[\begin{array}{cc}1& 0\end{array}\right]x+\left[\begin{array}{c}0\end{array}\right]u.\end{array}$$

Note that the original sensor model was linear, so there was no approximation here. We could probably have skipped the entire process of linearization and written the system in state-space form bny inspection. However, just as was true when putting dynamic models in state-space form (see example), it is nice to know that “linearization” still works even in this simple case.

Example (nonlinear sensor model)

Consider a system with the same dynamic model as before, but now suppose we have sensors that allow the measurement of

$${\displaystyle \frac{\cos q}{\sin q}}.$$

Again, let’s apply our method to put this measurement in state-space form.

First, we rewrite the measurement in the form $o=g(m,n)$, i.e., as a vector-valued function of $m$ and $n$:

$$o=\left[\begin{array}{c}\cos q/\sin q\end{array}\right].$$

Then, we define the output as the difference between the value of this function and what the value would be at equilibrium:

$$\begin{array}{rl}y& =o-g(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])\\ & =\left[\begin{array}{c}\cos q/\sin q\end{array}\right]-\left[\begin{array}{c}\cos q_e/\sin q_e\end{array}\right]\\ & =\left[\begin{array}{c}(\cos q/\sin q)-(\cos q_e/\sin q_e)\end{array}\right].\end{array}$$

Finally, we compute $C$ and $D$ by taking Jacobians:

$$\begin{array}{rl}C& =\frac{\partial g}{\partial \left[\begin{array}{c}q\\ v\end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}{\displaystyle \frac{\partial (\cos q/\sin q)}{\partial q}}& {\displaystyle \frac{\partial (\cos q/\sin q)}{\partial v}}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{cc}-1/(\sin q{)}^2& 0\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{cc}-1& 0\end{array}\right]\\ D& =\frac{\partial g}{\partial \left[\begin{array}{c}\tau \end{array}\right]}{|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}{\displaystyle \frac{\partial (\cos q/\sin q)}{\partial \tau }}\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & ={\left[\begin{array}{c}0\end{array}\right]|}_{(\left[\begin{array}{c}{q}_e\\ v_e\end{array}\right],\left[\begin{array}{c}{\tau }_e\end{array}\right])}\\ & =\left[\begin{array}{c}0\end{array}\right]\end{array}$$

The resulting state-space model is

$$\begin{array}{rl}y& =Cx+Du\\ & =\left[\begin{array}{cc}-1& 0\end{array}\right]x+\left[\begin{array}{c}0\end{array}\right]u.\end{array}$$

In both of the previous two examples, we found that $D=0$. This is so often the case that we may just write $$y=Cx$$ instead of $$y=Cx+Du$$ when describing a sensor model in state-space form.

Optimal observer design

These notes were originally written by T. Bretl and were transcribed for this reference page by S. Bout.

Statement of the problem

Here is the deterministic, finite-horizon, continuous-time Kalman Filter (KF) problem—i.e., the optimal control problem that one would solve to produce an optimal observer:

$$\begin{array}{rl}\underset{x(t_1),n_{[t_0,t_1]},d_{[t_0,t_1]}}{\mathrm{minimize}}& n(t_0{)}^T{M}_{o}n(t_0)+{\int }_{t_0}^{t_1}(n(t{)}^T{Q}_{o}n(t)+d(t{)}^T{R}_{o}d(t))dt\\ \text{subject to}& \dot{x}(t)=Ax(t)+Bu(t)+d(t)\\ & y(t)=Cx(t)+n(t)\end{array}$$

The interpretation of this problem is as follows. The current time is $t_1$. You have taken measurements $y(t)$ over the time interval $[t_0,t_1]$. You are looking for noise $n(t)$ and disturbance $d(t)$ over this same time interval and for an estimate $x(t_1)$ of the current state that would best explain these measurements.

The matrices $Q_o$, $R_o$, and $M_o$ are parameters that can be used to trade off noise (the difference between the measurements and what you expect them to be) with disturbance (the difference between the time derivative of the state and what you expect it to be). These matrices have to be symmetric, have to be the right size, and also have to satisfy the following conditions in order for the KF problem to have a solution:

$$Q_o\ge 0R_o>0M_o\ge 0.$$

Just as with the LQR problem, this notation means that $Q_o$ and $M_o$ are positive semidefinite and that $R_o$ is positive definite (see wikipedia).

By plugging in the expression for $n(t)$ that appears in the constraint, this optimal control problem can be rewritten as

$$\begin{array}{rl}\underset{x(t_1),d_{[t_0,t_1]}}{\mathrm{minimize}}& (Cx(t_0)-y(t_0){)}^T{M}_o(Cx(t_0)-y(t_0))\\ & +{\int }_{t_0}^{t_1}((Cx(t)-y(t){)}^T{Q}_o(Cx(t)-y(t))+d(t{)}^T{R}_{o}d(t))dt\\ \text{subject to}& \dot{x}(t)=Ax(t)+Bu(t)+d(t)\end{array}$$

It is an optimal control problem, just like LQR—if you define

$$\begin{array}{rl}f(t,x,d)& =Ax+Bu(t)+d\\ g(t,x,d)& =(Cx-y(t){)}^T{Q}_o(Cx-y(t))+d^T{R}_{o}d(t)\\ h(t,x)& =(Cx-y(t){)}^T{M}_o(Cx-y(t))\end{array}$$

then you see that this problem has the general form

$$\begin{array}{rl}\underset{x(t_1),d_{[t_0,t_1]}}{\text{minimize}}& h(t_0,x(t_0))+{\int }_{t_0}^{t_1}g(t,x(t),d(t))dt\\ \text{subject to}& \frac{dx(t)}{dt}=f(t,x(t),d(t)).\end{array}$$

There are four differences between this form and the one we saw when solving the LQR problem:

• The “input” in this problem is $u$, not $d$.

• The “current time” is $t_1$ and not $t_0$.

• The final state—i.e., the state at the current time—is not given. Indeed, the point here is to choose a final state $x(t_1)$ that best explains $u(t)$ and $y(t)$.

• The functions $f$, $g$, and $h$ vary with time (because they have parameters in them—$u(t)$ and $y(t)$—that are functions of time).

Because of these four differences, the HJB equation for a problem of this form is

$$0=-\frac{\partial v(t,x)}{\partial t}+\underset{d}{\mathrm{minimum}}\{-\frac{\partial v(t,x)}{\partial x}f(t,x,d)+g(t,x,d)\},v(t_0,x)=h(t_0,x(t_0)).$$

Note the change in sign of both the first term outside the minimum and the first term inside the minimum—this is because we are effectively solving an optimal control problem in which time flows backward (from the current time $t_1$ to the initial time $t_0$, instead of from the current time $t_0$ to the final time $t_1$). It is possible to derive this form of the HJB equation in exactly the same way as it was done in the notes on LQR.

Solution to the problem

As usual, our first step is to find a function $v(t,x)$ that satisfies the HJB equation. Here is that equation, with the functions $f$, $g$, and $h$ filled in:

$$\begin{array}{rl}0& =-\frac{\partial v(t,x)}{\partial t}+\underset{d}{\mathrm{minimum}}\{-\frac{\partial v(t,x)}{\partial x}(Ax+Bu(t)+d)\\ & +(Cx-y(t){)}^T{Q}_o(Cx-y(t))+d(t{)}^T{R}_{o}d(t)\}\\ v(t_0,x)& =(Cx(t_0)-y(t_0){)}^T{M}_o(Cx(t_0)-y(t_0)).\end{array}$$

Expand the boundary condition:

$$\begin{array}{rl}v(t_0,x)& =(Cx(t_0)-y(t_0){)}^T{M}_o(Cx(t_0)-y(t_0))\\ & =x(t_0{)}^T{C}^T{M}_{o}Cx(t_0)-2y(t_0{)}^T{M}_o{C}^{T}x(t_0)+y(t_0{)}^T{M}_{o}y(t_0)\end{array}$$

This function has the form

$$v(t,x)=x^{T}P(t)x+2o(t{)}^{T}x+w(t)$$

for some symmetric matrix $P(t)$ and some other matrices $o(t)$ and $w(t)$ that satisfy the following boundary conditions:

$$P(t_0)=C^T{M}_{o}Co(t_0)=-C{M}_{o}y(t_0)w(t_0)=y(t_0{)}^T{M}_{o}y(t_0).$$

Let’s “guess” that this form of $v$ is the solution we are looking for, and see if it satisfies the HJB equation. Before proceeding, we need to compute the partial derivatives of $v$:

$$\frac{\partial v}{\partial t}=x^T\dot{P}x+2{\dot{o}}^{T}x+\dot{w}\frac{\partial v}{\partial x}=2x^{T}P+2o^T$$

Here again—just as for LQR—we are applying matrix calculus. Plug these partial derivatives into HJB and we have

$$\begin{array}{rl}0& =-(x^T\dot{P}x+2{\dot{o}}^{T}x+\dot{w})+\underset{d}{\mathrm{minimum}}\{-(2x^{T}P+2o^T)(Ax+Bu+d)\\ & +(Cx-y{)}^T{Q}_o(Cx-y)+d^T{R}_{o}d\}\\ \text{(1)}& & =-(x^T\dot{P}x+2{\dot{o}}^{T}x+\dot{w})+\underset{d}{\mathrm{minimum}}\{d^T{R}_{o}d-2(Px+o{)}^{T}d& -2(x^{T}P+o^T)(Ax+Bu)\\ & +(Cx-y{)}^T{Q}_o(Cx-y)\}\end{array}$$

To evaluate the minimum, we apply the first-derivative test (more matrix calculus!):

$$\begin{array}{rl}0& =\frac{\partial }{\partial d}(d^T{R}_{o}d-2(Px+o{)}^{T}d-2(x^{T}P+o^T)(Ax+Bu)+(Cx-y{)}^T{Q}_o(Cx-y))\\ & =2d^T{R}_o-2(Px+o{)}^T.\end{array}$$

This equation is easily solved:

$$\begin{array}{}\text{(2)}& d=R^{-1}(Px+o).\end{array}$$

Plugging this back into (1), we have

$$\begin{array}{rl}0& =-(x^T\dot{P}x+2{\dot{o}}^{T}x+\dot{w})+\underset{d}{\mathrm{minimum}}\{-(2x^{T}P+2o^T)(Ax+Bu+d)\\ & +(Cx-y{)}^T{Q}_o(Cx-y)+d^T{R}_{o}d\}\\ & =-(x^T\dot{P}x+2{\dot{o}}^{T}x+\dot{w})-(Px+o{)}^T{R}_o^{-1}(Px+o)\\ & -2(x^{T}P+o^T)(Ax+Bu)+(Cx-y{)}^T{Q}_o(Cx-y)\\ & =x^T(-\dot{P}-P{R}_o^{-1}P-2PA+C^T{Q}_{o}C)x\\ & +2x^T(-\dot{o}-P{R}_o^{-1}o-PBu-C^T{Q}_{o}y-A^{T}o)\\ & +(-\dot{w}-o^T{R}_o^{-1}o-2o^{T}Bu+y^T{Q}_{o}y)\\ & =x^T(-\dot{P}-P{R}_o^{-1}P-PA-A^{T}P+C^T{Q}_{o}C)x\\ & +2x^T(-\dot{o}-P{R}_o^{-1}o-PBu-C^T{Q}_{o}y-A^{T}o)\\ & +(-\dot{w}-o^T{R}_o^{-1}o-2o^{T}Bu+y^T{Q}_{o}y)\end{array}$$

where the last step is because

$$x^T(N+N^T)x=2x^{T}Nx\text{ for any }N\text{ and }x.$$

In order for this equation to be true for any $x$, it must be the case that

$$\begin{array}{rl}\dot{P}& =-P{R}_o^{-1}P-PA-A^{T}P+C^T{Q}_{o}C\\ \dot{o}& =-P{R}_o^{-1}o-PBu-C^T{Q}_{o}y-A^{T}o\\ \dot{w}& =-o^T{R}_o^{-1}o-2o^{T}Bu+y^T{Q}_{o}y.\end{array}$$

In summary, we have found that

$$v(t,x)=x^{T}P(t)x+2o(t{)}^{T}x+w(t)$$

solves the HJB equation, where $P$, $o$, and $w$ are found by integrating the above ODEs forward in time, starting from

$$P(t_0)=C^T{M}_{o}Co(t_0)=-C{M}_{o}y(t_0)w(t_0)=y(t_0{)}^T{M}_{o}y(t_0).$$

The optimal choice of state estimate at time $t$ is the choice of $x$ that minimizes $v(t,x)$, that is, the solution to

$$\underset{x}{\mathrm{minimize}}{x}^{T}P(t)x+2o(t{)}^{T}x+w(t).$$

We can find the solution to this problem by application of the first derivative test, with some matrix calculus:

$$\begin{array}{rl}0& =\frac{\partial }{\partial x}(x^{T}Px+2o^{T}x+w)\\ & =2x^{T}P+2o^T,\end{array}$$

implying that

$$x=-P^{-1}o.$$

Let’s call this solution $\hat{x}$. Note that we can, equivalently, write

$$0=P\hat{x}+o.$$

Suppose we take the time derivative of this expression, plugging in what we found earlier for $\dot{P}$ and $\dot{o},$ as well as plugging in yet another version of this same expression, $o=-P\hat{x}$:

$$\begin{array}{rl}0& =\dot{P}\hat{x}+P\dot{\hat{x}}+\dot{o}\\ & =(-P{R}_o^{-1}P-PA-A^{T}P+C^T{Q}_{o}C)\hat{x}+P\dot{\hat{x}}-P{R}_o^{-1}o-PBu-C^T{Q}_{o}y-A^{T}o\\ & =-P{R}_o^{-1}P\hat{x}-PA\hat{x}-A^{T}P\hat{x}+C^T{Q}_{o}C\hat{x}+P\dot{\hat{x}}+P{R}_o^{-1}P\hat{x}-PBu-C^T{Q}_{o}y+A^{T}P\hat{x}\\ & =P\dot{\hat{x}}-PA\hat{x}-PBu+C^T{Q}_o(C\hat{x}-y)\\ & =P(\dot{\hat{x}}-A\hat{x}-Bu+P^{-1}{C}^T{Q}_o(C\hat{x}-y)).\end{array}$$

For this equation to hold for any $P$, we must have

$$\dot{\hat{x}}=A\hat{x}+Bu-P^{-1}{C}^T{Q}_o(C\hat{x}-y).$$

Behold! This is our expression for an optimal observer, if we define

$$L=P^{-1}{C}^T{Q}_o.$$

Finally, suppose we take the limit as $t_0\to -\mathrm{\infty }$, so assume an infinite horizon. It is a fact that $P$ tends to a steady-state value, and so $L$ does as well. When this happens, $\dot{P}=0$, and so the steady-state value of $P$ is the solution to the algebraic equation

$$0=-P{R}_o^{-1}P-PA-A^{T}P+C^T{Q}_{o}C.$$

It is customary to write these last two equations in a slightly different way. In particular, suppose we pre- and post-multiply both sides of this last equation by $P^{-1}$, and define

$$P_o=P^{-1}$$

Then, we have

$$L=P_o{C}^T{Q}_o$$

and

$$0=P_o{C}^T{Q}_{o}C{P}_o-A{P}_o-P_o{A}^T-R_o^{-1}.$$

Summary

An optimal observer—a deterministic, infinite-horizon, continuous-time Kalman Filter—is given by

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

where

$$L=P_o{C}^T{Q}_o$$

and $P_o$ satisfies

$$0=P_o{C}^T{Q}_{o}C{P}_o-A{P}_o-P_o{A}^T-R_o^{-1}.$$

Comparison between LQR and KF (i.e., why you can use “LQR” in Python to compute an optimal observer)

An optimal controller is given by

$$u=-Kx$$

where

$$\begin{array}{}\text{(3)}& K=R_c^{-1}{B}^T{P}_c\end{array}$$

and $P_c$ satisfies

$$\begin{array}{}\text{(4)}& 0=P_{c}B{R}_c^{-1}{B}^T{P}_c-P_{c}A-A^T{P}_c-Q_c.\end{array}$$

An optimal observer is given by

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

where

$$\begin{array}{}\text{(5)}& L=P_o{C}^T{Q}_o\end{array}$$

and $P_o$ satisfies

$$\begin{array}{}\text{(6)}& 0=P_o{C}^T{Q}_{o}C{P}_o-A{P}_o-P_o{A}^T-R_o^{-1}.\end{array}$$

Take the transpose of (5) and—remembering that $P_o$ and $Q_o$ are symmetric—we get

$$\begin{array}{}\text{(7)}& L^T=Q_{o}C{P}_o.\end{array}$$

Take the transpose of (6) and—remembering that $R_o$ is also symmetric—we get

$$\begin{array}{}\text{(8)}& 0=P_o{C}^T{Q}_{o}C{P}_o-P_o{A}^T-A{P}_o-R_o^{-1}.\end{array}$$

Compare (3) and (4) with (7) and (8). They are exactly the same if we make the following replacements:

• replace $K$ with $L^T$

• replace $A$ with $A^T$

• replace $B$ with $C^T$

• replace $Q_c$ with $R_o^{-1}$

• replace $R_c$ with $Q_o^{-1}$

This is the reason why

L = lqr(A.T, C.T, linalg.inv(Ro), linalg.inv(Qo)).T

produces an optimal observer, just like

K = lqr(A, B, Qc, Rc)

produces an optimal controller. WWWWWOOOOOWWWWW!!!!!!! (See above for the code used to find the solution to an LQR problem with Python.)

Reference tracking

The controllers we have designed so far make the state converge to zero — that is, they make

$$x(t)\to 0\text{as}t\to \mathrm{\infty }.$$

Suppose we want the state to converge to something else — that is, to make

$$x(t)\to x_{\text{des}}\text{as}t\to \mathrm{\infty }.$$

We will see that, under certain conditions, this is easy to do.

Reference tracking with full state feedback

Consider the dynamic model

$$\dot{m}=f(m,n)$$

where $m\in {\mathbb{R}}^{\ell }$. Suppose we linearize this model about some equilibrium point $(m_e,n_e)$ to produce the state-space model

$$\dot{x}=Ax+Bu$$

where

$$x=m-m_{e}u=n-n_e.$$

Suppose we design linear state feedback

$$u=-Kx$$

that would make the closed-loop system

$$\dot{x}=(A-BK)x$$

asymptotically stable — that is, that would make

$$x(t)\to 0\text{as}t\to 0.$$

Denote the standard basis for ${\mathbb{R}}^{\ell }$ by

$$e_1=\left[\begin{array}{c}1\\ 0\\ ⋮\\ 0\end{array}\right]e_2=\left[\begin{array}{c}0\\ 1\\ ⋮\\ 0\end{array}\right]\cdots e_{\ell }=\left[\begin{array}{c}0\\ 0\\ ⋮\\ 1\end{array}\right].$$

Suppose there is some index $i\in \{1,\dots ,\ell \}$ that satisfies

$$\begin{array}{}\text{(9)}& f(m+e_{i}r,n)=f(m,n)\text{for all}r\in \mathbb{R}.\end{array}$$

That is, suppose the function $f$ is constant in — or, does not vary with — the $i$’th element of $m$. Then, the following three things are true:

---

Invariance of equilibrium point. Since

$$\begin{array}{rlrl}f(m_e+e_{i}r,n_e)& =f(m_e,n_e)& & \text{because of (9)}\\ & =0& & \text{because (}{m}_e,n_e\text{) is an equilibrium point},\end{array}$$

then $(m_e+e_{i}r,n_e)$ is also an equilibrium point for any $r\in \mathbb{R}$.

---

Invariance of error in approximation of the dynamic model. The linear model

$$\dot{x}=Ax+Bu$$

is an approximation to the nonlinear model

$$\dot{m}=f(m,n).$$

The amount of error in this approximation is

$$\begin{array}{rl}e(m,n)& =f(m,n)-(Ax+Bu)\\ & =f(m,n)-(A(m-m_e)+B(n-n_e)).\end{array}$$

We will show that this approximation error is constant in — or, does not vary with — the $i$’th element of $m$. Before we do so, we will prove that

$$\begin{array}{}\text{(10)}& A{e}_{i}r=0\text{ for any }r\in \mathbb{R}.\end{array}$$

First, we show that the $i$’th column of $A$ is zero:

$$\begin{array}{rl}\frac{\partial f}{\partial m_i}{|}_{(m_e,n_e)}& =\underset{h\to 0}{lim}\frac{f(m_e+e_{i}h,n_e)-f(m_e,n_e)}{h}\\ & =\underset{h\to 0}{lim}\frac{f(m_e,n_e)-f(m_e,n_e)}{h}\\ & =\underset{h\to 0}{lim}\frac{0}{h}\\ & =0.\end{array}$$

Next, denote the columns of $A$ by

$$A=\left[\begin{array}{cccc}{A}_1& A_2& \cdots & A_{\ell }\end{array}\right].$$

Then, we compute

$$A{e}_{i}r=\left(\sum _{j=1}^{\ell }{A}_j{e}_{ij}\right)r=A_{i}r=0.$$

Now, for the approximation error:

$$\begin{array}{rl}e(m+e_{i}r,n)& =f(m+e_{i}r,n)-(A(m+e_{i}r-m_e)+B(n-n_e))\\ & =f(m,n)-(A(m+e_{i}r-m_e)+B(n-n_e))& & \text{because of (9)}\\ & =f(m,n)-(A(m+m_e)+B(n-n_e))-A{e}_{i}r\\ & =f(m,n)-(A(m+m_e)+B(n-n_e))& & \text{because of (10)}\\ & =e(m,n).\end{array}$$

What this means is that our state-space model is just as accurate near $(m_e+e_{i}r,n_e)$ as it is near the equilibrium point $(m_e,n_e)$.

---

Invariance of control. Suppose we implement linear state feedback with reference tracking:

$$u=-K(x-x_{\text{des}})$$

where

$$x_{\text{des}}=e_{i}r$$

for any $r\in \mathbb{R}$. Let’s assume (for now) that $r$ is constant, and so $x_{\text{des}}$ is also constant. What will $x(t)$ converge to in this case? Let’s find out. First, we define the error

$$z=x-x_{\text{des}}$$

and note that

$$u=-K(x-x_{\text{des}})=-Kz.$$

Second, we derive an expression for the closed-loop system in terms of this error:

$$\begin{array}{rl}\dot{z}& =\frac{d}{dt}(x-x_{\text{des}})\\ & =\dot{x}-0\\ & =Ax+Bu\\ & =A(z+x_{\text{des}})+B(-Kz)\\ & =(A-BK)z+A{x}_{\text{des}}\\ & =(A-BK)z+A{e}_{i}r\\ & =(A-BK)z& & \text{because of (10)}.\end{array}$$

This means that

$$z(t)\to 0\text{as}t\to \mathrm{\infty }$$

or equivalently that

$$x(t)\to x_{\text{des}}\text{as}t\to \mathrm{\infty }$$

so long as all eigenvalues of $A-BK$ have negative real part — exactly the same conditions under which the closed-loop system without reference tracking would have been asymptotically stable.

SUMMARY (reference tracking with full state feedback)

Consider a system $$\dot{m}=f(m,n)$$ that satisfies $$f(m+e_{i}r,n)=f(m,n)\text{for any}r\in \mathbb{R}.$$ Linearize this system about an equilibrium point $(m_e,n_e)$ to produce the state-space model $$\dot{x}=Ax+Bu$$ where $$x=m-m_e\text{and}u=n-n_e.$$ Apply linear state feedback with reference tracking as $$u=-K(x-x_{\text{des}})$$ where $$x_{\text{des}}=e_{i}r$$ for any $r\in \mathbb{R}$. Then, $$x(t)\to x_{\text{des}}\text{as}t\to \mathrm{\infty }$$ if and only if all eigenvalues of $A-BK$ have negative real part.

Reference tracking with partial state feedback

Consider the system

$$\begin{array}{rlrl}\dot{m}& =f(m,n)& & \text{dynamic model}\\ o& =g(m,n)& & \text{sensor model}\end{array}$$

where $m\in {\mathbb{R}}^{\ell }$. Suppose we linearize this model about some equilibrium point $(m_e,n_e)$ to produce the state-space model

$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ y& =Cx+Du\end{array}$$

where

$$x=m-m_{e}u=n-n_{e}y=o-g(m_e,n_e).$$

Suppose we design a controller

$$u=-K\hat{x}$$

and observer

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

that would make the closed-loop system

$$\left[\begin{array}{c}\dot{x}\\ {\dot{x}}_{\text{err}}\end{array}\right]=\left[\begin{array}{cc}A-BK& -BK\\ 0& A-LC\end{array}\right]\left[\begin{array}{c}x\\ x_{\text{err}}\end{array}\right]$$

asymptotically stable — that is, that would make

$$x(t)\to 0\text{and}{x}_{\text{err}}(t)\to 0\text{as}t\to 0$$

where

$$x_{\text{err}}=\hat{x}-x.$$

Suppose, as for reference tracking with full state feedback, that there is some index $i\in \{1,\dots ,\ell \}$ for which

$$f(m+e_{i}r,n)=f(m,n)\text{for all}r\in \mathbb{R}.$$

This implies invariance of equilibrium point and invariance of error in approximation of the dynamic model, just like before. Suppose it is also true that, for some constant vector $g_0$, the sensor model satisfies

$$\begin{array}{}\text{(11)}& g(m+e_{i}r,n)=g_{0}r+g(m,n)\text{for all}r\in \mathbb{R}.\end{array}$$

Then, the following two more things are true:

---

Invariance of error in approximation of the sensor model. The linear model

$$y=Cx+Du$$

is an approximation to the nonlinear model

$$o=g(m,n).$$

The amount of error in this approximation is

$$\begin{array}{rl}e(m,n)& =g(m,n)-(g(m_e,n_e)+Cx+Du)\\ & =g(m,n)-(g(m_e,n_e)+C(m-m_e)+D(n-n_e)).\end{array}$$

We will show that this approximation error is constant in — or, does not vary with — the $i$’th element of $m$. First, we show that the $i$’th column of $C$ is $g_0$:

$$\begin{array}{rl}\frac{\partial g}{\partial m_i}{|}_{(m_e,n_e)}& =\underset{h\to 0}{lim}\frac{g(m_e+e_{i}h,n_e)-g(m_e,n_e)}{h}\\ & =\underset{h\to 0}{lim}\frac{g_{0}h+g(m_e,n_e)-g(m_e,n_e)}{h}& & \text{because of (11)}\\ & =\underset{h\to 0}{lim}\frac{g_{0}h}{h}\\ & =g_0.\end{array}$$

Next, denote the columns of $C$ by

$$C=\left[\begin{array}{cccc}{C}_1& C_2& \cdots & C_{\ell }\end{array}\right].$$

Then, we compute

$$\begin{array}{}\text{(12)}& C{e}_{i}r=\left(\sum _{j=1}^{\ell }{C}_j{e}_{ij}\right)r=C_{i}r=g_{0}r.\end{array}$$

Now, for the approximation error:

$$\begin{array}{rl}e(m+e_{i}r,n)& =g(m+e_{i}r,n)-(g(m_e,n_e)+C(m+e_{i}r-m_e)+D(n-n_e))\\ & =g_{0}r+g(m,n)-(g(m_e,n_e)+C(m+e_{i}r-m_e)+D(n-n_e))& & \text{from (11)}\\ & =g_{0}r+g(m,n)-(g(m_e,n_e)+C(m-m_e)+D(n-n_e))-C{e}_{i}r\\ & =g_{0}r+g(m,n)-(g(m_e,n_e)+C(m-m_e)+D(n-n_e))-g_{0}r& & \text{from (12)}\\ & =g(m,n)-(g(m_e,n_e)+C(m-m_e)+D(n-n_e))\\ & =e(m,n).\end{array}$$

What this means is that our state-space model is just as accurate near $(m_e+e_{i}r,n_e)$ as it is near the equilibrium point $(m_e,n_e)$.

---

Invariance of control. Suppose we implement linear state feedback with reference tracking:

$$u=-K(\hat{x}-x_{\text{des}})$$

where

$$x_{\text{des}}=e_{i}r$$

for any $r\in \mathbb{R}$. Let’s assume (for now) that $r$ is constant, and so $x_{\text{des}}$ is also constant. What will $x(t)$ converge to in this case? Let’s find out. First, we define the state error

$$z=x-x_{\text{des}}$$

and the state estimate error

$$x_{\text{err}}=\hat{x}-x.$$

Second, we derive an expression for the closed-loop system in terms of these errors. Let’s start with the state error:

$$\begin{array}{rl}\dot{z}& =\frac{d}{dt}(x-x_{\text{des}})\\ & =\dot{x}-0\\ & =Ax+Bu\\ & =A(z+x_{\text{des}})+B(-K(\hat{x}-x))\\ & =A(z+x_{\text{des}})-BK(x_{\text{err}}+z)\\ & =(A-BK)z-BK{x}_{\text{err}}+A{x}_{\text{des}}\\ & =(A-BK)z-BK{x}_{\text{err}}+A{e}_{i}r\\ & =(A-BK)z-BK{x}_{\text{err}}& & \text{because of (10)}.\end{array}$$

Now, for the state estimate error:

$$\begin{array}{rl}{\dot{x}}_{\text{err}}& =\dot{\hat{x}}-\dot{x}\\ & =(A\hat{x}+Bu-L(C\hat{x}-y))-(Ax+Bu)\\ & =A(\hat{x}-x)-LC(\hat{x}-x)\\ & =(A-LC)x_{\text{err}}.\end{array}$$

Putting these together, we have

$$\left[\begin{array}{c}\dot{z}\\ {\dot{x}}_{\text{err}}\end{array}\right]=\left[\begin{array}{cc}A-BK& -BK\\ 0& A-LC\end{array}\right]\left[\begin{array}{c}z\\ x_{\text{err}}\end{array}\right].$$

This means that

$$z(t)\to 0\text{and}{x}_{\text{err}}(t)\to 0\text{as}t\to \mathrm{\infty }$$

or equivalently that

$$x(t)\to x_{\text{des}}\hat{x}(t)\to x(t)\text{as}t\to \mathrm{\infty }$$

so long as all eigenvalues of $A-BK$ and all eigenvalues of $A-LC$ have negative real part — exactly the same conditions under which the closed-loop system without reference tracking would have been asymptotically stable.

SUMMARY (reference tracking with partial state feedback)

Consider a system $$\begin{array}{rlrl}\dot{m}& =f(m,n)& & \text{dynamic model}\\ o& =g(m,n)& & \text{sensor model}\end{array}$$ that satisfies $$f(m+e_{i}r,n)=f(m,n)\text{for any}r\in \mathbb{R}$$ and $$g(m+e_{i}r,n)=g_{0}r+g(m,n)\text{for all}r\in \mathbb{R}$$ for some constant vector $g_0$. Linearize this system about some equilibrium point $(m_e,n_e)$ to produce the state-space model $$\begin{array}{rl}\dot{x}& =Ax+Bu\\ y& =Cx+Du\end{array}$$ where $$x=m-m_{e}u=n-n_{e}y=o-g(m_e,n_e).$$ Apply the observer $$\hat{x}=A\hat{x}+Bu-L(C\hat{x}-y)$$ and the controller (with reference tracking) $$u=-K(\hat{x}-x_{\text{des}})$$ where $$x_{\text{des}}=e_{i}r$$ for any $r\in \mathbb{R}$. Then, $$x(t)\to x_{\text{des}}\hat{x}(t)\to x(t)\text{as}t\to \mathrm{\infty }$$ if and only if all eigenvalues of $A-BK$ and all eigenvalues of $A-LC$ have negative real part.

Tracking more than one element of the state

Our discussion of reference tracking with full state feedback and with partial state feedback has assumed that we want to track desired values of exactly one element $m_i$ of the nonlinear state $m$. All of this generalizes immediately to the case where we want to track desired values of more than one element of $m$.

In particular, suppose there are two indices $i,j\in \{1,\dots ,\ell \}$ that satisfy

$$f(m+e_i{r}_i+e_j{r}_j,n)=f(m,n)\text{for any}{r}_i,r_j\in \mathbb{R}$$

and, in the case of partial state feedback, that also satisfy

$$g(m+e_i{r}_i+e_j{r}_j,n)=g_{0i}{r}_i+g_{0j}{r}_j+g(m,n)\text{for any}{r}_i,r_j\in \mathbb{R}$$

for constant vectors $g_{0i}$ and $g_{0j}$. Then, choosing

$$x_{\text{des}}=e_i{r}_i+e_j{r}_j$$

would produce the same results that were derived previously.

Choosing the desired state to keep errors small

Our proof that tracking “works” relies largely on having shown that our state-space model is just as accurate near $(m_e+e_{i}r,n_e)$ as it is near the equilibrium point $(m_e,n_e)$. Equivalently, it relies on having shown that this model is just as accurate near $x=x_{\text{des}}$ as it is near $x=0$.

Despite this fact, it is still important to keep the state error

$$x-x_{\text{des}}$$

small. The reason is that the input is proportional to the error — for full state feedback as

$$u=-K(x-x_{\text{des}})$$

and for partial state feedback as

$$u=-K(\hat{x}-x_{\text{des}}).$$

So, if error is large, the input may exceed bounds (e.g., limits on actuator torque). Since our state-space model does not include these bounds, it may be inaccurate when inputs are large.

As a consequence, it is important in practice to choose $x_{\text{des}}$ so that the state error

$$x-x_{\text{des}}$$

remains small. Here is one common way to do this:

Choosing $x_{\text{des}}$ in the case of full state feedback

Suppose $e_i{r}_{\text{goal}}$ is the state you actually want to achieve. Then, for some $e_{\text{max}}>0$, choose $$x_{\text{des}}=e_{i}r$$ where $$r=\{\begin{array}{ll}{e}_i^{T}x+\left({\displaystyle \frac{r_{\text{goal}}-e_i^{T}x}{\Vert r_{\text{goal}}-e_i^{T}x\Vert }}\right)& \text{if }\Vert r_{\text{goal}}-e_i^{T}x\Vert >e_{\text{max}},\\ r_{\text{goal}}& \text{otherwise.}\end{array}$$

Choosing $x_{\text{des}}$ in the case of partial state feedback

Suppose $e_i{r}_{\text{goal}}$ is the state you actually want to achieve. Then, for some $e_{\text{max}}>0$, choose $$x_{\text{des}}=e_{i}r$$ where $$r=\{\begin{array}{ll}{e}_i^T\hat{x}+\left({\displaystyle \frac{r_{\text{goal}}-e_i^T\hat{x}}{\Vert r_{\text{goal}}-e_i^T\hat{x}\Vert }}\right)& \text{if }\Vert r_{\text{goal}}-e_i^T\hat{x}\Vert >e_{\text{max}},\\ r_{\text{goal}}& \text{otherwise.}\end{array}$$