diff --git a/Project.toml b/Project.toml index 7cdcf1203..e3ee77f3a 100644 --- a/Project.toml +++ b/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" diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index a086dc787..38b7f01e8 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/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 ``` diff --git a/src/controller/execute.jl b/src/controller/execute.jl index f6f7beffa..7a7e08674 100644 --- a/src/controller/execute.jl +++ b/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 diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index c1080ae60..3532989be 100644 --- a/src/controller/nonlinmpc.jl +++ b/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 diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 4322131b4..6eb030784 100644 --- a/src/estimator/kalman.jl +++ b/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""" diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 1215caabf..714e365e5 100644 --- a/src/estimator/luenberger.jl +++ b/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!") \ No newline at end of file +"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 \ No newline at end of file diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 29a5ec002..bc0c4ef38 100644 --- a/src/estimator/mhe/execute.jl +++ b/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 diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 20e58acc5..ee0ae637a 100644 --- a/test/test_predictive_control.jl +++ b/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 diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index cdf4fce01..9eec37db7 100644 --- a/test/test_state_estim.jl +++ b/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