Skip to content

Commit b9a26e3

Browse files
franckgagafranckgaga
committed
debug: getinfo with NonLinMPC and linear preds
the predicted outputs `Ŷ` and terminal state `x̂end` was not accurate for `NonLinMPC` based on `LinModel`
1 parent f1b0e27 commit b9a26e3

File tree

2 files changed

+89
-61
lines changed

2 files changed

+89
-61
lines changed

‎src/predictive_control.jl

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -609,8 +609,8 @@ the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``.
609609
"""
610610
function predict!(Ŷ, x̂, mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
611611
# in-place operations to reduce allocations :
612-
mul!(Ŷ, mpc.Ẽ, ΔŨ) + mpc.F
613-
mul!(x̂, mpc.con.ẽx̂, ΔŨ) + mpc.con.fx̂
612+
Ŷ[:] = mul!(Ŷ, mpc.Ẽ, ΔŨ) + mpc.F
613+
x̂[:] = mul!(x̂, mpc.con.ẽx̂, ΔŨ) + mpc.con.fx̂
614614
x̂end = x̂
615615
return Ŷ, x̂end
616616
end

‎test/test_predictive_control.jl

Lines changed: 87 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -41,57 +41,67 @@ sys = [ tf(1.90,[18.0,1]) tf(1.90,[18.0,1]) tf(1.90,[18.0,1]);
4141
end
4242

4343
@testset "LinMPC moves and getinfo" begin
44-
mpc1 = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
45-
r = [5]
44+
linmodel = setop!(LinModel(tf(5, [2, 1]), 3), yop=[10])
45+
mpc1 = LinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
46+
r = [15]
4647
u = moveinput!(mpc1, r)
4748
@test u ≈ [1] atol=1e-2
4849
u = mpc1(r)
4950
@test u ≈ [1] atol=1e-2
5051
info = getinfo(mpc1)
5152
@test info[:u] ≈ u
5253
@test info[:Ŷ][end] ≈ r[1] atol=1e-2
53-
mpc2 = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
54-
u = moveinput!(mpc2, [5])
54+
mpc2 = LinMPC(linmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
55+
u = moveinput!(mpc2, r)
5556
@test u ≈ [1] atol=1e-2
56-
mpc3 = LinMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1])
57+
mpc3 = LinMPC(linmodel, Mwt=[0], Nwt=[0], Lwt=[1])
5758
u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp))
5859
@test u ≈ [12] atol=1e-2
59-
mpc_im = LinMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
60-
ym, u = mpc_im.estim.model() - [5], [0.0]
60+
61+
@test_throws ArgumentError moveinput!(mpc1, [0,0,0])
62+
@test_throws ArgumentError moveinput!(mpc1, [0], [0,0])
63+
@test_throws ArgumentError moveinput!(mpc1; D̂ = fill(0, mpc1.Hp+1))
64+
@test_throws ArgumentError moveinput!(mpc1; R̂y = fill(0, mpc1.Hp+1))
65+
@test_throws ArgumentError moveinput!(mpc1; R̂u = fill(0, mpc1.Hp+1))
66+
end
67+
68+
@testset "LinMPC step disturbance rejection" begin
69+
linmodel = setop!(LinModel(tf(5, [2, 1]), 3.0), yop=[10])
70+
r = [15]
71+
outdist = [5]
72+
mpc_im = LinMPC(InternalModel(linmodel))
73+
linmodel.x[:] .= 0
74+
ym, u = linmodel() - outdist, [0.0]
6175
for i=1:25
62-
ym = mpc_im.estim.model() - [5]
76+
ym = linmodel() - outdist
6377
u = moveinput!(mpc_im, r; ym)
6478
updatestate!(mpc_im, u, ym)
65-
updatestate!(mpc_im.estim.model, u)
79+
updatestate!(linmodel, u)
6680
end
6781
@test u ≈ [2] atol=1e-2
68-
@test ym ≈ [5] atol=1e-2
82+
@test ym ≈ r atol=1e-2
6983
mpc_nint_u = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
70-
ym, u = mpc_nint_u.estim.model() - [5], [0.0]
84+
linmodel.x[:] .= 0
85+
ym, u = linmodel() - outdist, [0.0]
7186
for i=1:25
72-
ym = mpc_nint_u.estim.model() - [5]
87+
ym = linmodel() - outdist
7388
u = moveinput!(mpc_nint_u, r; ym)
7489
updatestate!(mpc_nint_u, u, ym)
75-
updatestate!(mpc_nint_u.estim.model, u)
90+
updatestate!(linmodel, u)
7691
end
7792
@test u ≈ [2] atol=1e-2
78-
@test ym ≈ [5] atol=1e-2
93+
@test ym ≈ r atol=1e-2
7994
mpc_nint_ym = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
80-
ym, u = mpc_nint_ym.estim.model() - [5], [0.0]
95+
linmodel.x[:] .= 0
96+
ym, u = linmodel() - outdist, [0.0]
8197
for i=1:25
82-
ym = mpc_nint_ym.estim.model() - [5]
98+
ym = linmodel() - outdist
8399
u = moveinput!(mpc_nint_ym, r; ym)
84100
updatestate!(mpc_nint_ym, u, ym)
85-
updatestate!(mpc_nint_ym.estim.model, u)
101+
updatestate!(linmodel, u)
86102
end
87103
@test u ≈ [2] atol=1e-2
88-
@test ym ≈ [5] atol=1e-2
89-
90-
@test_throws ArgumentError moveinput!(mpc1, [0,0,0])
91-
@test_throws ArgumentError moveinput!(mpc1, [0], [0,0])
92-
@test_throws ArgumentError moveinput!(mpc1; D̂ = fill(0, mpc1.Hp+1))
93-
@test_throws ArgumentError moveinput!(mpc1; R̂y = fill(0, mpc1.Hp+1))
94-
@test_throws ArgumentError moveinput!(mpc1; R̂u = fill(0, mpc1.Hp+1))
104+
@test ym ≈ r atol=1e-2
95105
end
96106

97107
@testset "LinMPC other methods" begin
@@ -264,36 +274,46 @@ end
264274
mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1])
265275
u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp))
266276
@test u ≈ [12] atol=1e-2
267-
mpc_im = ExplicitMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
268-
ym, u = mpc_im.estim.model() - [5], [0.0]
277+
end
278+
279+
280+
@testset "ExplicitMPC step disturbance rejection" begin
281+
linmodel = setop!(LinModel(tf(5, [2, 1]), 3.0), yop=[10])
282+
r = [15]
283+
outdist = [5]
284+
mpc_im = ExplicitMPC(InternalModel(linmodel))
285+
linmodel.x[:] .= 0
286+
ym, u = linmodel() - outdist, [0.0]
269287
for i=1:25
270-
ym = mpc_im.estim.model() - [5]
288+
ym = linmodel() - outdist
271289
u = moveinput!(mpc_im, r; ym)
272290
updatestate!(mpc_im, u, ym)
273-
updatestate!(mpc_im.estim.model, u)
291+
updatestate!(linmodel, u)
274292
end
275293
@test u ≈ [2] atol=1e-2
276-
@test ym ≈ [5] atol=1e-2
294+
@test ym ≈ r atol=1e-2
277295
mpc_nint_u = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
278-
ym, u = mpc_nint_u.estim.model() - [5], [0.0]
296+
linmodel.x[:] .= 0
297+
ym, u = linmodel() - outdist, [0.0]
279298
for i=1:25
280-
ym = mpc_nint_u.estim.model() - [5]
299+
ym = linmodel() - outdist
281300
u = moveinput!(mpc_nint_u, r; ym)
282301
updatestate!(mpc_nint_u, u, ym)
283-
updatestate!(mpc_nint_u.estim.model, u)
302+
updatestate!(linmodel, u)
284303
end
285304
@test u ≈ [2] atol=1e-2
286-
@test ym ≈ [5] atol=1e-2
305+
@test ym ≈ r atol=1e-2
287306
mpc_nint_ym = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
288-
ym, u = mpc_nint_ym.estim.model() - [5], [0.0]
307+
linmodel.x[:] .= 0
308+
ym, u = linmodel() - outdist, [0.0]
289309
for i=1:25
290-
ym = mpc_nint_ym.estim.model() - [5]
310+
ym = linmodel() - outdist
291311
u = moveinput!(mpc_nint_ym, r; ym)
292312
updatestate!(mpc_nint_ym, u, ym)
293-
updatestate!(mpc_nint_ym.estim.model, u)
313+
updatestate!(linmodel, u)
294314
end
295315
@test u ≈ [2] atol=1e-2
296-
@test ym ≈ [5] atol=1e-2
316+
@test ym ≈ r atol=1e-2
297317
end
298318

299319
@testset "ExplicitMPC other methods" begin
@@ -356,9 +376,9 @@ end
356376
end
357377

358378
@testset "NonLinMPC moves and getinfo" begin
359-
linmodel = LinModel(tf(5, [2, 1]), 3.0)
379+
linmodel = setop!(LinModel(tf(5, [2, 1]), 3.0), yop=[10])
360380
nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
361-
r = [5]
381+
r = [15]
362382
u = moveinput!(nmpc_lin, r)
363383
@test u ≈ [1] atol=5e-2
364384
u = nmpc_lin(r)
@@ -367,14 +387,14 @@ end
367387
@test info[:u] ≈ u
368388
@test info[:Ŷ][end] ≈ r[1] atol=5e-2
369389
Hp = 1000
370-
R̂y = fill(5, Hp)
390+
R̂y = fill(r[1], Hp)
371391
JE = (_ , ŶE, _ ) -> sum((ŶE[2:end] - R̂y).^2)
372392
nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, Hp=Hp, Hc=1)
373393
u = moveinput!(nmpc)
374394
@test u ≈ [1] atol=5e-2
375-
linmodel = LinModel([tf(5, [2, 1]) tf(7, [8,1])], 3.0, i_d=[2])
376-
f(x,u,d) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
377-
h(x,d) = linmodel.C*x + linmodel.Dd*d
395+
linmodel2 = LinModel([tf(5, [2, 1]) tf(7, [8,1])], 3.0, i_d=[2])
396+
f(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
397+
h(x,d) = linmodel2.C*x + linmodel2.Dd*d
378398
nonlinmodel = NonLinModel(f, h, 3.0, 1, 2, 1, 1)
379399
nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
380400
d = [0.1]
@@ -395,37 +415,45 @@ end
395415
C_Ymax_end = nmpc5.optim.nlp_model.operators.registered_multivariate_operators[end].f
396416
@test C_Ymax_end(Float64.((1.0, 1.0))) ≤ 0.0 # test con_nonlinprog_i(i,::NTuple{N, Float64})
397417
@test C_Ymax_end(Float32.((1.0, 1.0))) ≤ 0.0 # test con_nonlinprog_i(i,::NTuple{N, Real})
398-
nmpc_im = NonLinMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
399-
ym, u = nmpc_im.estim.model() - [5], [0.0]
418+
end
419+
420+
@testset "NonLinMPC step disturbance rejection" begin
421+
linmodel = setop!(LinModel(tf(5, [2, 1]), 3.0), yop=[10])
422+
r = [15]
423+
outdist = [5]
424+
nmpc_im = NonLinMPC(InternalModel(linmodel))
425+
linmodel.x[:] .= 0
426+
ym, u = linmodel() - outdist, [0.0]
400427
for i=1:25
401-
ym = nmpc_im.estim.model() - [5]
428+
ym = linmodel() - outdist
402429
u = moveinput!(nmpc_im, r; ym)
403430
updatestate!(nmpc_im, u, ym)
404-
updatestate!(nmpc_im.estim.model, u)
431+
updatestate!(linmodel, u)
405432
end
406433
@test u ≈ [2] atol=1e-2
407-
@test ym ≈ [5] atol=1e-2
408-
nmpc_nint_u = NonLinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
409-
ym, u = nmpc_nint_u.estim.model() - [5], [0.0]
434+
@test ym ≈ r atol=1e-2
435+
nmpc_nint_u = NonLinMPC(SteadyKalmanFilter(linmodel, nint_u=[1]))
436+
linmodel.x[:] .= 0
437+
ym, u = linmodel() - outdist, [0.0]
410438
for i=1:25
411-
ym = nmpc_nint_u.estim.model() - [5]
439+
ym = linmodel() - outdist
412440
u = moveinput!(nmpc_nint_u, r; ym)
413441
updatestate!(nmpc_nint_u, u, ym)
414-
updatestate!(nmpc_nint_u.estim.model, u)
442+
updatestate!(linmodel, u)
415443
end
416444
@test u ≈ [2] atol=1e-2
417-
@test ym ≈ [5] atol=1e-2
418-
nmpc_nint_ym = NonLinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
419-
ym, u = nmpc_nint_ym.estim.model() - [5], [0.0]
445+
@test ym ≈ r atol=1e-2
446+
nmpc_nint_ym = NonLinMPC(SteadyKalmanFilter(linmodel, nint_ym=[1]))
447+
linmodel.x[:] .= 0
448+
ym, u = linmodel() - outdist, [0.0]
420449
for i=1:25
421-
ym = nmpc_nint_ym.estim.model() - [5]
450+
ym = linmodel() - outdist
422451
u = moveinput!(nmpc_nint_ym, r; ym)
423452
updatestate!(nmpc_nint_ym, u, ym)
424-
updatestate!(nmpc_nint_ym.estim.model, u)
453+
updatestate!(linmodel, u)
425454
end
426455
@test u ≈ [2] atol=1e-2
427-
@test ym ≈ [5] atol=1e-2
428-
456+
@test ym ≈ r atol=1e-2
429457
end
430458

431459
@testset "NonLinMPC other methods" begin

0 commit comments

Comments
 (0)