Skip to content

Commit 8c402f3

Browse files
franckgagafranckgaga
committed
added : unit tests for UnscentedKalmanFilter
1 parent 7b40131 commit 8c402f3

File tree

3 files changed

+259
-158
lines changed

3 files changed

+259
-158
lines changed

‎test/test_predictive_control.jl‎

Lines changed: 70 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -3,65 +3,86 @@ sys = [ tf(1.90,[18.0,1]) tf(1.90,[18.0,1]) tf(1.90,[18.0,1]);
33
tf(-0.74,[8.0,1]) tf(0.74,[8.0,1]) tf(-0.74,[8.0,1]) ]
44

55
@testset "LinMPC construction" begin
6+
model = LinModel(sys, Ts, i_d=[3])
7+
mpc1 = LinMPC(model, Hp=15)
8+
@test isa(mpc1.estim, SteadyKalmanFilter)
9+
@test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny
10+
mpc2 = LinMPC(model, Hc=4, Cwt=Inf)
11+
@test size(mpc2.Ẽ,2) == 4*mpc2.estim.model.nu
12+
mpc3 = LinMPC(model, Hc=4, Cwt=1e5)
13+
@test size(mpc3.Ẽ,2) == 4*mpc3.estim.model.nu + 1
14+
@test mpc3.C ≈ 1e5
15+
mpc4 = LinMPC(model, Mwt=[1,2], Hp=15)
16+
@test mpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15)))
17+
mpc5 = LinMPC(model, Nwt=[3,4], Cwt=1e3, Hc=5)
18+
@test mpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
19+
mpc6 = LinMPC(model, Lwt=[0,1], ru=[0,50], Hp=15)
20+
@test mpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
21+
@test mpc6.R̂u ≈ repeat([0,50], 15)
22+
mpc7 = LinMPC(model, optim=JuMP.Model(Ipopt.Optimizer))
23+
@test solver_name(mpc7.optim) == "Ipopt"
24+
kf = KalmanFilter(model)
25+
mpc8 = LinMPC(kf)
26+
@test isa(mpc8.estim, KalmanFilter)
627

7-
model = LinModel(sys, Ts, i_d=[3])
8-
mpc1 = LinMPC(model, Hp=15)
9-
@test isa(mpc1.estim, SteadyKalmanFilter)
10-
@test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny
11-
12-
mpc2 = LinMPC(model, Hc=4, Cwt=Inf)
13-
@test size(mpc2.Ẽ,2) == 4*mpc2.estim.model.nu
14-
15-
mpc3 = LinMPC(model, Hc=4, Cwt=1e5)
16-
@test size(mpc3.Ẽ,2) == 4*mpc3.estim.model.nu + 1
17-
18-
kf = KalmanFilter(model)
19-
mpc4 = LinMPC(kf)
20-
@test isa(mpc4.estim, KalmanFilter)
21-
22-
@test_throws ErrorException LinMPC(model, Hp=0)
23-
@test_throws ErrorException LinMPC(model, Hc=0)
24-
@test_throws ErrorException LinMPC(model, Hp=1, Hc=2)
25-
@test_throws ErrorException LinMPC(model, Mwt=[1])
26-
@test_throws ErrorException LinMPC(model, Mwt=[1])
27-
@test_throws ErrorException LinMPC(model, Lwt=[1])
28-
@test_throws ErrorException LinMPC(model, ru=[1])
29-
@test_throws ErrorException LinMPC(model, Cwt=[1])
30-
@test_throws ErrorException LinMPC(model, Mwt=[-1,1])
31-
@test_throws ErrorException LinMPC(model, Nwt=[-1,1])
32-
@test_throws ErrorException LinMPC(model, Lwt=[-1,1])
33-
@test_throws ErrorException LinMPC(model, Cwt=-1)
34-
28+
@test_throws ErrorException LinMPC(model, Hp=0)
29+
@test_throws ErrorException LinMPC(model, Hc=0)
30+
@test_throws ErrorException LinMPC(model, Hp=1, Hc=2)
31+
@test_throws ErrorException LinMPC(model, Mwt=[1])
32+
@test_throws ErrorException LinMPC(model, Mwt=[1])
33+
@test_throws ErrorException LinMPC(model, Lwt=[1])
34+
@test_throws ErrorException LinMPC(model, ru=[1])
35+
@test_throws ErrorException LinMPC(model, Cwt=[1])
36+
@test_throws ErrorException LinMPC(model, Mwt=[-1,1])
37+
@test_throws ErrorException LinMPC(model, Nwt=[-1,1])
38+
@test_throws ErrorException LinMPC(model, Lwt=[-1,1])
39+
@test_throws ErrorException LinMPC(model, Cwt=-1)
3540
end
3641

