Merge pull request #77 from JuliaControl/online_weigts_adapt

Added: online modification of weights and covariances

Author: franckgaga

README.md

@@ -85,10 +85,10 @@ for more detailed examples.
   - [x] input setpoint tracking
   - [x] terminal costs
   - [x] economic costs (economic model predictive control)
-- [ ] adaptive linear model predictive controller
+- [x] adaptive linear model predictive controller
   - [x] manual model modification
   - [x] automatic successive linearization of a nonlinear model
-  - [ ] objective function weights modification
+  - [x] objective function weights and covariance matrices modification
 - [x] explicit predictive controller for problems without constraint
 - [x] online-tunable soft and hard constraints on:
   - [x] output predictions
@@ -126,7 +126,9 @@ for more detailed examples.
   - [x] manipulated inputs
   - [x] measured outputs
 - [x] bumpless manual to automatic transfer for control with a proper initial estimate
-- [x] observers in predictor form to ease control applications
+- [ ] estimators in two possible forms:
+  - [x] predictor (or delayed) form to reduce computational load
+  - [ ] filter (or current) form to improve accuracy and robustness
 - [x] moving horizon estimator in two formulations:
   - [x] linear plant models (quadratic optimization)
   - [x] nonlinear plant models (nonlinear optimization)

docs/src/manual/nonlinmpc.md

@@ -12,8 +12,8 @@ old_logger = global_logger(); global_logger(errlogger);
 ## Nonlinear Model
 
 In this example, the goal is to control the angular position ``θ`` of a pendulum
-attached to a motor. Knowing that the manipulated input is the motor torque ``τ``, the I/O
-vectors are:
+attached to a motor. Knowing that the manipulated input is the motor torque ``τ`` in Nm, the
+I/O vectors are:
 
 ```math
 \begin{aligned}
@@ -49,7 +49,7 @@ 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]
+    τ  = u[1]               # [Nm]
     dθ = ω
     dω = -g/L*sin(θ) - K/m*ω + τ/m/L^2
     return [dθ, dω]
@@ -59,7 +59,7 @@ const par = (9.8, 0.4, 1.2, 0.3)
 f(x, u, _ ) = pendulum(par, x, u)
 h(x, _ )    = [180/π*x[1]]  # [°]
 nu, nx, ny, Ts = 1, 2, 1, 0.1
-vu, vx, vy = ["\$τ\$ (N m)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"]
+vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"]
 model = setname!(NonLinModel(f, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
 ```
 

docs/src/public/generic_func.md

@@ -37,7 +37,7 @@ initstate!
 setstate!
 ```
 
-## Set Plant Model
+## Set Model and Weights
 
 ```@docs
 setmodel!

src/controller/construct.jl

@@ -144,9 +144,8 @@ function setconstraint!(
     C_Δumin = C_Deltaumin, C_Δumax = C_Deltaumax,
 )
     model, con, optim = mpc.estim.model, mpc.con, mpc.optim
-    nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc
+    nu, ny, nx̂, Hp, Hc, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.nϵ
     notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED)
-    C = mpc.C
     isnothing(Umin)     && !isnothing(umin)     && (Umin    = repeat(umin,    Hp))
     isnothing(Umax)     && !isnothing(umax)     && (Umax    = repeat(umax,    Hp))
     isnothing(ΔUmin)    && !isnothing(Δumin)    && (ΔUmin   = repeat(Δumin,   Hc))
@@ -160,7 +159,7 @@ function setconstraint!(
     isnothing(C_ymin)   && !isnothing(c_ymin)   && (C_ymin  = repeat(c_ymin,  Hp))
     isnothing(C_ymax)   && !isnothing(c_ymax)   && (C_ymax  = repeat(c_ymax,  Hp))
     if !all(isnothing.((C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max)))
-        !isinf(C) || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
+        nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
         notSolvedYet || error("Cannot set softness parameters after calling moveinput!")
     end
     if !isnothing(Umin)
@@ -605,6 +604,7 @@ function init_defaultcon_mpc(
 ) where {NT<:Real}
     model = estim.model
     nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
+    nϵ = isinf(C) ? 0 : 1
     u0min,      u0max   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     Δumin,      Δumax   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     y0min,      y0max   = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny)
@@ -617,12 +617,12 @@ function init_defaultcon_mpc(
         repeat_constraints(Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max)
     C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = 
         repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax)
-    A_Umin,  A_Umax, S̃  = relaxU(model, C, C_umin, C_umax, S)
+    A_Umin,  A_Umax, S̃  = relaxU(model, nϵ, C_umin, C_umax, S)
     A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(
-        model, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
+        model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
     )
