Skip to content

Commit d446beb

Browse files
franckgagafranckgaga
committed
test: seperating MHE estimate and getinfo 
1 parent 60b13fb commit d446beb

1 file changed

Lines changed: 74 additions & 65 deletions

File tree

‎test/2_test_state_estim.jl‎

Lines changed: 74 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -924,16 +924,87 @@ end
924924
)
925925
end
926926

927-
@testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin
927+
@testitem "MovingHorizonEstimator estimation and getinfo (LinModel)" setup=[SetupMPCtests] begin
928928
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
929-
using JuMP, Ipopt, DAQP
929+
using JuMP, DAQP
930+
linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3])
931+
linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
932+
933+
mhe2 = MovingHorizonEstimator(linmodel, He=2)
934+
preparestate!(mhe2, [50, 30], [5])
935+
x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
936+
@test x̂ ≈ zeros(6) atol=1e-9
937+
@test mhe2.x̂0 ≈ zeros(6) atol=1e-9
938+
preparestate!(mhe2, [50, 30], [5])
939+
info = getinfo(mhe2)
940+
@test info[:x̂] ≈ x̂ atol=1e-9
941+
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
942+
for i in 1:40
943+
preparestate!(mhe2, [50, 30], [5])
944+
updatestate!(mhe2, [11, 52], [50, 30], [5])
945+
end
946+
preparestate!(mhe2, [50, 30], [5])
947+
@test mhe2([5]) ≈ [50, 30] atol=1e-3
948+
for i in 1:40
949+
preparestate!(mhe2, [51, 32], [5])
950+
updatestate!(mhe2, [10, 50], [51, 32], [5])
951+
end
952+
preparestate!(mhe2, [51, 32], [5])
953+
@test mhe2([5]) ≈ [51, 32] atol=1e-3
954+
955+
mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false)
956+
preparestate!(mhe2, [50, 30], [5])
957+
x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
958+
@test x̂ ≈ zeros(6) atol=1e-9
959+
@test mhe2.x̂0 ≈ zeros(6) atol=1e-9
960+
info = getinfo(mhe2)
961+
@test info[:x̂] ≈ x̂ atol=1e-9
962+
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
963+
for i in 1:40
964+
preparestate!(mhe2, [50, 30], [5])
965+
updatestate!(mhe2, [11, 52], [50, 30], [5])
966+
end
967+
@test mhe2([5]) ≈ [50, 30] atol=1e-2
968+
for i in 1:40
969+
preparestate!(mhe2, [51, 32], [5])
970+
updatestate!(mhe2, [10, 50], [51, 32], [5])
971+
end
972+
@test mhe2([5]) ≈ [51, 32] atol=1e-2
973+
974+
Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2)
975+
R̂ = diagm([1, 1].^2)
976+
optim = Model(DAQP.Optimizer)
977+
covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂)
978+
P̂_0 = covestim.cov.P̂
979+
mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim)
980+
preparestate!(mhe3, [50, 30], [5])
981+
x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5])
982+
@test x̂ ≈ zeros(4) atol=1e-9
983+
@test mhe3.x̂0 ≈ zeros(4) atol=1e-9
984+
preparestate!(mhe3, [50, 30], [5])
985+
info = getinfo(mhe3)
986+
@test info[:x̂] ≈ x̂ atol=1e-9
987+
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
988+
989+
linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
990+
mhe3 = MovingHorizonEstimator(linmodel3, He=1)
991+
preparestate!(mhe3, [0])
992+
x̂ = updatestate!(mhe3, [0], [0])
993+
@test x̂ ≈ [0, 0] atol=1e-3
994+
@test isa(x̂, Vector{Float32})
995+
end
996+
997+
@testitem "MovingHorizonEstimator estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin
998+
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
999+
using JuMP, Ipopt
9301000
linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3])
9311001
linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
9321002
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
9331003
h(x,d,model) = model.C*x + model.Dd*d
9341004
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel)
9351005
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
9361006

