Merge pull request #54 from JuliaControl/adapt_linmpc

Added: adaptation of controller/estimator based on `LinModel`

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.20.2"
+version = "0.21.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -85,8 +85,12 @@ for more detailed examples.
   - [x] input setpoint tracking
   - [x] terminal costs
   - [x] economic costs (economic model predictive control)
+- [ ] adaptive linear model predictive controller
+  - [x] manual model modification
+  - [x] automatic successive linearization of a nonlinear model
+  - [ ] objective function weights modification
 - [x] explicit predictive controller for problems without constraint
-- [x] soft and hard constraints on:
+- [x] online-tunable soft and hard constraints on:
   - [x] output predictions
   - [x] manipulated inputs
   - [x] manipulated inputs increments
@@ -126,7 +130,7 @@ for more detailed examples.
 - [x] moving horizon estimator in two formulations:
   - [x] linear plant models (quadratic optimization)
   - [x] nonlinear plant models (nonlinear optimization)
-- [x] moving horizon estimator soft and hard constraints on:
+- [x] moving horizon estimator online-tunable soft and hard constraints on:
   - [x] state estimates
   - [x] process noise estimates
   - [x] sensor noise estimates

docs/make.jl

@@ -49,4 +49,5 @@ makedocs(
 deploydocs(
     repo = "github.com/JuliaControl/ModelPredictiveControl.jl.git",
     devbranch = "main",
+    push_preview = true
 )

docs/src/internals/state_estim.md

@@ -11,12 +11,6 @@ ModelPredictiveControl.f̂!
 ModelPredictiveControl.ĥ!
 ```
 
-## Constraint Relaxation
-
-```@docs
-
-```
-
 ## Estimator Construction
 
 ```@docs
@@ -52,6 +46,12 @@ ModelPredictiveControl.evalŷ
 ModelPredictiveControl.remove_op!
 ```
 
+## Init Estimate
+
+```@docs
+ModelPredictiveControl.init_estimate!
+```
+
 ## Update Estimate
 
 !!! info
@@ -62,12 +62,3 @@ ModelPredictiveControl.remove_op!
 ```@docs
 ModelPredictiveControl.update_estimate!
 ```
-
-## Init Estimate
-
-!!! info
-    Same as above: the arguments should be called `u0`, `ym0` and `d0`, strickly speaking.
-
-```@docs
-ModelPredictiveControl.init_estimate!
-```

docs/src/manual/nonlinmpc.md

@@ -108,16 +108,19 @@ As the motor torque is limited to -1.5 to 1.5 N m, we incorporate the input cons
 a [`NonLinMPC`](@ref):
 
 ```@example 1
-nmpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf)
-nmpc = setconstraint!(nmpc, umin=[-1.5], umax=[+1.5])
+Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5]
+nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt=Inf)
+umin, umax = [-1.5], [+1.5]
+nmpc = setconstraint!(nmpc; umin, umax)
 ```
 
-The option `Cwt=Inf` disables the slack variable `ϵ` for constraint softening. We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted position):
+The option `Cwt=Inf` disables the slack variable `ϵ` for constraint softening. We test `mpc`
+performance on `plant` by imposing an angular setpoint of 180° (inverted position):
 
 ```@example 1
 using Logging; disable_logging(Warn)            # hide
 using JuMP; unset_time_limit_sec(nmpc.optim)    # hide
-res_ry = sim!(nmpc, N, [180.0], plant=plant, x0=[0, 0], x̂0=[0, 0, 0])
+res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
 plot(res_ry)
 savefig(ans, "plot3_NonLinMPC.svg"); nothing # hide
 ```
@@ -129,7 +132,7 @@ inverted position, the closed-loop response to a step disturbances of 10° is al
 satisfactory:
 
 ```@example 1
