Merge pull request #136 from JuliaControl/getinfo_gc

added: `gc` in `getinfo`

Author: franckgaga

Project.toml

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

docs/src/manual/nonlinmpc.md

@@ -265,7 +265,7 @@ argument ("left-hand side" of the inequality above) for the in-place version:
 
 ```@example man_nonlin
 function gc!(LHS, Ue, Ŷe, _, p, ϵ)
-    Pmax, Hp = p
+    Pmax = p
     i_τ, i_ω = 1, 2
     for i in eachindex(LHS)
         τ, ω = Ue[i_τ], Ŷe[i_ω]
@@ -287,8 +287,7 @@ specifying the number of custom inequality constraints `nc`:
 
 ```@example man_nonlin
 Cwt, Pmax, nc = 1e5, 3, Hp+1
-p_nmpc2 = [Pmax, Hp]
-nmpc2 = NonLinMPC(estim2; Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=p_nmpc2)
+nmpc2 = NonLinMPC(estim2; Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=Pmax)
 using JuMP; unset_time_limit_sec(nmpc2.optim) # hide
 nmpc2 # hide
 ```

src/controller/execute.jl

@@ -96,8 +96,8 @@ The function should be called after calling [`moveinput!`](@ref). It returns the
 - `:d`   : current measured disturbance, ``\mathbf{d}(k)``
 
 For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer
-solution summary that can be printed. Lastly, the optimal economic cost `:JE` is also
-available for [`NonLinMPC`](@ref).
+solution summary that can be printed. Lastly, the economical cost `:JE` and the custom
+nonlinear constraints `:gc` values at the optimum are also available for [`NonLinMPC`](@ref).
 
 # Examples
 ```jldoctest
@@ -131,14 +131,14 @@ 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[:ϵ]    = mpc.nϵ == 1 ? mpc.ΔŨ[end] : NaN
+    info[:ϵ]    = mpc.nϵ == 1 ? mpc.ΔŨ[end] : zero(NT)
     info[:J]    = J
     info[:U]    = U
     info[:u]    = info[:U][1:model.nu]
     info[:d]    = mpc.d0 + model.dop
     info[:D̂]    = mpc.D̂0 + mpc.Dop
     info[:ŷ]    = mpc.ŷ
-    info[:Ŷ]    = Ŷ0 + mpc.Yop
+    info[:Ŷ]    = Ŷ
     info[:x̂end] = x̂0end + mpc.estim.x̂op
     info[:Ŷs]   = Ŷs
     info[:R̂y]   = mpc.R̂y

src/controller/nonlinmpc.jl

@@ -423,7 +423,7 @@ function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop,
             exception=(err, catch_backtrace())
         )
     end
-    ϵ, gc = 0, Vector{NT}(undef, nc) 
+    ϵ, gc = zero(NT), Vector{NT}(undef, nc) 
     try
         gc!(gc, Ue, Ŷe, D̂e, p, ϵ)
     catch err
@@ -444,12 +444,16 @@ end
 
 For [`NonLinMPC`](@ref), add `:sol` and the optimal economic cost `:JE`.
 """
-function addinfo!(info, mpc::NonLinMPC)
-    U, Ŷ, D̂, ŷ, d = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d]
+function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
+    U, Ŷ, D̂, ŷ, d, ϵ = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d], info[:ϵ]
     Ue = [U; U[(end - mpc.estim.model.nu + 1):end]]
     Ŷe = [ŷ; Ŷ]
-    D̂e = [d; D̂]
-    info[:JE]  = mpc.JE(Ue, Ŷe, D̂e, mpc.p)
+    D̂e = [d; D̂] 
+    JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p)
+    LHS = Vector{NT}(undef, mpc.con.nc)
+    mpc.con.gc!(LHS, Ue, Ŷe, D̂e, mpc.p, ϵ)
+    info[:JE]  = JE 
+    info[:gc] = LHS
     info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
     return info
 end

src/estimator/kalman.jl

@@ -199,9 +199,12 @@ function SteadyKalmanFilter(
     return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
 end
 
-"Throw an error if `setmodel!` is called on a SteadyKalmanFilter"
-function setmodel_estimator!(::SteadyKalmanFilter, args...)
-    error("SteadyKalmanFilter does not support setmodel! (use KalmanFilter instead)")
+"Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values."
+function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, R̂)
+    if estim.model !== model || !isnothing(Q̂) || !isnothing(R̂)
+        error("SteadyKalmanFilter does not support setmodel! (use KalmanFilter instead)")
+    end
+    return nothing
 end
 
 @doc raw"""