3742
@testset "LinMPC constraints" begin
43+
model = LinModel(sys, Ts, i_d=[3])
44+
mpc = LinMPC(model, Hp=1, Hc=1)
45+
setconstraint!(mpc, umin=[5, 9.9], umax=[100,99])
46+
@test all((mpc.con.Umin, mpc.con.Umax) .≈ ([5, 9.9], [100,99]))
47+
setconstraint!(mpc, Δumin=[-5,-10], Δumax=[6,11])
48+
@test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf]))
49+
setconstraint!(mpc, ŷmin=[5,10],ŷmax=[55, 35])
50+
@test all((mpc.con.Ŷmin, mpc.con.Ŷmax) .≈ ([5,10], [55,35]))
51+
setconstraint!(mpc, c_umin=[0.1,0.2], c_umax=[0.3,0.4])
52+
@test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ ([0.1,0.2], [0.3,0.4]))
53+
setconstraint!(mpc, c_Δumin=[0.05,0.15], c_Δumax=[0.25,0.35])
54+
@test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ ([0.05,0.15], [0.25,0.35]))
55+
setconstraint!(mpc, c_ŷmin=[1.0,1.1], c_ŷmax=[1.2,1.3])
56+
@test all((-mpc.con.A_Ŷmin[:, end], -mpc.con.A_Ŷmax[:, end]) .≈ ([1.0,1.1], [1.2,1.3]))
57+
end
3858

39-
model = LinModel(sys, Ts, i_d=[3])
40-
mpc = LinMPC(model, Hp=1, Hc=1)
4159

42-
setconstraint!(mpc, umin=[5, 9.9], umax=[100,99])
43-
@test all((mpc.con.Umin, mpc.con.Umax) .≈ ([5, 9.9], [100,99]))
44-
setconstraint!(mpc, Δumin=[-5,-10], Δumax=[6,11])
45-
@test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf]))
46-
setconstraint!(mpc, ŷmin=[5,10],ŷmax=[55, 35])
47-
@test all((mpc.con.Ŷmin, mpc.con.Ŷmax) .≈ ([5,10], [55,35]))
48-
setconstraint!(mpc, c_umin=[0.1,0.2], c_umax=[0.3,0.4])
49-
@test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ ([0.1,0.2], [0.3,0.4]))
50-
setconstraint!(mpc, c_Δumin=[0.05,0.15], c_Δumax=[0.25,0.35])
51-
@test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ ([0.05,0.15], [0.25,0.35]))
52-
setconstraint!(mpc, c_ŷmin=[1.0,1.1], c_ŷmax=[1.2,1.3])
53-
@test all((-mpc.con.A_Ŷmin[:, end], -mpc.con.A_Ŷmax[:, end]) .≈ ([1.0,1.1], [1.2,1.3]))
60+
@testset "LinMPC moves" begin
61+
mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
62+
r = [5]
63+
u = moveinput!(mpc, r)
64+
@test u ≈ [1] atol=1e-3
65+
u = mpc(r)
66+
@test u ≈ [1] atol=1e-3
67+
end
5468

69+
@testset "LinMPC other methods" begin
70+
linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
71+
mpc1 = LinMPC(linmodel1)
72+
@test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
73+
setstate!(mpc1, [1,2,3,4])
74+
@test mpc1.estim.x̂ ≈ [1,2,3,4]
5575
end
5676

5777

58-
@testset "LinMPC moves" begin
78+
@testset "NonLinMPC construction" begin
79+
linmodel1 = LinModel(sys,Ts,i_d=[3])
80+
f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
81+
h(x,d) = linmodel1.C*x + linmodel1.Du*d
82+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
5983

60-
mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
61-
r = [5]
62-
u = moveinput!(mpc, r)
63-
@test u ≈ [1] atol=1e-3
64-
u = mpc(r)
65-
@test u ≈ [1] atol=1e-3
84+
nmpc1 = NonLinMPC(nonlinmodel, Hp=15)
85+
@test isa(nmpc1.estim, UnscentedKalmanFilter)
86+
@test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny
6687

6788
end

‎test/test_sim_model.jl‎

Lines changed: 108 additions & 108 deletions
Original file line numberDiff line numberDiff line change
@@ -6,124 +6,124 @@ Gss = c2d(sys_ss[:,1:2], Ts, :zoh)
66
Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
77

