debug : input setpoint ru for NonLinModel now works

added : unit test for input setpoint ru for NonLinMPC

Author: franckgaga

example/test.jl

@@ -0,0 +1,13 @@
+using JuMP, Ipopt
+
+optim = Model(Ipopt.Optimizer)
+
+nvar = 1 # nvar = 1 : error, nvar = 2 : works
+function Jfunc(ΔŨtup...)
+    ΔŨ = collect(ΔŨtup)
+    return sum(ΔŨ.^2)
+end
+
+@variable(optim, ΔŨvar[1:nvar])
+register(optim, :Jfunc, nvar, Jfunc, autodiff=true)
+@NLobjective(optim, Min, Jfunc(ΔŨvar...))

src/controller/nonlinmpc.jl

@@ -234,8 +234,8 @@ function init_optimization!(mpc::NonLinMPC)
     # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-functions-with-vector-outputs
     Jfunc, Cfunc = let mpc=mpc, model=model, nC=nC, nvar=nvar , nŶ=Hp*ny
         last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing
-        Ŷ_cache::DiffCacheType = DiffCache(zeros(nŶ), nvar)
-        C_cache::DiffCacheType = DiffCache(zeros(nC), nvar)
+        Ŷ_cache::DiffCacheType = DiffCache(zeros(nŶ), nvar + 3)
+        C_cache::DiffCacheType = DiffCache(zeros(nC), nvar + 3)
         function Jfunc(ΔŨtup::Float64...)
             Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
             ΔŨ = collect(ΔŨtup)
@@ -356,7 +356,7 @@ function obj_nonlinprog(mpc::NonLinMPC, model::SimModel, Ŷ, ΔŨ::Vector{T})
     # --- input setpoint tracking term ---
     if !isempty(mpc.R̂u)
         êu = mpc.R̂u - U
-        JR̂u = êu'*mpc.L_Hp*ê
+        JR̂u = êu'*mpc.L_Hp*êu
     else
         JR̂u = 0.0
     end

src/precompile_calls.jl

@@ -7,20 +7,25 @@ y = model()
 mpc_im = setconstraint!(LinMPC(InternalModel(model)), ŷmin=[45, -Inf])
 initstate!(mpc_im, model.uop, y)
 u = mpc_im([50, 30], ym=y)
-updatestate!(mpc_im, u, y)
+
 mpc_kf = setconstraint!(LinMPC(KalmanFilter(model)), ŷmin=[45, -Inf])
 initstate!(mpc_kf, model.uop, model())
 u = mpc_kf([50, 30])
-updatestate!(mpc_kf, u, y)
+sim!(mpc_kf, 15)
+mpc_lo = setconstraint!(LinMPC(Luenberger(model)), ŷmin=[45, -Inf])
+initstate!(mpc_lo, model.uop, model())
+u = mpc_lo([50, 30])
+sim!(mpc_lo, 15)
 mpc_ukf = setconstraint!(LinMPC(UnscentedKalmanFilter(model)), ŷmin=[45, -Inf])
 initstate!(mpc_ukf, model.uop, model())
 u = mpc_ukf([50, 30])
-updatestate!(mpc_ukf, u, y)
+sim!(mpc_ukf, 15)
 mpc_skf = setconstraint!(LinMPC(SteadyKalmanFilter(model)), ŷmin=[45, -Inf])
 initstate!(mpc_skf, model.uop, model())
 u = mpc_skf([50, 30])
-updatestate!(mpc_skf, u, y)
-updatestate!(model, u) 
+sim!(mpc_skf, 15)
+
+sim!(model, 15)
 
 f(x,u,_) = model.A*x + model.Bu*u
 h(x,_) = model.C*x
@@ -30,8 +35,8 @@ y = nlmodel()
 nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel)), ŷmin=[45, -Inf])
 initstate!(nmpc_im, nlmodel.uop, y)
 u = nmpc_im([50, 30], ym=y)
-updatestate!(nmpc_im, u, y)
+sim!(nmpc_im, 15)
 nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel)), ŷmin=[45, -Inf])
 initstate!(nmpc_ukf, nlmodel.uop, y)
 u = nmpc_ukf([50, 30])
-updatestate!(nmpc_ukf, u, y)
+sim!(nmpc_ukf, 15)

test/test_predictive_control.jl

@@ -87,7 +87,7 @@ end
     nmpc0 = NonLinMPC(linmodel1, Hp=15)
     @test isa(nmpc0.estim, SteadyKalmanFilter)
     f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h(x,d)   = linmodel1.C*x + linmodel1.Du*d
+    h(x,d)   = linmodel1.C*x + linmodel1.Dd*d
     nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
     nmpc1 = NonLinMPC(nonlinmodel, Hp=15)
     @test isa(nmpc1.estim, UnscentedKalmanFilter)
@@ -117,7 +117,7 @@ end
 @testset "NonLinMPC constraints" begin
     linmodel1 = LinModel(sys,Ts,i_d=[3])
     f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h(x,d)   = linmodel1.C*x + linmodel1.Du*d
+    h(x,d)   = linmodel1.C*x + linmodel1.Dd*d
     nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
     nmpc = NonLinMPC(nonlinmodel, Hp=1, Hc=1)
     setconstraint!(nmpc, umin=[5, 9.9], umax=[100,99])
@@ -146,19 +146,24 @@ end
     info, _ = getinfo(nmpc_lin)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 5 atol=1e-2
-
     f(x,u,_) = linmodel.A*x + linmodel.Bu*u
     h(x,_)   = linmodel.C*x 
     nonlinmodel = NonLinModel(f, h, 3, 1, 1, 1)
-    nmpc_nonlin = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
+    nmpc1 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
     r = [5]
-    u = moveinput!(nmpc_nonlin, r)
+    u = moveinput!(nmpc1, r)
     @test u ≈ [1] atol=1e-2
-    u = nmpc_nonlin(r)
+    u = nmpc1(r)
     @test u ≈ [1] atol=1e-2
-    info, _ = getinfo(nmpc_nonlin)
+    info, _ = getinfo(nmpc1)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 5 atol=1e-2
+    nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
+    u = moveinput!(nmpc2, [5])
+    @test u ≈ [1] atol=1e-2
+    nmpc3 = NonLinMPC(nonlinmodel, Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
+    u = moveinput!(nmpc3, [0])
+    @test u ≈ [12] atol=1e-2
 end
 
 @testset "NonLinMPC other methods" begin
@@ -170,4 +175,4 @@ end
     @test initstate!(nmpc1, [10, 50], [20, 25]) ≈ [zeros(2); [20, 25]]
     setstate!(nmpc1, [1,2,3,4])
     @test nmpc1.estim.x̂ ≈ [1,2,3,4]
-end
+end