Skip to content

Commit 2049705

Browse files
franckgagafranckgaga
committed
idem
1 parent 1f72126 commit 2049705

File tree

2 files changed

+83
-19
lines changed

2 files changed

+83
-19
lines changed

‎test/test_predictive_control.jl

Lines changed: 59 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,6 @@ end
5656
@test all((-mpc.con.A_Ŷmin[:, end], -mpc.con.A_Ŷmax[:, end]) .≈ ([1.0,1.1], [1.2,1.3]))
5757
end
5858

59-
6059
@testset "LinMPC moves" begin
6160
mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
6261
r = [5]
@@ -80,7 +79,6 @@ end
8079
f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
8180
h(x,d) = linmodel1.C*x + linmodel1.Du*d
8281
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
83-
8482
nmpc1 = NonLinMPC(nonlinmodel, Hp=15)
8583
@test isa(nmpc1.estim, UnscentedKalmanFilter)
8684
@test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny
@@ -89,15 +87,72 @@ end
8987
nmpc3 = NonLinMPC(nonlinmodel, Hc=4, Cwt=1e6)
9088
@test size(nmpc3.Ẽ, 2) == 4*nonlinmodel.nu + 1
9189
@test nmpc3.C == 1e6
92-
9390
nmpc4 = NonLinMPC(nonlinmodel, Mwt=[1,2], Hp=15)
9491
@test nmpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
9592
nmpc5 = NonLinMPC(nonlinmodel, Nwt=[3,4], Cwt=1e3, Hc=5)
9693
@test nmpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
9794
nmpc6 = NonLinMPC(nonlinmodel, Lwt=[0,1], ru=[0,50], Hp=15)
9895
@test nmpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
9996
@test nmpc6.R̂u ≈ repeat([0,50], 15)
100-
nmpc7 = NonLinMPC
97+
nmpc7 = NonLinMPC(nonlinmodel, Ewt=1e-3, JE=(UE,ŶE,D̂E) -> UE.*ŶE.*D̂E)
98+
@test nmpc7.E == 1e-3
99+
@test nmpc7.JE([1,2],[3,4],[4,6]) == [12, 48]
100+
nmpc8 = NonLinMPC(nonlinmodel, optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer))
101+
@test solver_name(nmpc8.optim) == "OSQP"
102+
im = InternalModel(nonlinmodel)
103+
nmpc9 = NonLinMPC(im)
104+
@test isa(nmpc9.estim, InternalModel)
105+
end
106+
107+
@testset "NonLinMPC constraints" begin
108+
linmodel1 = LinModel(sys,Ts,i_d=[3])
109+
f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
110+
h(x,d) = linmodel1.C*x + linmodel1.Du*d
111+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
112+
nmpc = NonLinMPC(nonlinmodel, Hp=1, Hc=1)
113+
setconstraint!(nmpc, umin=[5, 9.9], umax=[100,99])
114+
@test all((nmpc.con.Umin, nmpc.con.Umax) .≈ ([5, 9.9], [100,99]))
115+
setconstraint!(nmpc, Δumin=[-5,-10], Δumax=[6,11])
116+
@test all((nmpc.con.ΔŨmin, nmpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf]))
117+
setconstraint!(nmpc, ŷmin=[5,10],ŷmax=[55, 35])
118+
@test all((nmpc.con.Ŷmin, nmpc.con.Ŷmax) .≈ ([5,10], [55,35]))
119+
setconstraint!(nmpc, c_umin=[0.1,0.2], c_umax=[0.3,0.4])
120+
@test all((-nmpc.con.A_Umin[:, end], -nmpc.con.A_Umax[:, end]) .≈ ([0.1,0.2], [0.3,0.4]))
121+
setconstraint!(nmpc, c_Δumin=[0.05,0.15], c_Δumax=[0.25,0.35])
122+
@test all((-nmpc.con.A_ΔŨmin[1:end-1, end], -nmpc.con.A_ΔŨmax[1:end-1, end]) .≈ ([0.05,0.15], [0.25,0.35]))
123+
setconstraint!(nmpc, c_ŷmin=[1.0,1.1], c_ŷmax=[1.2,1.3])
124+
println(nmpc.con.A_Ŷmin)
125+
@test all((-nmpc.con.A_Ŷmin, -nmpc.con.A_Ŷmax) .≈ (zeros(0,3), zeros(0,3)))
126+
@test all((nmpc.con.c_Ŷmin, nmpc.con.c_Ŷmax) .≈ ([1.0,1.1], [1.2,1.3]))
127+
end
128+
129+
@testset "NonLinMPC moves" begin
130+
linmodel = LinModel(tf(5, [2, 1]), 3)
131+
nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
132+
r = [5]
133+
u = moveinput!(nmpc_lin, r)
134+
@test u ≈ [1] atol=1e-3
135+
u = nmpc_lin(r)
136+
@test u ≈ [1] atol=1e-3
101137

