add NonLinMPC example in Manual

Author: franckgaga

docs/src/manual.md

@@ -136,3 +136,93 @@ p3 = plot(t_data,u_data[1,:],label="cold", linetype=:steppost); ylabel!("flow ra
 plot!(t_data,u_data[2,:],label="hot", linetype=:steppost); xlabel!("time (s)")
 p = plot(p1, p2, p3, layout=(3,1), fmt=:svg)
 ```
+
+### Nonlinear Model
+
+In this example, the goal is to control the angular position ``θ`` of a pendulum
+attached to a motor. If the manipulated input is the motor torque ``τ``, the vectors
+are:
+
+```math
+\begin{aligned}
+    \mathbf{u} &= \begin{bmatrix} τ \end{bmatrix} \\
+    \mathbf{y} &= \begin{bmatrix} θ \end{bmatrix}
+\end{aligned}
+```
+
+The plant model is nonlinear:
+
+```math
+\begin{aligned}
+    \dot{θ}(t) &= ω(t)                                                                    \\
+    \dot{ω}(t) &= -\frac{g}{L}\sin\big( θ(t) \big) - \frac{k}{m} ω(t) + \frac{m}{L^2}τ(t)
+\end{aligned}
+```
+
+in which ``g`` is the gravitational acceleration, ``L``, the pendulum length, ``k``, the
+friction coefficient at the pivot point, and ``m``, the mass attached at the end of the
+pendulum. Here, the explicit Euler method discretizes the system to construct a
+[`NonLinModel`](@ref):
+
+```@example 2
+using ModelPredictiveControl
+function pendulum(par, x, u)
+    g, L, k, m = par        # [m/s], [m], [kg/s], [kg]
+    θ, ω = x[1], x[2]       # [rad], [rad/s]
+    τ  = u[1]               # [N m]
+    dθ = ω
+    dω = -g/L*sin(θ) - k/m*ω + τ/m/L^2
+    return [dθ, dω]
+end
+Ts  = 0.1                   # [s]
+par = (9.8, 0.4, 1.2, 0.3)
+f(x, u, _ ) = x + Ts*pendulum(par, x, u) # Euler method
+h(x, _ )    = [180/π*x[1]]  # [°]
+nu, nx, ny = 1, 2, 1
+model = NonLinModel(f, h, Ts, nu, nx, ny)
+```
+
+The output function ``\mathbf{h}`` converts the angular position ``θ`` to degrees. It
+is good practice to first simulate `model` using [`sim!`](@ref) as a quick sanity check:
+
+```@example 2
+using Plots
+u = [0.5] # τ = 0.5 N m
+plot(sim!(model, 60, u), plotu=false)
+```
+
+An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
+
+```@example 2
+estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[0.5])
+```
+
+The standard deviation of the angular velocity ``ω`` is higher here (second value of `σQ`)
+since ``\dot{ω}(t)`` equation includes the friction coefficient ``k``, an uncertain
+parameter. The estimator tuning is tested on a simulated plant with a different ``k`` value:
+
+```@example 2
+par_plant = (par[1], par[2], par[3] + 0.25, par[4])
+f_plant(x, u, _) = x + Ts*pendulum(par_plant, x, u)
+plant = NonLinModel(f_plant, h, Ts, nu, nx, ny)
+res = sim!(estim, 30, [0.5], plant=plant, y_noise=[0.5]) # τ = 0.5 N m
+p2 = plot(res, plotu=false, plotx=true, plotx̂=true)
+```
+
+The Kalman filter performance seems sufficient for control applications. As the motor torque
+is limited to -1.5 to 1.5 N m, we incorporate the manipulated input constraints in a
+[`NonLinMPC`](@ref):
+
+```@example 2
+mpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.1], Nwt=[1.0], Cwt=Inf)
+mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
+```
+
+We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted position):
+
+```@example 2
+res = sim!(mpc, 30, [180.0], x̂0=zeros(mpc.estim.nx̂), plant=plant, x0=zeros(plant.nx))
+plot(res, plotŷ=true)
+```
+
+The controller here seems robust enough to variations on ``k`` coefficients.

docs/src/test.jl

@@ -0,0 +1,39 @@
+using ModelPredictiveControl
+
+function pendulum(par, x, u)
+    g, L, K, m = par    # [m/s], [m], [kg/s], [kg]
+    θ, ω = x[1], x[2]   # [rad], [rad/s]
+    τ  = u[1]           # [N m]
+    dθ = ω
+    dω = -g/L*sin(θ) - K/m*ω + τ/m/L^2
+    return [dθ, dω]
+end
+
+Ts = 0.1    # [s]
+par = (9.8, 0.4, 1.2, 0.3)
+f(x, u, _) = x + Ts*pendulum(par, x, u)
+h(x, _ ) = [180/π*x[1]]      # [rad] to [°]
+nu, nx, ny = 1, 2, 1
+model = NonLinModel(f, h, Ts, nu, nx, ny)
+
+using Plots
+p1 = plot(sim!(model, 50, [0.5])) # τ = 0.5 N m
+display(p1)
+
+par_plant = (par[1], par[2], par[3] + 0.25, par[4])
+f_plant(x, u, _) = x + Ts*pendulum(par_plant, x, u)
+plant = NonLinModel(f_plant, h, Ts, nu, nx, ny)
+
+estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[0.5])
+
+
+res = sim!(estim, 30, [0.5], plant=plant, y_noise=[0.5]) # τ = 0.5 N m
+p2 = plot(res, plotu=false, plotx=true, plotx̂=true)
+display(p2)
+
+
+mpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.1], Nwt=[1.0], Cwt=Inf)
+mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
+
+res = sim!(mpc, 30, [180.0], x̂=zeros(mpc.estim.nx̂), plant=plant)
+plot(res, plotŷ=true)

src/controller/nonlinmpc.jl

@@ -230,7 +230,7 @@ function init_optimization!(mpc::NonLinMPC)
     # --- nonlinear optimization init ---
     model = mpc.estim.model
     ny, nu, Hp, Hc = model.ny, model.nu, mpc.Hp, mpc.Hc
-    nC = (2*Hc*nu + 2*Hc*nu + 2*Hp*ny + 2) - length(mpc.con.b)
+    nC = (2*Hc*nu + 2*nvar + 2*Hp*ny) - length(mpc.con.b)
     # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-functions-with-vector-outputs
     Jfunc, Cfunc = let mpc=mpc, model=model, nC=nC, nvar=nvar , nŶ=Hp*ny
         last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing

src/estimator/internal_model.jl

@@ -39,7 +39,7 @@ struct InternalModel{M<:SimModel} <: StateEstimator
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
-        x̂d = x̂ = copy(model.x) # x̂ and x̂d are same object (updating x̂d will update x̂)
+        x̂d = x̂ = zeros(model.nx) # x̂ and x̂d are same object (updating x̂d will update x̂)
         x̂s = zeros(nxs)
         return new(
             model, 

src/estimator/kalman.jl

@@ -40,7 +40,7 @@ struct SteadyKalmanFilter <: StateEstimator
         end
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
-        x̂ = [copy(model.x); zeros(nxs)]
+        x̂ = [zeros(model.nx); zeros(nxs)]
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
         return new(
@@ -202,7 +202,7 @@ struct KalmanFilter <: StateEstimator
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
-        x̂ = [copy(model.x); zeros(nxs)]
+        x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
@@ -352,7 +352,7 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
         nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
-        x̂ = [copy(model.x); zeros(nxs)]
+        x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)

src/estimator/luenberger.jl

@@ -33,7 +33,7 @@ struct Luenberger <: StateEstimator
         end
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
-        x̂ = [copy(model.x); zeros(nxs)]
+        x̂ = [zeros(model.nx); zeros(nxs)]
         return new(
             model, 
             lastu0, x̂,

src/plot_sim.jl

@@ -186,6 +186,7 @@ function sim_closedloop!(
     lastd, lasty = d, evaloutput(plant, d)
     if isnothing(x̂0)
         initstate!(est_mpc, lastu, lasty[estim.i_ym], lastd)
+
     else
         setstate!(est_mpc, x̂0)
     end