src/estimator/luenberger.jl

@@ -135,5 +135,10 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0)
     return predict_estimate_obsv!(estim, y0m, d0, u0)
 end
 
-"Throw an error if `setmodel!` is called on `Luenberger` observer."
-setmodel_estimator!(::Luenberger, args...) = error("Luenberger does not support setmodel!")
+"Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values."
+function setmodel!(estim::Luenberger, model, args...)
+    if estim.model !== model
+        error("Luenberger does not support setmodel!")
+    end
+    return nothing
+end

src/estimator/mhe/execute.jl

@@ -134,7 +134,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     end
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
     info[:x̂arr] = x̂0arr + estim.x̂op
-    info[:ϵ]  = nϵ ≠ 0 ? estim.Z̃[begin] : NaN
+    info[:ϵ]  = nϵ ≠ 0 ? estim.Z̃[begin] : zero(NT)
     info[:J]  = obj_nonlinprog!(x̄, estim, estim.model, V̂, estim.Z̃)
     info[:X̂]  = X̂0       .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]]
     info[:x̂]  = estim.x̂0 .+ estim.x̂op

test/test_predictive_control.jl

@@ -566,32 +566,42 @@ end
 
 @testset "NonLinMPC moves and getinfo" begin
     linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10])
-    nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
-    r = [15]
+    Hp = 1000
+    nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=Hp, Hc=1)
+    ry, ru = [15], [4]
     preparestate!(nmpc_lin, [10])
-    u = moveinput!(nmpc_lin, r)
+    u = moveinput!(nmpc_lin, ry)
     @test u ≈ [1] atol=5e-2
-    u = nmpc_lin(r)
+    u = nmpc_lin(ry)
     @test u ≈ [1] atol=5e-2
     info = getinfo(nmpc_lin)
     @test info[:u] ≈ u
-    @test info[:Ŷ][end] ≈ r[1] atol=5e-2
-    Hp = 1000
-    R̂y = fill(r[1], Hp)
-    JE = (_ , Ŷe, _ , R̂y) -> sum((Ŷe[2:end] - R̂y).^2)
-    nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=R̂y, Hp=Hp, Hc=1)
+    @test info[:Ŷ][end] ≈ ry[1] atol=5e-2
+    setmodel!(nmpc_lin; Mwt=[0], Lwt=[1])
+    u = moveinput!(nmpc_lin; R̂u=fill(ru[1], Hp))
+    @test u ≈ [4] atol=5e-2
+    function JE(Ue, Ŷe, _ , p)
+        Wy, R̂y, Wu, R̂u = p
+        return Wy*sum((R̂y-Ŷe[2:end]).^2) + Wu*sum((R̂u-Ue[1:end-1]).^2)
+    end
+    R̂y, R̂u = fill(ry[1], Hp), fill(ru[1], Hp)
+    p = [1, R̂y, 0, R̂u]
+    nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=p, Hp=Hp, Hc=1)
     preparestate!(nmpc, [10])
     u = moveinput!(nmpc)
     @test u ≈ [1] atol=5e-2
     # ensure that the current estimated output is updated for correct JE values:
     @test nmpc.ŷ ≈ evaloutput(nmpc.estim, Float64[])
+    nmpc.p .= [0, R̂y, 1, R̂u]
+    u = moveinput!(nmpc)
+    @test u ≈ [4] atol=5e-2
     linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2])
     f = (x,u,d,_) -> linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
     h = (x,d,_)   -> linmodel2.C*x + linmodel2.Dd*d
     nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing)
     nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
     d = [0.1]
-    preparestate!(nmpc2, [0], [0])
+    preparestate!(nmpc2, [0], d)
     u = moveinput!(nmpc2, 7d, d)
     @test u ≈ [0] atol=5e-2
     u = nmpc2(7d, d)
@@ -613,7 +623,7 @@ end
     @test g_Y0min_end(20.0, 10.0) ≤ 0.0 
     # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : 
     @test ForwardDiff.gradient(vec->g_Y0min_end(vec...), [20.0, 10.0]) ≈ [-5, -5] atol=1e-3