-res_yd = sim!(nmpc, N, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
+res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
 plot(res_yd)
 savefig(ans, "plot4_NonLinMPC.svg"); nothing # hide
 ```
@@ -184,8 +187,8 @@ function JE(UE, ŶE, _ )
     τ, ω = UE[1:end-1], ŶE[2:2:end-1]
     return Ts*sum(τ.*ω)
 end
-empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Cwt=Inf, Ewt=3.5e3, JE=JE)
-empc = setconstraint!(empc, umin=[-1.5], umax=[+1.5])
+empc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=[0.5, 0], Cwt=Inf, Ewt=3.5e3, JE=JE)
+empc = setconstraint!(empc; umin, umax)
 ```
 
 The keyword argument `Ewt` weights the economic costs relative to the other terms in the
@@ -196,7 +199,7 @@ setpoint is similar:
 
 ```@example 1
 unset_time_limit_sec(empc.optim) # hide
-res2_ry = sim!(empc, N, [180, 0], plant=plant2, x0=[0, 0], x̂0=[0, 0, 0])
+res2_ry = sim!(empc, N, [180, 0], plant=plant2, x_0=[0, 0], x̂_0=[0, 0, 0])
 plot(res2_ry)
 savefig(ans, "plot5_NonLinMPC.svg"); nothing # hide
 ```
@@ -216,7 +219,7 @@ Dict(:W_nmpc => calcW(res_ry), :W_empc => calcW(res2_ry))
 Also, for a 10° step disturbance:
 
 ```@example 1
-res2_yd = sim!(empc, N, [180; 0]; plant=plant2, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10, 0])
+res2_yd = sim!(empc, N, [180; 0]; plant=plant2, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10, 0])
 plot(res2_yd)
 savefig(ans, "plot6_NonLinMPC.svg"); nothing # hide
 ```
@@ -232,7 +235,7 @@ Dict(:W_nmpc => calcW(res_yd), :W_empc => calcW(res2_yd))
 Of course, this gain is only exploitable if the motor electronic includes some kind of
 regenerative circuitry.
 
-## Linearizing the Model
+## Model Linearization
 
 Nonlinear MPC is more computationally expensive than [`LinMPC`](@ref). Solving the problem
 should always be faster than the sampling time ``T_s = 0.1`` s for real-time operation. This
@@ -248,15 +251,15 @@ linmodel = linearize(model, x=[π, 0], u=[0])
 A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmodel`:
 
 ```@example 1
-kf  = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
-mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf)
+skf = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
+mpc = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf)
 mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
 ```
 
 The linear controller has difficulties to reject the 10° step disturbance:
 
 ```@example 1
-res_lin = sim!(mpc, N, [180.0]; plant, x0=[π, 0], y_step=[10])
+res_lin = sim!(mpc, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
 plot(res_lin)
 savefig(ans, "plot7_NonLinMPC.svg"); nothing # hide
 ```
@@ -287,22 +290,98 @@ Constructing a [`LinMPC`](@ref) with `DAQP`:
 ```@example 1
 using JuMP, DAQP
 daqp = Model(DAQP.Optimizer, add_bridges=false)
-mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf, optim=daqp)
-mpc2 = setconstraint!(mpc2, umin=[-1.5], umax=[+1.5])
+mpc2 = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp)
+mpc2 = setconstraint!(mpc2; umin, umax)
 ```
 
 does improve the rejection of the step disturbance:
 
 ```@example 1
