Merge pull request #246 from JuliaControl/debug_ms_setmodel

debug: `setmodel!` now works with `LinMPC` based on `MultipleShooting`

Author: franckgaga

Project.toml

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

src/controller/execute.jl

@@ -611,7 +611,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
-    # --- predictions matrices ---
+    # --- prediction matrices ---
     E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
         model, estim, transcription, Hp, Hc
     )
@@ -623,20 +623,46 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     mpc.K .= K
     mpc.V .= V
     mpc.B .= B
+    # --- defect matrices ---
+    Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)
+    A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ)
+    con.Ẽŝ .= Ẽŝ
+    con.Gŝ .= Gŝ
+    con.Jŝ .= Jŝ
+    con.Kŝ .= Kŝ
+    con.Vŝ .= Vŝ
+    con.Bŝ .= Bŝ
     # --- linear inequality constraints ---
     con.ẽx̂ .= ẽx̂ 
     con.gx̂ .= gx̂
     con.jx̂ .= jx̂
     con.kx̂ .= kx̂
     con.vx̂ .= vx̂
     con.bx̂ .= bx̂
+    con.A_Ymin .= A_Ymin
+    con.A_Ymax .= A_Ymax
+    con.A_x̂min .= A_x̂min
+    con.A_x̂max .= A_x̂max
+    con.A .= [
+        con.A_Umin
+        con.A_Umax 
+        con.A_ΔŨmin 
+        con.A_ΔŨmax 
+        con.A_Ymin  
+        con.A_Ymax 
+        con.A_x̂min  
+        con.A_x̂max
+    ]
+    # --- linear equality constraints ---
+    con.A_ŝ .= A_ŝ
+    con.Aeq .= A_ŝ
+    # --- operating points ---
     con.U0min .+= mpc.Uop # convert U0 to U with the old operating point
     con.U0max .+= mpc.Uop # convert U0 to U with the old operating point
     con.Y0min .+= mpc.Yop # convert Y0 to Y with the old operating point
     con.Y0max .+= mpc.Yop # convert Y0 to Y with the old operating point
     con.x̂0min .+= x̂op_old # convert x̂0 to x̂ with the old operating point
     con.x̂0max .+= x̂op_old # convert x̂0 to x̂ with the old operating point
-    # --- operating points ---
     mpc.lastu0 .+= uop_old .- model.uop
     for i in 0:Hp-1
         mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop
@@ -649,35 +675,20 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     con.Y0max .-= mpc.Yop # convert Y to Y0 with the new operating point
     con.x̂0min .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
     con.x̂0max .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
-    con.A_Ymin .= A_Ymin
-    con.A_Ymax .= A_Ymax
-    con.A_x̂min .= A_x̂min
-    con.A_x̂max .= A_x̂max
-    con.A .= [
-        con.A_Umin
-        con.A_Umax 
-        con.A_ΔŨmin 
-        con.A_ΔŨmax 
-        con.A_Ymin  
-        con.A_Ymax 
-        con.A_x̂min  
-        con.A_x̂max
-    ]
+    # --- quadratic programming Hessian matrix ---
+    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u)
+    mpc.H̃ .= H̃
+    # --- JuMP optimization ---
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
     # deletion is required for sparse solvers like OSQP, when the sparsity pattern changes
     JuMP.delete(optim, optim[:linconstraint])
     JuMP.unregister(optim, :linconstraint)
     @constraint(optim, linconstraint, A*Z̃var .≤ b)
-    Aeq = con.Aeq
-    beq = con.beq
     JuMP.delete(optim, optim[:linconstrainteq])
     JuMP.unregister(optim, :linconstrainteq)
-    @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq)
-    # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u)
-    mpc.H̃ .= H̃
+    @constraint(optim, linconstrainteq, con.Aeq*Z̃var .== con.beq)
     set_objective_hessian!(mpc, Z̃var)
     return nothing
 end

test/3_test_predictive_control.jl

@@ -436,6 +436,17 @@ end
     @test mpc.weights.M_Hp ≈ diagm(1:1000)
     @test mpc.weights.Ñ_Hc ≈ diagm([0.1;1e6])
     @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1)
+
+    estim2 = KalmanFilter(LinModel(tf(5, [2, 1]), 3))
+    mpc_ms = LinMPC(estim2, Nwt=[0], Hp=1000, Hc=1, transcription=MultipleShooting())
+    r = [15]
+    preparestate!(mpc_ms, [0])
+    u = moveinput!(mpc_ms, r)
+    @test u ≈ [3] atol=1e-2
+    setmodel!(mpc_ms, LinModel(tf(10, [2, 1]), 3))
+    r = [40]
+    u = moveinput!(mpc_ms, r)
+    @test u ≈ [4] atol=1e-2
 end
 
 @testitem "LinMPC real-time simulations" setup=[SetupMPCtests] begin