# Solving an optimal control problem (LQR)

Given a linear time-invariant system:

$$\dot{x}(t)=Ax(t)+Bu(t)$$

with initial state $x(0)=x_0$ and final state $x(T)=x_T$.

The performance measure to be minimized is:

$$\int_{0}^{T} ((x_T-x(t))^T(x_T-x(t))+u(t)^Tu(t) dt$$

$(x_T-x(t))$ is the difference between the state of the system at time $t$ and the final state. I want to compute an optimal control $u^*$ that induces a transition from the initial state $x_0$ to the target state $x_T$. This is a LQR.

So if I let

$$A=\begin{bmatrix} -1 & 0.5 \\ 0.3 & -1 \end{bmatrix}, B=\begin{bmatrix} 1 \\ 1 \end{bmatrix}$$ and let the initial state be $x_0=\begin{bmatrix} 1 \\ 0 \end{bmatrix}$ and the final state be $x_T=\begin{bmatrix} 0 \\ 1 \end{bmatrix}$.

What would be the optimal control trajectories in this specific case? I know there are some functions in mathematica for control theoretical application, but I have zero experience using them and just started to get deeper into this topic.

I would appreciate any help on this!

• There are no built-in functions for this. But you could setup the Hamiltonian and costate equations and use NDSolve to solve them. Oct 20 '17 at 20:28
• $B$ is a $2$x$1$ matrix, and $u$ is a vector of length $1$. Oct 20 '17 at 20:46
• @SubaThomas: Thanks for the hint, I really thought mathematica has some functions for that ;) Oct 21 '17 at 12:34

my solution

A={{-1,0.5},{0.3,-1}}; B={{1},{1}};
x0={{1},{0}}; xT={{0},{1}};


You have to choose an end time

T=10;


Next you have to define the Hamilton function

L[t_]=1/2(u[t]^2)
lambda[t_]:={{l1[t]},{l2[t]}}
x[t_] := {{x1[t]}, {x2[t]}}
f[t_] = A.x[t] + B u[t]
H[t_] = Flatten[L[t] + lambda[t]\[Transpose].f[t]][[1]]


Then, according to the calculus of variations the following has to hold

uSol = First@Solve[0 == -D[H[t], u[t]], u[t]]


TableForm[eqn1=Table[D[lambda[t][[i,1]],t]==-D[H[t]/.uSol,x[t][[i,1]]],{i,1,2}]]


TableForm[
eqn2 = Table[
D[x[t][[i, 1]], t] == D[H[t] /. uSol, lambda[t][[i, 1]]], {i, 1,
2}]]


Now you have a boundary value problem. You just need boundary values

bc1 = Table[x[0][[i, 1]] == x0[[i, 1]], {i, 1, 2}]
bc2 = Table[x[T][[i, 1]] == xT[[i, 1]], {i, 1, 2}]


And you are ready to solve the bvp

sol = NDSolve[
Flatten[{eqn1, eqn2, bc1, bc2}], {x1[t], x2[t], l1[t], l2[t]}, {t,
0, T}]


which gives the numerical solution functions, that you can plot.

Grid[{{Plot[{Evaluate[x1[t] /. sol], Evaluate[x2[t] /. sol]}, {t, 0,
T}, ImageSize -> Medium],
Plot[{Evaluate[x1[t] /. sol], Evaluate[x2[t] /. sol]}, {t, 0, T},
PlotRange -> {Automatic, {-1, 1}}, ImageSize -> Medium]}}]


You can also verify the solution

sol /. t -> T


and plot the optimal control signal

Plot[{Evaluate[u[t] /. uSol /. sol]}, {t, 0, T}, ImageSize -> Medium]


As you can see the final state is almost exactly the requested. The control effort is dependent on the time T. All of the above would also work for nonlinear problems and with little effort for higher order systems.

Edit:

You could also define the Lagrange function as follows

L[t_] = 1/2 (x1[t]^2 + x2[t]^2 + u[t]^2)


This weighting off the states is not mandatory because the resulting weighting matrix Q just has to be positiv semi definit, so all zeros would be an option. By the way the above weighting corresponds to Q being a unity matrix. With this you get

As you can see the states deviate a little less from zero than before.

• Thanks so much, that helped me understand how to work this problem in mathematica ! I'm confused about one thing though: 1. Shouldn't the Lagrange function be $(x1[t]-x2[t])^2+u[t]^2$ based on the cost function I specified above? Oct 20 '17 at 23:17
• I don't think so the weighting off the final state can not be part of the integral, after all it only would be a scalar value and no time function. If you have a look at: en.wikipedia.org/wiki/… you can see the weighting is done by the first part in front off the integral. Oct 21 '17 at 18:00
• I see, thank you! Just wondering one more thing, shouldn't there be two optimal control signals $u_1(t)$ and $u_2(t)$? Oct 21 '17 at 18:41
• You have a single input system (the input matrix has one column) so no. Oct 21 '17 at 19:03
• Ah my mistake, I got confused! Thanks again! Oct 21 '17 at 19:27