Newer
Older
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
145
146
147
148
149
150
151
152
153
from scipy import signal
from scipy import sqrt, linspace, exp, pi, polymul, sinh, cosh, arange
from scipy import real, array, eye, kron, diag, sparse
from numpy.linalg import eig
class CFM:
# General car following model class
def go(self, re):
# Compute linearization about equilibrium point re = ve / v0 :
# v_e, h_e : equilibrium velocity and headway
# F, G, H : linearized gains
# then generate state-space matrices and transfer function
self.re = re
self._linearize()
self._mn()
self._sys()
def _linearize(self):
# generate F, G, H linearized gains
raise NotImplementedError
def _mn(self):
# use F, G, H linearization to create M, N state-space matrices
self.m = array([[0,1],[-self.f, -self.g-self.h]])
self.n = array([[0,0],[self.f, self.g]])
def _sys(self):
# use F, G, H linearization to create transfer function
self.sys = signal.TransferFunction([self.g, self.f], [1, self.g+self.h, self.f])
def sysn(self, cnt):
# use F, G, H linearization to create head-to-tail transfer function
# for cnt cars in a row
if cnt == 0:
return signal.TransferFunction([1],[1])
f,g,h = self.f, self.g, self.h
num = [g, f]
den = [1, h+g, f]
for i in range(cnt-1):
num = polymul(num, [g, f])
den = polymul(den, [1, h+g, f])
sys = signal.TransferFunction(num, den)
return sys
def loop(self, cnt):
m = self.m
n = self.n
a = kron(eye(cnt), m) + \
kron(diag([1]*(cnt-1), -1) + \
diag([1], cnt-1), n)
'''
a = sparse.kron(sparse.identity(cnt), m) + \
sparse.kron(sparse.diags(([1]*(cnt-1), [1]), (-1, cnt-1)), n)
'''
return a
def loopWithRobot(self, cnt):
m = self.m
n = self.n
cnt -= 2
a = kron(eye(cnt+2), m) + \
kron(diag([1]*(cnt-1) + [1/2., 1], -1) + \
diag([0]* cnt + [1/2.] , 1) + \
diag([1], cnt+1), n)
return a
def loopStablity(self, cnt):
a = self.loop(cnt)
w, _ = eig(a)
# w,v = sparse.linalg.eigs(a)
return max(real(w))
def tfStability(self):
_, mag, _ = self.sys.bode()
return max(mag)
class Wang(CFM):
def __init__(self, kd, kv):
self.kd = kd
self.kv = kv
def _linearize(self):
self.ve = 0
self.he = 0
self.f = self.kd
self.g = self.kv
self.h = 0
class IDM(CFM):
D = 4
def __init__(self, t, a, b, s0, v0):
self.t = float(t)
self.a = float(a)
self.b = float(b)
self.s0 = float(s0)
self.v0 = float(v0)
def _linearize(self):
t = self.t
a = self.a
b = self.b
d = self.D
s0 = self.s0
v0 = self.v0
re = self.re
r1 = (1-re**d)
r32 = r1**(1.5)
ge = s0 + t * v0 * re
self.ve = re * v0
self.he = ge * r1**(-0.5)
self.f = 1*a*r32/ge
self.g = v0 * sqrt(a/b) * re * r1/ge
self.h = a * d * re**(d-1)/v0 + 2*a*t*r1/ge
def plotSweep(fn, rngs, axis=None, separate=True, logx=False):
if logx:
plot = plt.semilogx
else:
plot = plt.plot
defaults = [x["default"] for x in rngs]
if not separate:
plot(*fn(*defaults, fmt='k'))
for i, r in enumerate(rngs):
if "values" in r and r["values"] is not None:
if separate:
plot(*fn(*defaults, fmt='k'))
print r["name"], defaults[i], r["values"]
args = list(defaults)
for dot, arg in zip(r["dots"], r["values"]):
args[i] = arg
plot(*fn(*args, fmt=r["color"]+dot))
if axis is not None:
plt.axis(axis)
if separate:
plt.show()
if not separate:
plt.show()