From c1fa2dea7ea593f4543aeff67117179d97a74473 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Sep 2025 15:08:09 -0400 Subject: [PATCH 1/3] debug: `setmodel!` now works with `MultipleShooting` --- src/controller/execute.jl | 55 +++++++++++++++++++++++---------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a5ea51563..2e4bd283c 100644 --- a/src/controller/execute.jl +++ b/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,6 +623,15 @@ 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̂ @@ -630,13 +639,30 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) 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,20 +675,10 @@ 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] @@ -670,14 +686,9 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) 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 From a5cde6cbae88842bf550977fb32e95ca0cc89002 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Sep 2025 15:40:18 -0400 Subject: [PATCH 2/3] test: new test covering `setmodel!` with `MultipleShooting` --- test/3_test_predictive_control.jl | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 2991faf35..53a96b78b 100644 --- a/test/3_test_predictive_control.jl +++ b/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 From 07a9967fbdc415bdefff5ea7f8660469cc70ae55 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Sep 2025 15:40:57 -0400 Subject: [PATCH 3/3] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 9c18f8c7e..18d4df711 100644 --- a/Project.toml +++ b/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"