138+
f(x,u,_) = linmodel.A*x + linmodel.Bu*u
139+
h(x,_) = linmodel.C*x
140+
nonlinmodel = NonLinModel(f, h, 3, 1, 1, 1)
141+
nmpc_nonlin = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
142+
r = [5]
143+
u = moveinput!(nmpc_nonlin, r)
144+
@test u ≈ [1] atol=1e-3
145+
u = nmpc_nonlin(r)
146+
@test u ≈ [1] atol=1e-3
147+
end
102148

149+
@testset "NonLinMPC other methods" begin
150+
linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
151+
f(x,u,_) = linmodel.A*x + linmodel.Bu*u
152+
h(x,_) = linmodel.C*x
153+
nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2)
154+
nmpc1 = NonLinMPC(nonlinmodel)
155+
@test initstate!(nmpc1, [10, 50], [20, 25]) ≈ [zeros(2); [20, 25]]
156+
setstate!(nmpc1, [1,2,3,4])
157+
@test nmpc1.estim.x̂ ≈ [1,2,3,4]
103158
end

‎test/test_state_estim.jl

Lines changed: 24 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -112,30 +112,39 @@ end
112112
@test internalmodel2.nxs == 1
113113
@test internalmodel2.nx̂ == 4
114114

115-
stoch_ym_tf = tf([1, -0.3],[1, -0.5],Ts)*tf([1,0],[1,-1],Ts).*I(2)
116-
internalmodel3 = InternalModel(linmodel2,stoch_ym=stoch_ym_tf)
115+
f(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
116+
h(x,d) = linmodel2.C*x + linmodel2.Dd*d
117+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2)
118+
internalmodel3 = InternalModel(nonlinmodel)
117119
@test internalmodel3.nym == 2
118120
@test internalmodel3.nyu == 0
119-
@test internalmodel3.nxs == 4
120-
@test internalmodel3.nx̂ == 4
121+
@test internalmodel3.nxs == 2
122+
@test internalmodel3.nx̂ == 4
121123

122-
stoch_ym_ss=minreal(ss(stoch_ym_tf))
123-
internalmodel4 = InternalModel(linmodel2,stoch_ym=stoch_ym_ss)
124+
stoch_ym_tf = tf([1, -0.3],[1, -0.5],Ts)*tf([1,0],[1,-1],Ts).*I(2)
125+
internalmodel4 = InternalModel(linmodel2,stoch_ym=stoch_ym_tf)
124126
@test internalmodel4.nym == 2
125127
@test internalmodel4.nyu == 0
126128
@test internalmodel4.nxs == 4
127129
@test internalmodel4.nx̂ == 4
128-
@test internalmodel4.As == stoch_ym_ss.A
129-
@test internalmodel4.Bs == stoch_ym_ss.B
130-
@test internalmodel4.Cs == stoch_ym_ss.C
131-
@test internalmodel4.Ds == stoch_ym_ss.D
130+
131+
stoch_ym_ss=minreal(ss(stoch_ym_tf))
132+
internalmodel5 = InternalModel(linmodel2,stoch_ym=stoch_ym_ss)
133+
@test internalmodel5.nym == 2
134+
@test internalmodel5.nyu == 0
135+
@test internalmodel5.nxs == 4
136+
@test internalmodel5.nx̂ == 4
137+
@test internalmodel5.As == stoch_ym_ss.A
138+
@test internalmodel5.Bs == stoch_ym_ss.B
139+
@test internalmodel5.Cs == stoch_ym_ss.C
140+
@test internalmodel5.Ds == stoch_ym_ss.D
132141

133142
stoch_ym_resample = c2d(d2c(ss(1,1,1,1,linmodel2.Ts), :tustin), 2linmodel2.Ts, :tustin)
134-
internalmodel5 = InternalModel(linmodel2, i_ym=[2], stoch_ym=stoch_ym_resample)
135-
@test internalmodel5.As ≈ internalmodel2.As
136-
@test internalmodel5.Bs ≈ internalmodel2.Bs
137-
@test internalmodel5.Cs ≈ internalmodel2.Cs
138-
@test internalmodel5.Ds ≈ internalmodel2.Ds
143+
internalmodel6 = InternalModel(linmodel2, i_ym=[2], stoch_ym=stoch_ym_resample)
144+
@test internalmodel6.As ≈ internalmodel2.As
145+
@test internalmodel6.Bs ≈ internalmodel2.Bs
146+
@test internalmodel6.Cs ≈ internalmodel2.Cs
147+
@test internalmodel6.Ds ≈ internalmodel2.Ds
139148

140149
unstablemodel = LinModel(ss(diagm([0.5, -0.5, 1.5]), ones(3,1), I, 0, 1))
141150
@test_throws ErrorException InternalModel(unstablemodel)

0 commit comments

Comments
 (0)