88
@testset "LinModel construction" begin
9-
linmodel1 = LinModel(sys, Ts, i_u=1:2)
10-
@test linmodel1.nx == 2
11-
@test linmodel1.nu == 2
12-
@test linmodel1.nd == 0
13-
@test linmodel1.ny == 2
14-
@test linmodel1.A ≈ Gss.A
15-
@test linmodel1.Bu ≈ Gss.B
16-
@test linmodel1.Bd ≈ zeros(2,0)
17-
@test linmodel1.C ≈ Gss.C
18-
@test linmodel1.Dd ≈ zeros(2,0)
19-
20-
linmodel2 = LinModel(Gss)
21-
setop!(linmodel2, uop=[10,50], yop=[50,30])
22-
@test linmodel2.A ≈ Gss.A
23-
@test linmodel2.Bu ≈ Gss.B
24-
@test linmodel2.Bd ≈ zeros(2,0)
25-
@test linmodel2.C ≈ Gss.C
26-
@test linmodel2.Dd ≈ zeros(2,0)
27-
@test linmodel2.uop ≈ [10,50]
28-
@test linmodel2.yop ≈ [50,30]
29-
@test linmodel2.dop ≈ zeros(0,1)
30-
31-
linmodel3 = LinModel(Gss, 0.5Ts)
32-
@test linmodel3.Ts == 2.0
33-
@test linmodel3.A ≈ Gss2.A
34-
@test linmodel3.C ≈ Gss2.C
35-
36-
linmodel4 = LinModel(Gss)
37-
setstate!(linmodel4, [1;-1])
38-
@test linmodel4.x ≈ [1;-1]
39-
40-
linmodel5 = LinModel(sys,Ts,i_d=[3])
41-
@test linmodel5.nx == 4
42-
@test linmodel5.nu == 2
43-
@test linmodel5.nd == 1
44-
@test linmodel5.ny == 2
45-
sysu_ss = sminreal(c2d(minreal(ss(sys))[:,1:2], Ts, :zoh))
46-
sysd_ss = sminreal(c2d(minreal(ss(sys))[:,3], Ts, :tustin))
47-
sys_ss = [sysu_ss sysd_ss]
48-
@test linmodel5.A ≈ sys_ss.A
49-
@test linmodel5.Bu ≈ sys_ss.B[:,1:2]
50-
@test linmodel5.Bd ≈ sys_ss.B[:,3]
51-
@test linmodel5.C ≈ sys_ss.C
52-
@test linmodel5.Dd ≈ sys_ss.D[:,3]
53-
54-
linmodel6 = LinModel([delay(4) delay(4)]*sys,Ts,i_d=[3])
55-
@test linmodel6.nx == 6
56-
@test sum(eigvals(linmodel6.A) .≈ 0) == 2
57-
58-
@test_throws ErrorException LinModel(sys)
59-
@test_throws ErrorException LinModel(sys,-Ts)
60-
@test_throws ErrorException LinModel(sys,Ts,i_u=[1,1])
61-
@test_throws ErrorException LinModel(sys_ss,Ts+1)
62-
@test_throws ErrorException setop!(linmodel5, uop=[0,0,0,0,0])
63-
@test_throws ErrorException setop!(linmodel5, yop=[0,0,0,0,0])
64-
@test_throws ErrorException setop!(linmodel5, dop=[0,0,0,0,0])
65-
sys_ss.D .= 1
66-
@test_throws ErrorException LinModel(sys_ss,Ts)
9+
linmodel1 = LinModel(sys, Ts, i_u=1:2)
10+
@test linmodel1.nx == 2
11+
@test linmodel1.nu == 2
12+
@test linmodel1.nd == 0
13+
@test linmodel1.ny == 2
14+
@test linmodel1.A ≈ Gss.A
15+
@test linmodel1.Bu ≈ Gss.B
16+
@test linmodel1.Bd ≈ zeros(2,0)
17+
@test linmodel1.C ≈ Gss.C
18+
@test linmodel1.Dd ≈ zeros(2,0)
19+
20+
linmodel2 = LinModel(Gss)
21+
setop!(linmodel2, uop=[10,50], yop=[50,30])
22+
@test linmodel2.A ≈ Gss.A
23+
@test linmodel2.Bu ≈ Gss.B
24+
@test linmodel2.Bd ≈ zeros(2,0)
25+
@test linmodel2.C ≈ Gss.C
26+
@test linmodel2.Dd ≈ zeros(2,0)
27+
@test linmodel2.uop ≈ [10,50]
28+
@test linmodel2.yop ≈ [50,30]
29+
@test linmodel2.dop ≈ zeros(0,1)
30+
31+
linmodel3 = LinModel(Gss, 0.5Ts)
32+
@test linmodel3.Ts == 2.0
33+
@test linmodel3.A ≈ Gss2.A
34+
@test linmodel3.C ≈ Gss2.C
35+
36+
linmodel4 = LinModel(Gss)
37+
setstate!(linmodel4, [1;-1])
38+
@test linmodel4.x ≈ [1;-1]
39+
40+
linmodel5 = LinModel(sys,Ts,i_d=[3])
41+
@test linmodel5.nx == 4
42+
@test linmodel5.nu == 2
43+
@test linmodel5.nd == 1
44+
@test linmodel5.ny == 2
45+
sysu_ss = sminreal(c2d(minreal(ss(sys))[:,1:2], Ts, :zoh))
46+
sysd_ss = sminreal(c2d(minreal(ss(sys))[:,3], Ts, :tustin))
47+
sys_ss = [sysu_ss sysd_ss]
48+
@test linmodel5.A ≈ sys_ss.A
49+
@test linmodel5.Bu ≈ sys_ss.B[:,1:2]
50+
@test linmodel5.Bd ≈ sys_ss.B[:,3]
51+
@test linmodel5.C ≈ sys_ss.C
52+
@test linmodel5.Dd ≈ sys_ss.D[:,3]
53+
54+
linmodel6 = LinModel([delay(4) delay(4)]*sys,Ts,i_d=[3])
55+
@test linmodel6.nx == 6
56+
@test sum(eigvals(linmodel6.A) .≈ 0) == 2
57+
58+
@test_throws ErrorException LinModel(sys)
59+
@test_throws ErrorException LinModel(sys,-Ts)
60+
@test_throws ErrorException LinModel(sys,Ts,i_u=[1,1])
61+
@test_throws ErrorException LinModel(sys_ss,Ts+1)
62+
@test_throws ErrorException setop!(linmodel5, uop=[0,0,0,0,0])
63+
@test_throws ErrorException setop!(linmodel5, yop=[0,0,0,0,0])
64+
@test_throws ErrorException setop!(linmodel5, dop=[0,0,0,0,0])
65+
sys_ss.D .= 1
66+
@test_throws ErrorException LinModel(sys_ss,Ts)
6767
end
6868