1007+
9371008
mhe1 = MovingHorizonEstimator(nonlinmodel, He=2)
9381009
JuMP.set_attribute(mhe1.optim, "tol", 1e-7)
9391010
preparestate!(mhe1, [50, 30], [5])
@@ -998,69 +1069,6 @@ end
9981069
end
9991070
@test mhe1c([5]) ≈ [51, 32] atol=1e-3
10001071

1001-
mhe2 = MovingHorizonEstimator(linmodel, He=2)
1002-
preparestate!(mhe2, [50, 30], [5])
1003-
x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
1004-
@test x̂ ≈ zeros(6) atol=1e-9
1005-
@test mhe2.x̂0 ≈ zeros(6) atol=1e-9
1006-
preparestate!(mhe2, [50, 30], [5])
1007-
info = getinfo(mhe2)
1008-
@test info[:x̂] ≈ x̂ atol=1e-9
1009-
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
1010-
for i in 1:40
1011-
preparestate!(mhe2, [50, 30], [5])
1012-
updatestate!(mhe2, [11, 52], [50, 30], [5])
1013-
end
1014-
preparestate!(mhe2, [50, 30], [5])
1015-
@test mhe2([5]) ≈ [50, 30] atol=1e-3
1016-
for i in 1:40
1017-
preparestate!(mhe2, [51, 32], [5])
1018-
updatestate!(mhe2, [10, 50], [51, 32], [5])
1019-
end
1020-
preparestate!(mhe2, [51, 32], [5])
1021-
@test mhe2([5]) ≈ [51, 32] atol=1e-3
1022-
1023-
mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false)
1024-
preparestate!(mhe2, [50, 30], [5])
1025-
x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
1026-
@test x̂ ≈ zeros(6) atol=1e-9
1027-
@test mhe2.x̂0 ≈ zeros(6) atol=1e-9
1028-
info = getinfo(mhe2)
1029-
@test info[:x̂] ≈ x̂ atol=1e-9
1030-
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
1031-
for i in 1:40
1032-
preparestate!(mhe2, [50, 30], [5])
1033-
updatestate!(mhe2, [11, 52], [50, 30], [5])
1034-
end
1035-
@test mhe2([5]) ≈ [50, 30] atol=1e-2
1036-
for i in 1:40
1037-
preparestate!(mhe2, [51, 32], [5])
1038-
updatestate!(mhe2, [10, 50], [51, 32], [5])
1039-
end
1040-
@test mhe2([5]) ≈ [51, 32] atol=1e-2
1041-
1042-
Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2)
1043-
R̂ = diagm([1, 1].^2)
1044-
optim = Model(DAQP.Optimizer)
1045-
covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂)
1046-
P̂_0 = covestim.cov.P̂
1047-
mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim)
1048-
preparestate!(mhe3, [50, 30], [5])
1049-
x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5])
1050-
@test x̂ ≈ zeros(4) atol=1e-9
1051-
@test mhe3.x̂0 ≈ zeros(4) atol=1e-9
1052-
preparestate!(mhe3, [50, 30], [5])
1053-
info = getinfo(mhe3)
1054-
@test info[:x̂] ≈ x̂ atol=1e-9
1055-
@test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
1056-
1057-
linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
1058-
mhe3 = MovingHorizonEstimator(linmodel3, He=1)
1059-
preparestate!(mhe3, [0])
1060-
x̂ = updatestate!(mhe3, [0], [0])
1061-
@test x̂ ≈ [0, 0] atol=1e-3
1062-
@test isa(x̂, Vector{Float32})
1063-
10641072
Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2)
10651073
R̂ = diagm([1, 1].^2)
10661074
optim = Model(Ipopt.Optimizer)
@@ -1092,6 +1100,7 @@ end
10921100
@test x̂ ≈ zeros(6) atol=1e-9
10931101
@test_nowarn ModelPredictiveControl.info2debugstr(info)
10941102
@test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6))
1103+
10951104
end
10961105

10971106
@testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin

0 commit comments

Comments
 (0)