A simple Linear–quadratic regulator example

Problem statement

We consider the following Linear Quadratic Regulator (LQR) problem, which consists in minimizing

\[ \frac{1}{2} \int_{0}^{t_f} \left( x_1^2(t) + x_2^2(t) + u^2(t) \right) \, \mathrm{d}t \]

subject to the dynamics

\[ \dot x_1(t) = x_2(t), \quad \dot x_2(t) = -x_1(t) + u(t), \quad u(t) \in \R\]

and the initial condition

\[ x(0) = (0,1).\]

We aim to solve this optimal control problem for different values of $t_f$.

Required packages

We begin by importing the necessary packages:

using OptimalControl
using NLPModelsIpopt
using Plots
using Plots.PlotMeasures # for leftmargin, bottommargin

Problem definition

We define the LQR problem as a function of the final time tf:

function lqr(tf)

    x0 = [ 0
           1 ]

    ocp = @def begin
        t ∈ [0, tf], time
        x ∈ R², state
        u ∈ R, control
        x(0) == x0
        ẋ(t) == [x₂(t), - x₁(t) + u(t)]
        0.5∫( x₁(t)^2 + x₂(t)^2 + u(t)^2 ) → min
    end

    return ocp
end

Solving the problem for different final times

We solve the problem for $t_f \in \{3, 5, 30\}$.

solutions = []   # empty list of solutions
tfs = [3, 5, 30]

for tf ∈ tfs
    solution = solve(lqr(tf), display=false)
    push!(solutions, solution)
end

Plotting the Solutions

We plot the state and control variables using normalized time $s = (t - t_0)/(t_f - t_0)$:

plt = plot(solutions[1], :state, :control; time=:normalize, label="tf = $(tfs[1])")
for (tf, sol) ∈ zip(tfs[2:end], solutions[2:end])
    plot!(plt, sol, :state, :control; time=:normalize, label="tf = $tf")
end

px1 = plot(plt[1], legend=false, xlabel="s", ylabel="x₁")
px2 = plot(plt[2], legend=true, xlabel="s", ylabel="x₂")
pu  = plot(plt[3], legend=false, xlabel="s", ylabel="u")
plot(px1, px2, pu, layout=(1, 3), size=(800, 300), leftmargin=5mm, bottommargin=5mm)

Limitations: using matrix formulation

The following definition will lead to an error when solving the problem:

julia> x0 = [ 0
              1 ]2-element Vector{Int64}:
 0
 1
julia> A = [  0 1
             -1 0 ]2×2 Matrix{Int64}:
  0  1
 -1  0
julia> B = [ 0
             1 ]2-element Vector{Int64}:
 0
 1
julia> Q = [ 1 0
             0 1 ]2×2 Matrix{Int64}:
 1  0
 0  1
julia> R = 11
julia> tf = 33
julia> ocp = @def begin
           t ∈ [0, tf], time
           x ∈ R², state
           u ∈ R, control
           x(0) == x0
           ẋ(t) == A * x(t) + B * u(t)
           0.5∫( x(t)' * Q * x(t) + u(t)' * R * u(t) ) → min
       end    t ∈ [0, tf], time
    x ∈ R², state
    u ∈ R, control
    x(0) == x0
    ẋ(t) == A * x(t) + B * u(t)
    0.5 * ∫((x(t))' * Q * x(t) + (u(t))' * R * u(t)) → min

The optimal control problem is of the form:

    minimize  J(x, u) = ∫ f⁰(t, x(t), u(t)) dt, over [0, 3]

    subject to

        ẋ(t) = f(t, x(t), u(t)), t in [0, 3] a.e.,

        ϕ₋ ≤ ϕ(x(0), x(3)) ≤ ϕ₊,

    where x(t) ∈ R² and u(t) ∈ R.

Declarations (* required):
╭────────┬────────┬──────────┬──────────┬───────────┬────────────┬─────────────╮
│ times* │ state* │ control* │ variable │ dynamics* │ objective* │ constraints │
├────────┼────────┼──────────┼──────────┼───────────┼────────────┼─────────────┤
│   V    │   V    │    V     │    X     │     V     │     V      │      V      │
╰────────┴────────┴──────────┴──────────┴───────────┴────────────┴─────────────╯
julia> solve(ocp)ERROR: Cannot determine ordering of Dual tags ForwardDiff.Tag{ReverseDiff.var"#131#134"{typeof(+), ForwardDiff.Dual{ForwardDiff.Tag{ADNLPModels.var"#ψ#72"{CTDirect.var"#14#16"{CTDirect.DOCP{CTDirect.Trapeze, Model{CTModels.TimesModel{CTModels.FixedTimeModel{Int64}, CTModels.FixedTimeModel{Int64}}, CTModels.StateModel, CTModels.ControlModel, CTModels.EmptyVariableModel, Main.var"Main".var"##484#5", CTModels.LagrangeObjectiveModel{Main.var"Main".var"##488#6"}, CTModels.ConstraintsModel{Tuple{Vector{Real}, CTModels.var"#path_cons_nl!#106"{Int64, Vector{Int64}, Tuple{}}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Main.var"Main".var"##478#4", Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}}}}}}, Float64}, Float64, 1}}, ForwardDiff.Dual{ForwardDiff.Tag{ADNLPModels.var"#ψ#72"{CTDirect.var"#14#16"{CTDirect.DOCP{CTDirect.Trapeze, Model{CTModels.TimesModel{CTModels.FixedTimeModel{Int64}, CTModels.FixedTimeModel{Int64}}, CTModels.StateModel, CTModels.ControlModel, CTModels.EmptyVariableModel, Main.var"Main".var"##484#5", CTModels.LagrangeObjectiveModel{Main.var"Main".var"##488#6"}, CTModels.ConstraintsModel{Tuple{Vector{Real}, CTModels.var"#path_cons_nl!#106"{Int64, Vector{Int64}, Tuple{}}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Main.var"Main".var"##478#4", Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}}}}}}, Float64}, Float64, 1}} and ForwardDiff.Tag{ADNLPModels.var"#ψ#72"{CTDirect.var"#14#16"{CTDirect.DOCP{CTDirect.Trapeze, Model{CTModels.TimesModel{CTModels.FixedTimeModel{Int64}, CTModels.FixedTimeModel{Int64}}, CTModels.StateModel, CTModels.ControlModel, CTModels.EmptyVariableModel, Main.var"Main".var"##484#5", CTModels.LagrangeObjectiveModel{Main.var"Main".var"##488#6"}, CTModels.ConstraintsModel{Tuple{Vector{Real}, CTModels.var"#path_cons_nl!#106"{Int64, Vector{Int64}, Tuple{}}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Main.var"Main".var"##478#4", Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}, Tuple{Vector{Real}, Vector{Int64}, Vector{Real}, Vector{Symbol}}}}}}}, Float64}