-    A_Ymin,  A_Ymax, Ẽ  = relaxŶ(model, C, C_ymin, C_ymax, E)
-    A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(model, C, c_x̂min, c_x̂max, ex̂)
+    A_Ymin,  A_Ymax, Ẽ  = relaxŶ(model, nϵ, C_ymin, C_ymax, E)
+    A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(model, nϵ, c_x̂min, c_x̂max, ex̂)
     i_Umin,  i_Umax  = .!isinf.(U0min),  .!isinf.(U0max)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Y0min),  .!isinf.(Y0max)
@@ -639,7 +639,7 @@ function init_defaultcon_mpc(
         A_Umin  , A_Umax, A_ΔŨmin, A_ΔŨmax  , A_Ymin , A_Ymax , A_x̂min , A_x̂max,
         A       , b     , i_b    , C_ymin   , C_ymax , c_x̂min , c_x̂max , i_g
     )
-    return con, S̃, Ñ_Hc, Ẽ
+    return con, nϵ, S̃, Ñ_Hc, Ẽ
 end
 
 "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
@@ -656,7 +656,7 @@ end
 
 
 @doc raw"""
-    relaxU(model, C, C_umin, C_umax, S) -> A_Umin, A_Umax, S̃
+    relaxU(model, nϵ, C_umin, C_umax, S) -> A_Umin, A_Umax, S̃
 
 Augment manipulated inputs constraints with slack variable ϵ for softening.
 
@@ -678,8 +678,8 @@ constraints:
 in which ``\mathbf{U_{min}, U_{max}}`` and ``\mathbf{U_{op}}`` vectors respectively contains
 ``\mathbf{u_{min}, u_{max}}`` and ``\mathbf{u_{op}}`` repeated ``H_p`` times.
 """
-function relaxU(::SimModel{NT}, C, C_umin, C_umax, S) where {NT<:Real}
-    if !isinf(C) # ΔŨ = [ΔU; ϵ]
+function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real
+    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
         # ϵ impacts ΔU → U conversion for constraint calculations:
         A_Umin, A_Umax = -[S  C_umin], [S -C_umax] 
         # ϵ has no impact on ΔU → U conversion for prediction calculations:
@@ -693,7 +693,7 @@ end
 
 @doc raw"""
     relaxΔU(
-        model, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
+        model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc
     ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc
 
 Augment input increments constraints with slack variable ϵ for softening.
@@ -714,9 +714,9 @@ returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{
 \end{bmatrix}
 ```
 """
-function relaxΔU(::SimModel{NT}, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) where {NT<:Real}
+function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) where NT<:Real
     nΔU = size(N_Hc, 1)
-    if !isinf(C) # ΔŨ = [ΔU; ϵ]
+    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
         # 0 ≤ ϵ ≤ ∞  
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]]
         A_ϵ = [zeros(NT, 1, length(ΔUmin)) NT[1.0]]
@@ -732,7 +732,7 @@ function relaxΔU(::SimModel{NT}, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) w
 end
 
 @doc raw"""
-    relaxŶ(::LinModel, C, C_ymin, C_ymax, E) -> A_Ymin, A_Ymax, Ẽ
+    relaxŶ(::LinModel, nϵ, C_ymin, C_ymax, E) -> A_Ymin, A_Ymax, Ẽ
 
 Augment linear output prediction constraints with slack variable ϵ for softening.
 
@@ -753,8 +753,8 @@ Denoting the input increments augmented with the slack variable
 in which ``\mathbf{Y_{min}, Y_{max}}`` and ``\mathbf{Y_{op}}`` vectors respectively contains
 ``\mathbf{y_{min}, y_{max}}`` and ``\mathbf{y_{op}}`` repeated ``H_p`` times.
 """
-function relaxŶ(::LinModel{NT}, C, C_ymin, C_ymax, E) where {NT<:Real}
-    if !isinf(C) # ΔŨ = [ΔU; ϵ]
+function relaxŶ(::LinModel{NT}, nϵ, C_ymin, C_ymax, E) where NT<:Real
+    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
         # ϵ impacts predicted output constraint calculations:
         A_Ymin, A_Ymax = -[E  C_ymin], [E -C_ymax] 
         # ϵ has no impact on output predictions:
@@ -767,14 +767,14 @@ function relaxŶ(::LinModel{NT}, C, C_ymin, C_ymax, E) where {NT<:Real}
 end
 
 "Return empty matrices if model is not a [`LinModel`](@ref)"
-function relaxŶ(::SimModel{NT}, C, C_ymin, C_ymax, E) where {NT<:Real}
-    Ẽ = !isinf(C) ? [E zeros(NT, 0, 1)] : E
+function relaxŶ(::SimModel{NT}, nϵ, C_ymin, C_ymax, E) where NT<:Real
+    Ẽ = [E zeros(NT, 0, nϵ)]
     A_Ymin, A_Ymax = -Ẽ,  Ẽ 
     return A_Ymin, A_Ymax, Ẽ
 end
 
 @doc raw"""
-    relaxterminal(::LinModel, C, c_x̂min, c_x̂max, ex̂) -> A_x̂min, A_x̂max, ẽx̂
+    relaxterminal(::LinModel, nϵ, c_x̂min, c_x̂max, ex̂) -> A_x̂min, A_x̂max, ẽx̂
 
 Augment terminal state constraints with slack variable ϵ for softening.
 
@@ -794,8 +794,8 @@ the inequality constraints:
 \end{bmatrix}
 ```
 """