-res_lin2 = sim!(mpc2, N, [180.0]; plant, x0=[π, 0], y_step=[10])
+res_lin2 = sim!(mpc2, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
 plot(res_lin2)
 savefig(ans, "plot8_NonLinMPC.svg"); nothing # hide
 ```
 
 ![plot8_NonLinMPC](plot8_NonLinMPC.svg)
 
 The closed-loop performance is still lower than the nonlinear controller, as expected, but
-computations are about 2000 times faster (0.00002 s versus 0.04 s per time steps, on
-average). Note that `linmodel` is only valid for angular positions near 180°. Multiple
-linearized models and controllers are required for large deviations from this operating
-point. This is known as gain scheduling.
+computations are about 210 times faster (0.000071 s versus 0.015 s per time steps, on
+average). However, remember that `linmodel` is only valid for angular positions near 180°.
+For example, the 180° setpoint response from 0° is unsatisfactory since the predictions are
+poor in the first quadrant:
+
+```@example 1
+res_lin3 = sim!(mpc2, N, [180.0]; plant, x_0=[0, 0])
+plot(res_lin3)
+savefig(ans, "plot9_NonLinMPC.svg"); nothing # hide
+```
+
+![plot9_NonLinMPC](plot9_NonLinMPC.svg)
+
+Multiple linearized model and controller objects are required for large deviations from this
+operating point. This is known as gain scheduling. Another approach is adapting the model of
+the [`LinMPC`](@ref) instance based on repeated online linearization.
+
+## Adapting the Model via Successive Linearization
+
+The [`setmodel!`](@ref) method allows online adaptation of a linear plant model. Combined
+with the automatic linearization of [`linearize`](@ref), a successive linearization MPC can
+be designed with minimal efforts. The [`SteadyKalmanFilter`](@ref) does not support
+[`setmodel!`](@ref), so we need to use the time-varying [`KalmanFilter`](@ref) instead:
+
+```@example 1
+kf   = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
+mpc3 = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp)
+mpc3 = setconstraint!(mpc3; umin, umax)
+```
+
+We create a function that simulates the plant and the adaptive controller:
+
+```@example 1
+function test_slmpc(nonlinmodel, mpc, ry, plant; x_0=plant.xop, y_step=0)
+    N = 35
+    U_data, Y_data, Ry_data = zeros(plant.nu, N), zeros(plant.ny, N), zeros(plant.ny, N)
+    setstate!(plant, x_0)
+    u, y = [0.0], plant()
+    x̂ = initstate!(mpc, u, y)
+    for i = 1:N
+        y = plant() .+ y_step
+        u = moveinput!(mpc, ry)
+        linmodel = linearize(nonlinmodel; u, x=x̂[1:2])
+        setmodel!(mpc, linmodel)
+        U_data[:,i], Y_data[:,i], Ry_data[:,i] = u, y, ry
+        x̂ = updatestate!(mpc, u, y) # update mpc state estimate
+        updatestate!(plant, u)      # update plant simulator
+    end
+    res = SimResult(mpc, U_data, Y_data; Ry_data)
+    return res
+end
+nothing # hide
+```
+
+The [`setmodel!`](@ref) method must be called after solving the optimization problem with
+[`moveinput!`](@ref), and before updating the state estimate with [`updatestate!`](@ref).
+The [`SimResult`](@ref) object is for plotting purposes only. The adaptive [`LinMPC`](@ref)
+performances are similar to the nonlinear MPC, both for the 180° setpoint:
+
+```@example 1
+res_slin = test_slmpc(model, mpc3, [180], plant, x_0=[0, 0]) 
+plot(res_slin)
+savefig(ans, "plot10_NonLinMPC.svg"); nothing # hide
+```
+
+![plot10_NonLinMPC](plot10_NonLinMPC.svg)
+
+and the 10° step disturbance:
+
+```@example 1
+res_slin = test_slmpc(model, mpc3, [180], plant, x_0=[π, 0], y_step=[10]) 
+plot(res_slin)
+savefig(ans, "plot11_NonLinMPC.svg"); nothing # hide
+```
+
+![plot11_NonLinMPC](plot11_NonLinMPC.svg)
+
+The computations of the successive linearization MPC are about 125 times faster than the
+nonlinear MPC (0.00012 s per time steps versus 0.015 s per time steps, on average), an
+impressive gain for similar closed-loop performances!

docs/src/public/sim_model.md

@@ -53,4 +53,5 @@ setop!
 
 ```@docs
 linearize
+linearize!
 ```

src/ModelPredictiveControl.jl

@@ -23,7 +23,7 @@ import OSQP, Ipopt
 
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta
-export setop!, setstate!, setmodel!, updatestate!, evaloutput, linearize
+export setop!, setstate!, setmodel!, updatestate!, evaloutput, linearize, linearize!
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export MovingHorizonEstimator