6969
@testset "LinModel sim methods" begin
70-
linmodel1 = setop!(LinModel(Gss), uop=[10,50], yop=[50,30])
70+
linmodel1 = setop!(LinModel(Gss), uop=[10,50], yop=[50,30])
7171

72-
@test updatestate!(linmodel1, [10, 50]) ≈ zeros(2)
73-
@test updatestate!(linmodel1, [10, 50], Float64[]) ≈ zeros(2)
74-
@test linmodel1.x ≈ zeros(2)
75-
@test evaloutput(linmodel1) ≈ linmodel1() ≈ [50,30]
76-
@test evaloutput(linmodel1, Float64[]) ≈ linmodel1(Float64[]) ≈ [50,30]
72+
@test updatestate!(linmodel1, [10, 50]) ≈ zeros(2)
73+
@test updatestate!(linmodel1, [10, 50], Float64[]) ≈ zeros(2)
74+
@test linmodel1.x ≈ zeros(2)
75+
@test evaloutput(linmodel1) ≈ linmodel1() ≈ [50,30]
76+
@test evaloutput(linmodel1, Float64[]) ≈ linmodel1(Float64[]) ≈ [50,30]
7777

78-
@test_throws DimensionMismatch updatestate!(linmodel1, zeros(2), zeros(1))
79-
@test_throws DimensionMismatch evaloutput(linmodel1, zeros(1))
78+
@test_throws DimensionMismatch updatestate!(linmodel1, zeros(2), zeros(1))
79+
@test_throws DimensionMismatch evaloutput(linmodel1, zeros(1))
8080
end
8181

8282