-function relaxterminal(::LinModel{NT}, C, c_x̂min, c_x̂max, ex̂) where {NT<:Real}
-    if !isinf(C) # ΔŨ = [ΔU; ϵ]
+function relaxterminal(::LinModel{NT}, nϵ, c_x̂min, c_x̂max, ex̂) where {NT<:Real}
+    if nϵ == 1 # ΔŨ = [ΔU; ϵ]
         # ϵ impacts terminal state constraint calculations:
         A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max]
         # ϵ has no impact on terminal state predictions:
@@ -808,8 +808,8 @@ function relaxterminal(::LinModel{NT}, C, c_x̂min, c_x̂max, ex̂) where {NT<:R
 end
 
 "Return empty matrices if model is not a [`LinModel`](@ref)"
-function relaxterminal(::SimModel{NT}, C, c_x̂min, c_x̂max, ex̂) where {NT<:Real}
-    ẽx̂ = !isinf(C) ? [ex̂ zeros(NT, 0, 1)] : ex̂
+function relaxterminal(::SimModel{NT}, nϵ, c_x̂min, c_x̂max, ex̂) where {NT<:Real}
+    ẽx̂ = [ex̂ zeros(NT, 0, nϵ)]
     A_x̂min, A_x̂max = -ẽx̂,  ẽx̂
     return A_x̂min, A_x̂max, ẽx̂
 end
@@ -853,7 +853,7 @@ function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real
 end
 
 "Validate predictive controller weight and horizon specified values."
-function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C, E=nothing)
+function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing)
     nu, ny = model.nu, model.ny
     nM, nN, nL = ny*Hp, nu*Hc, nu*Hp
     Hp < 1  && throw(ArgumentError("Prediction horizon Hp should be ≥ 1"))

src/controller/execute.jl

@@ -127,7 +127,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     Ŷs .= mpc.F # predictstoch! init mpc.F with Ŷs value if estim is an InternalModel
     mpc.F .= oldF  # restore old F value
     info[:ΔU]   = mpc.ΔŨ[1:mpc.Hc*model.nu]
-    info[:ϵ]    = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
+    info[:ϵ]    = mpc.nϵ == 1 ? mpc.ΔŨ[end] : NaN
     info[:J]    = J
     info[:U]    = U0 + mpc.Uop
     info[:u]    = info[:U][1:model.nu]
@@ -468,7 +468,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     model = mpc.estim.model
     ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar]
     # initial ΔŨ (warm-start): [Δu_{k-1}(k); Δu_{k-1}(k+1); ... ; 0_{nu × 1}; ϵ_{k-1}]
-    ϵ0  = !isinf(mpc.C) ? mpc.ΔŨ[end] : empty(mpc.ΔŨ)
+    ϵ0  = (mpc.nϵ == 1) ? mpc.ΔŨ[end] : empty(mpc.ΔŨ)
     ΔŨ0 = [mpc.ΔŨ[(model.nu+1):(mpc.Hc*model.nu)]; zeros(NT, model.nu); ϵ0]
     JuMP.set_start_value.(ΔŨvar, ΔŨ0)
     set_objective_linear_coef!(mpc, ΔŨvar)
