Merge pull request #86 from JuliaControl/simpler_setmodel!

Added: `Mwt`, `Nwt` and `Lwt` kwargs in `setmodel!`

Project.toml

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

src/controller/construct.jl

@@ -146,97 +146,127 @@ function setconstraint!(
     model, con, optim = mpc.estim.model, mpc.con, mpc.optim
     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)
-    isnothing(Umin)     && !isnothing(umin)     && (Umin    = repeat(umin,    Hp))
-    isnothing(Umax)     && !isnothing(umax)     && (Umax    = repeat(umax,    Hp))
-    isnothing(ΔUmin)    && !isnothing(Δumin)    && (ΔUmin   = repeat(Δumin,   Hc))
-    isnothing(ΔUmax)    && !isnothing(Δumax)    && (ΔUmax   = repeat(Δumax,   Hc))
-    isnothing(Ymin)     && !isnothing(ymin)     && (Ymin    = repeat(ymin,    Hp))
-    isnothing(Ymax)     && !isnothing(ymax)     && (Ymax    = repeat(ymax,    Hp))
-    isnothing(C_umin)   && !isnothing(c_umin)   && (C_umin  = repeat(c_umin,  Hp))
-    isnothing(C_umax)   && !isnothing(c_umax)   && (C_umax  = repeat(c_umax,  Hp))
-    isnothing(C_Δumin)  && !isnothing(c_Δumin)  && (C_Δumin = repeat(c_Δumin, Hc))
-    isnothing(C_Δumax)  && !isnothing(c_Δumax)  && (C_Δumax = repeat(c_Δumax, Hc))
-    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)))
-        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)
-        size(Umin)   == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))"))
+    if isnothing(Umin) && !isnothing(umin)
+        size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))"))
+        for i = 1:nu*Hp
+            con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i]
+        end
+    elseif !isnothing(Umin)
+        size(Umin) == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))"))
         con.U0min .= Umin .- mpc.Uop
     end
-    if !isnothing(Umax)
+    if isnothing(Umax) && !isnothing(umax)
+        size(umax) == (nu,) || throw(ArgumentError("umax size must be $((nu,))"))
+        for i = 1:nu*Hp
+            con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i]
+        end
+    elseif !isnothing(Umax)
         size(Umax)   == (nu*Hp,) || throw(ArgumentError("Umax size must be $((nu*Hp,))"))
         con.U0max .= Umax .- mpc.Uop
     end
-    if !isnothing(ΔUmin)
+    if isnothing(ΔUmin) && !isnothing(Δumin)
+        size(Δumin) == (nu,) || throw(ArgumentError("Δumin size must be $((nu,))"))
+        for i = 1:nu*Hc
+            con.ΔŨmin[i] = Δumin[(i-1) % nu + 1]
+        end
+    elseif !isnothing(ΔUmin)
         size(ΔUmin)  == (nu*Hc,) || throw(ArgumentError("ΔUmin size must be $((nu*Hc,))"))
         con.ΔŨmin[1:nu*Hc] .= ΔUmin
     end
-    if !isnothing(ΔUmax)
+    if isnothing(ΔUmax) && !isnothing(Δumax)
+        size(Δumax) == (nu,) || throw(ArgumentError("Δumax size must be $((nu,))"))
+        for i = 1:nu*Hc
+            con.ΔŨmax[i] = Δumax[(i-1) % nu + 1]
+        end
+    elseif !isnothing(ΔUmax)
         size(ΔUmax)  == (nu*Hc,) || throw(ArgumentError("ΔUmax size must be $((nu*Hc,))"))
         con.ΔŨmax[1:nu*Hc] .= ΔUmax
     end
-    if !isnothing(Ymin)
+    if isnothing(Ymin) && !isnothing(ymin)
+        size(ymin) == (ny,) || throw(ArgumentError("ymin size must be $((ny,))"))
+        for i = 1:ny*Hp
+            con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i]
+        end
+    elseif !isnothing(Ymin)
         size(Ymin)   == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))"))
         con.Y0min .= Ymin .- mpc.Yop
     end
-    if !isnothing(Ymax)
+    if isnothing(Ymax) && !isnothing(ymax)
+        size(ymax) == (ny,) || throw(ArgumentError("ymax size must be $((ny,))"))
+        for i = 1:ny*Hp
+            con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i]
+        end
+    elseif !isnothing(Ymax)
         size(Ymax)   == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))"))
         con.Y0max .= Ymax .- mpc.Yop
     end
     if !isnothing(x̂min)
-        size(x̂min)   == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))"))
+        size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))"))
         con.x̂0min .= x̂min .- mpc.estim.x̂op
     end
     if !isnothing(x̂max)
