debug: correct lastu0 size in LinMPC

Author: franckgaga

src/controller/explicitmpc.jl

@@ -53,7 +53,7 @@ struct ExplicitMPC{
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
-        F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
+        F = zeros(NT, ny*Hp)
         P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
@@ -218,7 +218,7 @@ addinfo!(info, mpc::ExplicitMPC) = info
 
 
 "Update the prediction matrices and Cholesky factorization."
-function setmodel_controller!(mpc::ExplicitMPC, _ )
+function setmodel_controller!(mpc::ExplicitMPC, uop_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
     # --- predictions matrices ---
@@ -235,6 +235,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ )
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc)
     # --- operating points ---
+    mpc.lastu0 .+= uop_old .- model.uop
     for i in 0:Hp-1
         mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop
         mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop

src/controller/linmpc.jl

@@ -61,7 +61,7 @@ struct LinMPC{
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
-        lastu0 = zeros(NT, ny)
+        lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(

test/2_test_state_estim.jl

@@ -264,7 +264,6 @@ end
     @test kalmanfilter. ≈ [0.2]
     preparestate!(kalmanfilter, [55.0])
     @test evaloutput(kalmanfilter) ≈ [55.0]
-    @test kalmanfilter.lastu0 ≈ [2.0 - 3.0]
     preparestate!(kalmanfilter, [55.0])
     x̂ = updatestate!(kalmanfilter, [3.0], [55.0])
     @test x̂ ≈ [3.0]
@@ -507,7 +506,6 @@ end
     @test internalmodel. ≈ [0.2]
     preparestate!(internalmodel, [55.0])
     @test evaloutput(internalmodel) ≈ [55.0]
-    @test internalmodel.lastu0 ≈ [2.0 - 3.0]
     preparestate!(internalmodel, [55.0])
     x̂ = updatestate!(internalmodel, [3.0], [55.0])
     @test x̂ ≈ [3.0]
@@ -653,7 +651,6 @@ end
     @test ukf1. ≈ [0.2]
     preparestate!(ukf1, [55.0])
     @test evaloutput(ukf1) ≈ [55.0]
-    @test ukf1.lastu0 ≈ [2.0 - 3.0]
     preparestate!(ukf1, [55.0])
     x̂ = updatestate!(ukf1, [3.0], [55.0])
     @test x̂ ≈ [3.0]
@@ -818,7 +815,6 @@ end
     @test ekf1. ≈ [0.2]
     preparestate!(ekf1, [55.0])
     @test evaloutput(ekf1) ≈ [55.0]
-    @test ekf1.lastu0 ≈ [2.0 - 3.0]
     preparestate!(ekf1, [55.0])
     x̂ = updatestate!(ekf1, [3.0], [55.0])
     @test x̂ ≈ [3.0]

test/3_test_predictive_control.jl

@@ -361,13 +361,15 @@ end
     preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
+    @test mpc.lastu0 ≈ [2] - [1] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11]))
     @test mpc.Yop ≈ fill(20.0, 1000)
     @test mpc.Uop ≈ fill(11.0, 1000)
     @test mpc.con.U0min ≈ fill(-24.0 - 1  + 1  - 11,  1000)
     @test mpc.con.U0max ≈ fill(26.0  - 1  + 1  - 11,  1000)
     @test mpc.con.Y0min ≈ fill(-54.0 - 10 + 10 - 20, 1000)
     @test mpc.con.Y0max ≈ fill(56.0  - 10 + 10 - 20, 1000)
+    @test mpc.lastu0 ≈ [2] - [11] atol=1e-2
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [15] atol=1e-2
@@ -556,9 +558,11 @@ end
     preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
+    @test mpc.lastu0 ≈ [2] - [1] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11]))
     @test mpc.Yop ≈ fill(20.0, 1000)
     @test mpc.Uop ≈ fill(11.0, 1000)
+    @test mpc.lastu0 ≈ [2] - [11] atol=1e-2
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [15] atol=1e-2
@@ -1112,13 +1116,15 @@ end
     preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
+    @test mpc.lastu0 ≈ [2] - [1] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [200, 1]), 300), yop=[20], uop=[11]))
     @test mpc.Yop ≈ fill(20.0, 1000)
     @test mpc.Uop ≈ fill(11.0, 1000)
     @test mpc.con.U0min ≈ fill(-24.0 - 1  + 1  - 11,  1000)
     @test mpc.con.U0max ≈ fill(26.0  - 1  + 1  - 11,  1000)
     @test mpc.con.Y0min ≈ fill(-54.0 - 10 + 10 - 20, 1000)
     @test mpc.con.Y0max ≈ fill(56.0  - 10 + 10 - 20, 1000)
+    @test mpc.lastu0 ≈ [2] - [11] atol=1e-2
     r = [40]
     u = moveinput!(mpc, r)
     @test u ≈ [15] atol=1e-2