-
Notifications
You must be signed in to change notification settings - Fork 83
/
test_timeresp.jl
144 lines (121 loc) · 3.65 KB
/
test_timeresp.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
import OrdinaryDiffEq
@testset "test_timeresp" begin
A = [0 1; 0 0]
B = [0;1]
C = [1 0]
sys = ss(A,B,C,0)
Q = I # eye(2)
R = I # eye(1)
L = lqr(sys,Q,R)
u(x,i) = -L*x # Form control law
t=0:0.1:50
x0 = [1.,0]
y, t, x, uout = lsim(sys,u,t,x0=x0) # Continuous time
th = 1e-6
@test sum(abs.(x[end,:])) < th
y, t, x, uout = lsim(c2d(sys,0.1)[1],u,t,x0=x0) # Discrete time
@test sum(abs.(x[end,:])) < th
#Do a manual simulation with uout
ym, tm, xm = lsim(sys, uout, t, x0=x0)
@test y ≈ ym
@test x ≈ xm
# Now compare to closed loop
# Discretization is needed before feedback
sysd = c2d(sys, 0.1)[1]
# Create the closed loop system
sysdfb = ss(sysd.A-sysd.B*L, sysd.B, sysd.C, sysd.D, 0.1)
#Simulate without input
yd, td, xd = lsim(sysdfb, zeros(501), t, x0=x0)
@test y ≈ yd
@test x ≈ xd
# Test that the discrete lsim accepts u function that returns scalar
L = lqr(sysd,Q,R)
u(x,i) = -L*x
yd, td, xd = lsim(sysd, u, t, x0=x0)
@test norm(y - yd)/norm(y) < 0.05 # Since the cost matrices are not discretized, these will differ a bit
@test norm(x - xd)/norm(x) < 0.05
#Test step and impulse Continuous
t0 = 0:0.05:2
systf = [tf(1,[1,1]) 0; 0 tf([1,-1],[1,2,1])]
sysss = ss([-1 0 0; 0 -2 -1; 0 1 0], [1 0; 0 1; 0 0], [1 0 0; 0 1 -1], 0)
yreal = zeros(length(t0), 2, 2)
xreal = zeros(length(t0), 3, 2)
#Step tf
y, t, x = step(systf, t0)
yreal[:,1,1] = 1 .- exp.(-t)
yreal[:,2,2] = -1 .+ exp.(-t)+2*exp.(-t).*t
@test y ≈ yreal atol=1e-4
#Step ss
y, t, x = step(sysss, t)
@test y ≈ yreal atol=1e-4
xreal[:,1,1] = yreal[:,1,1]
xreal[:,2,2] = exp.(-t).*t
xreal[:,3,2] = exp.(-t).*(-t .- 1) .+ 1
@test x ≈ xreal atol=1e-5
#Impulse tf
y, t, x = impulse(systf, t)
yreal[:,1,1] = exp.(-t)
yreal[:,2,2] = exp.(-t).*(1 .- 2*t)
@test y ≈ yreal atol=1e-2
#Impulse ss
y, t, x = impulse(sysss, t)
@test y ≈ yreal atol=1e-2
xreal[:,1,1] = yreal[:,1,1]
xreal[:,2,2] = -exp.(-t).*t + exp.(-t)
xreal[:,3,2] = exp.(-t).*t
@test x ≈ xreal atol=1e-1
#Test step and impulse Discrete
t0 = 0:0.05:2
systf = [tf(1,[1,1]) 0; 0 tf([1,-1],[1,2,1])]
sysss = ss([-1 0 0; 0 -2 -1; 0 1 0], [1 0; 0 1; 0 0], [1 0 0; 0 1 -1], 0)
yreal = zeros(length(t0), 2, 2)
xreal = zeros(length(t0), 3, 2)
#Step tf
y, t, x = step(systf, t0, method=:zoh)
yreal[:,1,1] = 1 .- exp.(-t)
yreal[:,2,2] = -1 .+ exp.(-t) + 2*exp.(-t).*t
@test y ≈ yreal atol=1e-14
#Step ss
y, t, x = step(sysss, t, method=:zoh)
@test y ≈ yreal atol=1e-13
xreal[:,1,1] = yreal[:,1,1]
xreal[:,2,2] = exp.(-t).*t
xreal[:,3,2] = exp.(-t).*(-t .- 1) .+ 1
@test x ≈ xreal atol=1e-14
#Impulse tf
y, t, x = impulse(systf, t, method=:zoh)
yreal[:,1,1] = exp.(-t)
yreal[:,2,2] = exp.(-t).*(1 .- 2*t)
@test y ≈ yreal atol=1e-14
#Impulse ss
y, t, x = impulse(1.0sysss, t, method=:zoh)
@test y ≈ yreal atol=1e-13
xreal[:,1,1] = yreal[:,1,1]
xreal[:,2,2] = -exp.(-t).*t + exp.(-t)
xreal[:,3,2] = exp.(-t).*t
@test x ≈ xreal atol=1e-13
#Step response of discrete system with specified final time
G = tf([1], [1; zeros(3)], 1)
y, t2, x = step(G, 10)
@test y ≈ [zeros(3); ones(8)] atol=1e-5
@test t2 == 0:1:10 # isapprox is broken for ranges (julia 1.3.1)
#Impulse response of discrete system to final time that is not mulitple of the sample time
G = tf([1], [1; zeros(3)], 0.3)
y, t2, x = step(G, 2)
@test y ≈ [zeros(3); ones(4)] atol=1e-5
@test t2 ≈ 0:0.3:1.8 atol=1e-5
#Make sure t was never changed
@test t0 == t
@testset "Simulators" begin
h = 0.1
Tf = 20
t = 0:h:Tf
P = ss(tf(1,[2,1])^2)
reference(x,t) = [1.]
s = Simulator(P, reference)
x0 = [0.,0]
tspan = (0.0,Tf)
sol = solve(s, x0, tspan, OrdinaryDiffEq.Tsit5())
@test step(P,t)[3] ≈ reduce(hcat,sol.(t))'
end
end