Solving an optimal control problem (LQR)

Question

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!

Answer 1

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.