@@ -523,46 +523,70 @@ Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`.
 setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
 
 
-"""
-    setmodel!(mpc::PredictiveController, model::LinModel) -> mpc
+@doc raw"""
+    setmodel!(mpc::PredictiveController, model=mpc.estim.model, <keyword arguments>) -> mpc
+
+Set `model` and objective function weights of `mpc` [`PredictiveController`](@ref).
 
-Set model and operating points of `mpc` [`PredictiveController`](@ref) to `model` values.
+Allows model adaptation of controllers based on [`LinModel`](@ref) at runtime. Modification
+of [`NonLinModel`](@ref) state-space functions is not supported. New weight matrices in the
+objective function can be specified with the keyword arguments (see [`LinMPC`](@ref) for the
+nomenclature). If `Cwt ≠ Inf`, the augmented move suppression weight is ``\mathbf{Ñ}_{H_c} =
+\mathrm{diag}(\mathbf{N}_{H_c}, C)``, else ``\mathbf{Ñ}_{H_c} = \mathbf{N}_{H_c}``. The
+[`StateEstimator`](@ref) `mpc.estim` cannot be a [`Luenberger`](@ref) observer or a
+[`SteadyKalmanFilter`](@ref) (the default estimator). Construct the `mpc` object with a
+time-varying [`KalmanFilter`](@ref) instead. Note that the model is constant over the
+prediction horizon ``H_p``.
 
-Allows model adaptation of controllers based on [`LinModel`](@ref) at runtime ([`NonLinModel`](@ref)
-is not supported). The [`StateEstimator`](@ref) `mpc.estim` cannot be a [`Luenberger`](@ref)
-observer or a [`SteadyKalmanFilter`](@ref) (the default estimator). Construct the `mpc`
-object with a time-varying [`KalmanFilter`](@ref) instead. Note that the model is constant
-over the prediction horizon ``H_p``.
+# Arguments
+
+- `mpc::PredictiveController` : controller to set model and weights.
+- `model=mpc.estim.model` : new plant model ([`NonLinModel`](@ref) not supported).
+- `M_Hp=mpc.M_Hp` : new ``\mathbf{M_{H_p}}`` weight matrix.
+- `Ñ_Hc=mpc.Ñ_Hc` : new ``\mathbf{Ñ_{H_c}}`` weight matrix (see definition above).
+- `L_Hp=mpc.L_Hp` : new ``\mathbf{L_{H_p}}`` weight matrix.
+- additional keyword arguments are passed to `setmodel!(::StateEstimator)`.
 
 # Examples
 ```jldoctest
-julia> kf = KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)));
+julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1);
+
+julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
+(0.1, 25.0, 1.0)
 
-julia> mpc = LinMPC(kf); mpc.estim.model.A
-1×1 Matrix{Float64}:
- 0.1
+julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[0]);
 
-julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0))); mpc.estim.model.A
-1×1 Matrix{Float64}:
- 0.42
+julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
+(0.42, 9.0, 0.0)
 ```
 """
-function setmodel!(mpc::PredictiveController, model::LinModel)
+function setmodel!(
+        mpc::PredictiveController, 
+        model = mpc.estim.model;
+        M_Hp = mpc.M_Hp,
+        Ñ_Hc = mpc.Ñ_Hc,
+        L_Hp = mpc.L_Hp,
+        kwargs...
+    )
     x̂op_old = copy(mpc.estim.x̂op)
-    setmodel!(mpc.estim, model)
-    setmodel_controller!(mpc, model, x̂op_old)
+    nu, ny, Hp, Hc, nϵ = model.nu, model.ny, mpc.Hp, mpc.Hc, mpc.nϵ
+    setmodel!(mpc.estim, model; kwargs...)
+    mpc.M_Hp .= to_hermitian(M_Hp)
+    mpc.Ñ_Hc .= to_hermitian(Ñ_Hc)
+    mpc.L_Hp .= to_hermitian(L_Hp)
+    setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
     return mpc
 end
 
 "Update the prediction matrices, linear constraints and JuMP optimization."
-function setmodel_controller!(mpc::PredictiveController, model::LinModel, x̂op_old)
-    estim = mpc.estim
+function setmodel_controller!(mpc::PredictiveController, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
+    estim, model = mpc.estim, mpc.estim.model
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
     # --- predictions matrices ---
     E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
-    A_Ymin, A_Ymax, Ẽ = relaxŶ(model, mpc.C, con.C_ymin, con.C_ymax, E)
-    A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, mpc.C, con.c_x̂min, con.c_x̂max, ex̂)
+    A_Ymin, A_Ymax, Ẽ = relaxŶ(model, mpc.nϵ, con.C_ymin, con.C_ymax, E)
+    A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, mpc.nϵ, con.c_x̂min, con.c_x̂max, ex̂)
     mpc.Ẽ .= Ẽ
     mpc.G .= G
     mpc.J .= J
@@ -598,11 +622,20 @@ function setmodel_controller!(mpc::PredictiveController, model::LinModel, x̂op_
     con.A_Ymax .= A_Ymax
     con.A_x̂min .= A_x̂min
     con.A_x̂max .= A_x̂max
-    nUandΔŨ = length(con.U0min) + length(con.U0max) + length(con.ΔŨmin) + length(con.ΔŨmax)
-    con.A[nUandΔŨ+1:end, :] = [con.A_Ymin; con.A_Ymax; con.A_x̂min; con.A_x̂max]
+    con.A .= [
+        con.A_Umin
+        con.A_Umax 
+        con.A_ΔŨmin 
+        con.A_ΔŨmax 
+        con.A_Ymin  
+        con.A_Ymax 
+        con.A_x̂min  
+        con.A_x̂max
+    ]
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
     ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar]
+    # deletion is required for sparse solvers like OSQP, when the sparsity pattern changes
     JuMP.delete(optim, optim[:linconstraint])
     JuMP.unregister(optim, :linconstraint)
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)