8383
@testset "NonLinModel construction" begin
84-
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
85-
f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
86-
h1(x,_) = linmodel1.C*x
87-
nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2)
88-
@test nonlinmodel1.nx == 2
89-
@test nonlinmodel1.nu == 2
90-
@test nonlinmodel1.nd == 0
91-
@test nonlinmodel1.ny == 2
92-
@test nonlinmodel1.f([0,0],[0,0],[1]) ≈ zeros(2,)
93-
@test nonlinmodel1.h([0,0],[1]) ≈ zeros(2,)
94-
95-
linmodel2 = LinModel(sys,Ts,i_d=[3])
96-
f2(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
97-
h2(x,_) = linmodel2.C*x
98-
nonlinmodel2 = NonLinModel(f2,h2,Ts,2,4,2,1)
99-
100-
@test nonlinmodel2.nx == 4
101-
@test nonlinmodel2.nu == 2
102-
@test nonlinmodel2.nd == 1
103-
@test nonlinmodel2.ny == 2
104-
@test nonlinmodel2.f([0,0,0,0],[0,0],[0]) ≈ zeros(4,)
105-
@test nonlinmodel2.h([0,0,0,0],[0]) ≈ zeros(2,)
106-
107-
@test_throws ErrorException NonLinModel(
108-
(x,u)->linmodel1.A*x + linmodel1.Bu*u,
109-
(x,_)->linmodel1.C*x, Ts, 2, 4, 2, 1)
110-
@test_throws ErrorException NonLinModel(
111-
(x,u,_)->linmodel1.A*x + linmodel1.Bu*u,
112-
(x)->linmodel1.C*x, Ts, 2, 4, 2, 1)
84+
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
85+
f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
86+
h1(x,_) = linmodel1.C*x
87+
nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2)
88+
@test nonlinmodel1.nx == 2
89+
@test nonlinmodel1.nu == 2
90+
@test nonlinmodel1.nd == 0
91+
@test nonlinmodel1.ny == 2
92+
@test nonlinmodel1.f([0,0],[0,0],[1]) ≈ zeros(2,)
93+
@test nonlinmodel1.h([0,0],[1]) ≈ zeros(2,)
94+
95+
linmodel2 = LinModel(sys,Ts,i_d=[3])
96+
f2(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
97+
h2(x,_) = linmodel2.C*x
98+
nonlinmodel2 = NonLinModel(f2,h2,Ts,2,4,2,1)
99+
100+
@test nonlinmodel2.nx == 4
101+
@test nonlinmodel2.nu == 2
102+
@test nonlinmodel2.nd == 1
103+
@test nonlinmodel2.ny == 2
104+
@test nonlinmodel2.f([0,0,0,0],[0,0],[0]) ≈ zeros(4,)
105+
@test nonlinmodel2.h([0,0,0,0],[0]) ≈ zeros(2,)
106+
107+
@test_throws ErrorException NonLinModel(
108+
(x,u)->linmodel1.A*x + linmodel1.Bu*u,
109+
(x,_)->linmodel1.C*x, Ts, 2, 4, 2, 1)
110+
@test_throws ErrorException NonLinModel(
111+
(x,u,_)->linmodel1.A*x + linmodel1.Bu*u,
112+
(x)->linmodel1.C*x, Ts, 2, 4, 2, 1)
113113
end
114114

115115
@testset "NonLinModel sim methods" begin
116-
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
117-
f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
118-
h1(x,_) = linmodel1.C*x
119-
nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2)
120-
121-
@test updatestate!(nonlinmodel, zeros(2,)) ≈ zeros(2)
122-
@test updatestate!(nonlinmodel, zeros(2,), Float64[]) ≈ zeros(2)
123-
@test nonlinmodel.x ≈ zeros(2)
124-
@test evaloutput(nonlinmodel) ≈ nonlinmodel() ≈ zeros(2)
125-
@test evaloutput(nonlinmodel, Float64[]) ≈ nonlinmodel(Float64[]) ≈ zeros(2)
126-
127-
@test_throws DimensionMismatch updatestate!(nonlinmodel, zeros(2), zeros(1))
128-
@test_throws DimensionMismatch evaloutput(nonlinmodel, zeros(1))
116+
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
117+
f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
118+
h1(x,_) = linmodel1.C*x
119+
nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2)
120+
121+
@test updatestate!(nonlinmodel, zeros(2,)) ≈ zeros(2)
122+
@test updatestate!(nonlinmodel, zeros(2,), Float64[]) ≈ zeros(2)
123+
@test nonlinmodel.x ≈ zeros(2)
124+
@test evaloutput(nonlinmodel) ≈ nonlinmodel() ≈ zeros(2)
125+
@test evaloutput(nonlinmodel, Float64[]) ≈ nonlinmodel(Float64[]) ≈ zeros(2)
126+
127+
@test_throws DimensionMismatch updatestate!(nonlinmodel, zeros(2), zeros(1))
128+
@test_throws DimensionMismatch evaloutput(nonlinmodel, zeros(1))
129129
end

0 commit comments

Comments
 (0)