-    linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
+    linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0)
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
     preparestate!(nmpc6, [0])
     @test moveinput!(nmpc6, [0]) ≈ [0.0]
@@ -720,10 +730,11 @@ end
 end
 
 @testset "NonLinMPC constraint violation" begin
-    gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue .- 4.2 .- ϵ); p[2]*(Ŷe .- 3.14 .- ϵ)]
+    gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue[1:end-1] .- 4.2 .- ϵ); p[2]*(Ŷe[2:end] .- 3.14 .- ϵ)]
+    Hp=50
 
     linmodel = LinModel(tf([2], [10000, 1]), 3000.0)
-    nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0])
+    nmpc_lin = NonLinMPC(linmodel, Hp=Hp, Hc=5, gc=gc, nc=2Hp, p=[0; 0])
 
     setconstraint!(nmpc_lin, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf])
     setconstraint!(nmpc_lin, umin=[-10], umax=[10])
@@ -758,7 +769,7 @@ end
     @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1))
     setconstraint!(nmpc_lin, ymin=[-100], ymax=[100])
 
-    setconstraint!(nmpc_lin, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.9; fill(+100, 49)])
+    setconstraint!(nmpc_lin, Ymin=[-0.5; fill(-100, Hp-1)], Ymax=[0.9; fill(+100, Hp-1)])
     moveinput!(nmpc_lin, [-10])
     info = getinfo(nmpc_lin)
     @test info[:Ŷ][end]   ≈ -10  atol=1e-1
@@ -782,17 +793,18 @@ end
     moveinput!(nmpc_lin, [100])
     info = getinfo(nmpc_lin)
     @test all(isapprox.(info[:U], 4.2; atol=1e-1))
+    @test all(isapprox.(info[:gc][1:Hp], 0.0; atol=1e-1))
 
     nmpc_lin.p .= [0; 1]
     moveinput!(nmpc_lin, [100])
     info = getinfo(nmpc_lin)
     @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1))
-
+    @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1))
 
     f = (x,u,_,p) -> p.A*x + p.Bu*u
     h = (x,_,p)   -> p.C*x
     nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing, p=linmodel)
-    nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0])
+    nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=2Hp, p=[0; 0])
 
     setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf])
     setconstraint!(nmpc, umin=[-10], umax=[10])
@@ -827,7 +839,7 @@ end
     @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1))
     setconstraint!(nmpc, ymin=[-100], ymax=[100])
 
-    setconstraint!(nmpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.9; fill(+100, 49)])
+    setconstraint!(nmpc, Ymin=[-0.5; fill(-100, Hp-1)], Ymax=[0.9; fill(+100, Hp-1)])
     moveinput!(nmpc, [-10])
     info = getinfo(nmpc)
     @test info[:Ŷ][end]   ≈ -10  atol=1e-1
@@ -851,11 +863,13 @@ end
     moveinput!(nmpc, [100])
     info = getinfo(nmpc)
     @test all(isapprox.(info[:U], 4.2; atol=1e-1))
+    @test all(isapprox.(info[:gc][1:Hp], 0.0; atol=1e-1))
 
     nmpc.p .= [0; 1]
     moveinput!(nmpc, [100])
     info = getinfo(nmpc)
     @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1))
+    @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1))
 
 end
 

test/test_state_estim.jl

@@ -119,7 +119,10 @@ end
     linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0))
     linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0])
     skalmanfilter = SteadyKalmanFilter(linmodel, nint_ym=0)
-    @test_throws ErrorException setmodel!(skalmanfilter, linmodel)
+    @test_nowarn setmodel!(skalmanfilter, linmodel)
+    @test_throws ErrorException setmodel!(skalmanfilter, deepcopy(linmodel))
+    @test_throws ErrorException setmodel!(skalmanfilter, linmodel, Q̂=[0.01])
+    @test_throws ErrorException setmodel!(skalmanfilter, linmodel, R̂=[0.01])
 end
 
 @testset "SteadyKalmanFilter real-time simulations" begin
@@ -349,7 +352,8 @@ end
     linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0))
     linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0])
     lo = Luenberger(linmodel, nint_ym=0)
-    @test_throws ErrorException setmodel!(lo, linmodel)
+    @test_nowarn setmodel!(lo, linmodel)
+    @test_throws ErrorException setmodel!(lo, deepcopy(linmodel))
 end
 
 @testset "InternalModel construction" begin