-        size(x̂max)   == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))"))
+        size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))"))
         con.x̂0max .= x̂max .- mpc.estim.x̂op
     end
-    if !isnothing(C_umin)
-        size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))"))
-        any(C_umin .< 0) && error("C_umin weights should be non-negative")
-        con.A_Umin[:, end] .= -C_umin
-    end
-    if !isnothing(C_umax)
-        size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))"))
-        any(C_umax .< 0) && error("C_umax weights should be non-negative")
-        con.A_Umax[:, end] .= -C_umax
-    end
-    if !isnothing(C_Δumin)
-        size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))"))
-        any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative")
-        con.A_ΔŨmin[1:end-1, end] .= -C_Δumin 
-    end
-    if !isnothing(C_Δumax)
-        size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))"))
-        any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative")
-        con.A_ΔŨmax[1:end-1, end] .= -C_Δumax
-    end
-    if !isnothing(C_ymin)
-        size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))"))
-        any(C_ymin .< 0) && error("C_ymin weights should be non-negative")
-        con.C_ymin .= C_ymin
-        size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel
-    end
-    if !isnothing(C_ymax)
-        size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))"))
-        any(C_ymax .< 0) && error("C_ymax weights should be non-negative")
-        con.C_ymax .= C_ymax
-        size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel
-    end
-    if !isnothing(c_x̂min)
-        size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))"))
-        any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative")
-        con.c_x̂min .= c_x̂min
-        size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel
+    allECRs = (
+        c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax,
+        C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max,
+    )
+    if any(ECR -> !isnothing(ECR), allECRs)
+        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(c_x̂max)
-        size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))"))
-        any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative")
-        con.c_x̂max .= c_x̂max
-        size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
+    if notSolvedYet
+        isnothing(C_umin)   && !isnothing(c_umin)   && (C_umin  = repeat(c_umin,  Hp))
+        isnothing(C_umax)   && !isnothing(c_umax)   && (C_umax  = repeat(c_umax,  Hp))
+        isnothing(C_Δumin)  && !isnothing(c_Δumin)  && (C_Δumin = repeat(c_Δumin, Hc))
+        isnothing(C_Δumax)  && !isnothing(c_Δumax)  && (C_Δumax = repeat(c_Δumax, Hc))
+        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 !isnothing(C_umin)
+            size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))"))
+            any(C_umin .< 0) && error("C_umin weights should be non-negative")
+            con.A_Umin[:, end] .= -C_umin
+        end
+        if !isnothing(C_umax)
+            size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))"))
+            any(C_umax .< 0) && error("C_umax weights should be non-negative")
+            con.A_Umax[:, end] .= -C_umax
+        end
+        if !isnothing(C_Δumin)
+            size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))"))
+            any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative")
+            con.A_ΔŨmin[1:end-1, end] .= -C_Δumin 
+        end
+        if !isnothing(C_Δumax)
+            size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))"))
+            any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative")
+            con.A_ΔŨmax[1:end-1, end] .= -C_Δumax
+        end
+        if !isnothing(C_ymin)
+            size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))"))
+            any(C_ymin .< 0) && error("C_ymin weights should be non-negative")
+            con.C_ymin .= C_ymin
+            size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel
+        end
+        if !isnothing(C_ymax)
+            size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))"))
+            any(C_ymax .< 0) && error("C_ymax weights should be non-negative")
+            con.C_ymax .= C_ymax
+            size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel
+        end
+        if !isnothing(c_x̂min)
+            size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))"))
+            any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative")
+            con.c_x̂min .= c_x̂min
+            size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel
+        end
+        if !isnothing(c_x̂max)
+            size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))"))
+            any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative")
+            con.c_x̂max .= c_x̂max
+            size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
+        end
     end
     i_Umin,  i_Umax  = .!isinf.(con.U0min), .!isinf.(con.U0max)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax)

src/controller/execute.jl

