added: Mwt, Nwt and Lwt kwargs in setmodel!

Author: franckgaga

src/controller/construct.jl

@@ -268,7 +268,6 @@ function setconstraint!(
             size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
         end
     end
-    # TODO: test setmodel! new arguments
     i_Umin,  i_Umax  = .!isinf.(con.U0min), .!isinf.(con.U0max)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(con.Y0min), .!isinf.(con.Y0max)

src/controller/execute.jl

@@ -543,11 +543,13 @@ 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
@@ -566,18 +568,48 @@ julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
 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)
+        size(M_Hp) == (ny*Hp, ny*Hp) || throw(ArgumentError("M_Hp size should be (ny*Hp, ny*Hp)"))
+        mpc.M_Hp .= to_hermitian(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)
+        size(Ñ_Hc) == (nu*Hc+nϵ, nu*Hc+nϵ) || throw(ArgumentError("Ñ_Hc size should be (nu*Hc+nϵ, nu*Hc+nϵ)"))
+        mpc.Ñ_Hc .= to_hermitian(Ñ_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)
+        size(L_Hp) == (nu*Hp, nu*Hp) || throw(ArgumentError("L_Hp size should be (nu*Hp, nu*Hp)"))
+        mpc.L_Hp .= to_hermitian(L_Hp)
+    end
     setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
     return mpc
 end

test/test_predictive_control.jl

@@ -260,7 +260,7 @@ end
 
 @testset "LinMPC set model" begin
     estim = KalmanFilter(setop!(LinModel(tf(5, [2, 1]), 3), yop=[10], uop=[1]))
-    mpc = LinMPC(estim, Nwt=[0], Hp=1000, Hc=1)
+    mpc = LinMPC(estim, Nwt=[0], Cwt=1e4, Hp=1000, Hc=1)
     mpc = setconstraint!(mpc, umin=[-24], umax=[26])
     mpc = setconstraint!(mpc, ymin=[-54], ymax=[56])
     @test mpc.Yop ≈ fill(10.0, 1000)
@@ -286,6 +286,10 @@ end
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [13] atol=1e-2
+    setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300])
+    @test mpc.M_Hp ≈ diagm(fill(100, 1000))
+    @test mpc.Ñ_Hc ≈ diagm([200, 1e4])
+    @test mpc.L_Hp ≈ diagm(fill(300, 1000))
     setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([0.1;1e6]), L_Hp=diagm(1.1:1000.1))
     @test mpc.M_Hp ≈ diagm(1:1000)
     @test mpc.Ñ_Hc ≈ diagm([0.1;1e6])
@@ -417,6 +421,10 @@ end
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [13] atol=1e-2
+    setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300])
+    @test mpc.M_Hp ≈ diagm(fill(100, 1000))
+    @test mpc.Ñ_Hc ≈ diagm([200])
+    @test mpc.L_Hp ≈ diagm(fill(300, 1000))
     setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=[0.1], L_Hp=diagm(1.1:1000.1))
     @test mpc.M_Hp ≈ diagm(1:1000)
     @test mpc.Ñ_Hc ≈ [0.1]
@@ -694,7 +702,7 @@ end
 
 @testset "NonLinMPC set model" begin
     estim = KalmanFilter(setop!(LinModel(tf(5, [2, 1]), 3), yop=[10], uop=[1]))
-    mpc = NonLinMPC(estim, Nwt=[0], Hp=1000, Hc=1)
+    mpc = NonLinMPC(estim, Nwt=[0], Cwt=1e4, Hp=1000, Hc=1)
     mpc = setconstraint!(mpc, umin=[-24], umax=[26])
     mpc = setconstraint!(mpc, ymin=[-54], ymax=[56])
     @test mpc.Yop ≈ fill(10.0, 1000)
@@ -720,17 +728,25 @@ end
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [13] atol=1e-2
+    setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300])
+    @test mpc.M_Hp ≈ diagm(fill(100, 1000))
+    @test mpc.Ñ_Hc ≈ diagm([200, 1e4])
+    @test mpc.L_Hp ≈ diagm(fill(300, 1000))
     setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([0.1;1e6]), L_Hp=diagm(1.1:1000.1))
     @test mpc.M_Hp ≈ diagm(1:1000)
     @test mpc.Ñ_Hc ≈ diagm([0.1;1e6])
     @test mpc.L_Hp ≈ diagm(1.1:1000.1)
     f(x,u,d) = estim.model.A*x + estim.model.Bu*u + estim.model.Bd*d
     h(x,d)   = estim.model.C*x + estim.model.Du*d
     nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1)
-    nmpc = NonLinMPC(nonlinmodel, Hp=1000, Hc=1)
-    setmodel!(nmpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([0.1;1e6]), L_Hp=diagm(1.1:1000.1))
+    nmpc = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=1e4, Hp=1000, Hc=10)
+    setmodel!(nmpc, Mwt=[100], Nwt=[200], Lwt=[300])
+    @test nmpc.M_Hp ≈ diagm(fill(100, 1000))
+    @test nmpc.Ñ_Hc ≈ diagm([fill(200, 10); 1e4])
+    @test nmpc.L_Hp ≈ diagm(fill(300, 1000))
+    setmodel!(nmpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([fill(0.1, 10);1e6]), L_Hp=diagm(1.1:1000.1))
     @test nmpc.M_Hp ≈ diagm(1:1000)
-    @test nmpc.Ñ_Hc ≈ diagm([0.1;1e6])
+    @test nmpc.Ñ_Hc ≈ diagm([fill(0.1, 10);1e6])
     @test nmpc.L_Hp ≈ diagm(1.1:1000.1)
     @test_throws ErrorException setmodel!(nmpc, deepcopy(nonlinmodel))
 end