@@ -543,41 +543,79 @@ prediction horizon ``H_p``.
     Keyword arguments with *`emphasis`* are non-Unicode alternatives.
 
 - `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` or *`Ntilde_Hc`* : new ``\mathbf{Ñ}_{H_c}`` weight matrix (see definition
-   above).
-- `L_Hp=mpc.L_Hp` : new ``\mathbf{L}_{H_p}`` weight matrix.
+- `model=mpc.estim.model` : new plant model (not supported by [`NonLinModel`](@ref)).
+- `Mwt=nothing` : new main diagonal in ``\mathbf{M}`` weight matrix (vector).
+- `Nwt=nothing` : new main diagonal in ``\mathbf{N}`` weight matrix (vector).
+- `Lwt=nothing` : new main diagonal in ``\mathbf{L}`` weight matrix (vector).
+- `M_Hp=nothing` : new ``\mathbf{M}_{H_p}`` weight matrix.
+- `Ñ_Hc=nothing` or *`Ntilde_Hc`* : new ``\mathbf{Ñ}_{H_c}`` weight matrix (see def. above).
+- `L_Hp=nothing` : new ``\mathbf{L}_{H_p}`` weight matrix.
 - additional keyword arguments are passed to `setmodel!(mpc.estim)`.
 
 # Examples
 ```jldoctest
 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.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
+(0.1, 25.0, 1.0, 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)); R̂=[9], M_Hp=[10], Nwt=[0.666]);
 
-julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
-(0.42, 9.0, 0.0)
+julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
+(0.42, 9.0, 10.0, 0.666)
 ```
 """
 function setmodel!(
         mpc::PredictiveController, 
         model = mpc.estim.model;
-        M_Hp      = mpc.M_Hp,
-        Ntilde_Hc = mpc.Ñ_Hc,
-        L_Hp      = mpc.L_Hp,
+        Mwt       = nothing,
+        Nwt       = nothing,
+        Lwt       = nothing,
+        M_Hp      = nothing,
+        Ntilde_Hc = nothing,
+        L_Hp      = nothing,
         Ñ_Hc      = Ntilde_Hc,
         kwargs...
     )
     x̂op_old = copy(mpc.estim.x̂op)
     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)
+    if isnothing(M_Hp) && !isnothing(Mwt)
+        size(Mwt) == (ny,) || throw(ArgumentError("Mwt should be a vector of length $ny"))
+        any(x -> x < 0, Mwt) && throw(ArgumentError("Mwt values should be nonnegative"))
+        for i=1:ny*Hp
+            mpc.M_Hp[i, i] = Mwt[(i-1) % ny + 1]
+        end
+    elseif !isnothing(M_Hp)
+        M_Hp = to_hermitian(M_Hp)
+        nŶ = ny*Hp
+        size(M_Hp) == (nŶ, nŶ) || throw(ArgumentError("M_Hp size should be ($nŶ, $nŶ)"))
+        mpc.M_Hp .= M_Hp
+    end
+    if isnothing(Ñ_Hc) && !isnothing(Nwt)
+        size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu"))
+        any(x -> x < 0, Nwt) && throw(ArgumentError("Nwt values should be nonnegative"))
+        for i=1:nu*Hc
+            mpc.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1]
+        end
+    elseif !isnothing(Ñ_Hc)
+        Ñ_Hc = to_hermitian(Ñ_Hc)
+        nΔŨ = nu*Hc+nϵ
+        size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)"))
+        mpc.Ñ_Hc .= Ñ_Hc
+    end
+    if isnothing(L_Hp) && !isnothing(Lwt)
+        size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu"))
+        any(x -> x < 0, Lwt) && throw(ArgumentError("Lwt values should be nonnegative"))
+        for i=1:nu*Hp
+            mpc.L_Hp[i, i] = Lwt[(i-1) % nu + 1]
+        end
+    elseif !isnothing(L_Hp)
+        L_Hp = to_hermitian(L_Hp)
+        nU = nu*Hp
+        size(L_Hp) == (nU, nU) || throw(ArgumentError("L_Hp size should be ($nU, $nU)"))
+        mpc.L_Hp .= L_Hp
+    end
     setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
     return mpc
 end

src/estimator/execute.jl

@@ -254,11 +254,13 @@ time must stay the same. Note that the observability and controllability of the
 augmented model is not verified (see Extended Help for more info).
 
 # Arguments
+!!! info
+    Keyword arguments with *`emphasis`* are non-Unicode alternatives.
 
 - `estim::StateEstimator` : estimator to set model and covariances.
-- `model=estim.model` : new plant model ([`NonLinModel`](@ref) not supported).
-- `Q̂=nothing` : new augmented model ``\mathbf{Q̂}`` covariance matrix.
-- `R̂=nothing` : new augmented model ``\mathbf{R̂}`` covariance matrix.
+- `model=estim.model` : new plant model (not supported by [`NonLinModel`](@ref)).
+- `Q̂=nothing` or *`Qhat`* : new augmented model ``\mathbf{Q̂}`` covariance matrix.
+- `R̂=nothing` or *`Rhat`* : new augmented model ``\mathbf{R̂}`` covariance matrix.
 
 # Examples
 ```jldoctest
@@ -283,8 +285,10 @@ julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2]
 function setmodel!(
         estim::StateEstimator, 
         model = estim.model;
-        Q̂ = nothing,
-        R̂ = nothing
+        Qhat = nothing,
+        Rhat = nothing,
+        Q̂ = Qhat,
+        R̂ = Rhat
     )
     uop_old = copy(estim.model.uop)
     yop_old = copy(estim.model.yop)