Skip to content
Snippets Groups Projects
Commit b7e3a7f9 authored by Franz Bethke's avatar Franz Bethke
Browse files

Start cleanup of CACC sim

parent fe1b71f0
Branches CACC-uncertainty
No related merge requests found
Hide whitespace changes
Inline Side-by-side
modules/CACC/CACC-Module-Test.py
+ 69
− 61
View file @ b7e3a7f9
...@@ -6,46 +6,47 @@ matplotlib.use('TkAgg') ...@@ -6,46 +6,47 @@ matplotlib.use('TkAgg')
from matplotlib import pyplot as plt from matplotlib import pyplot as plt
from kalmanfilter import KalmanFilter from kalmanfilter import KalmanFilter
### is thrown if a Car is too close to its previous platoon member
class UnderCIPDerror(Exception): class UnderCIPDerror(Exception):
pass pass
class Car(object): class Car(object):
def __init__(self, prev, speed, position, IPD, PS, tStep): def __init__(self, prev, speed, position, IPD, PS, tps):
self.plattonPrev = prev self.platoonPrev = prev
self.cPos = position
self.IPD = IPD
self.PS = PS
self.tps = tps
self.bcIPD = False
# initialise calman filter
if not prev == None: if not prev == None:
dt = 1/tStep dt = 1/tps
x = np.array([[0,0]]).T x = np.array([[0,0]]).T
P = np.array([[10,0],[0,10]]) P = np.array([[10,0],[0,10]])
# f(x,v,a) =
# x = f1(x,v,a) = x + v*dt + a * dt**2
# v = f2(x,v,a) = v + a*dt
# a = f3(x,v,a) = a
F = np.array([[1,dt],[0,1]]) F = np.array([[1,dt],[0,1]])
Q = np.array([[1,0],[0,1]]) Q = np.array([[1,0],[0,1]])
H = np.array([[1,0]]) H = np.array([[1,0]])
R = np.array([[5]]) R = np.array([[5]])
self.KF = KalmanFilter(x, P, F, Q, H, R, tStep) self.KF = KalmanFilter(x, P, F, Q, H, R, tps)
self.KF.predict() self.KF.predict()
self.cPos = position
self.IPD = IPD
self.PS = PS
self.tStep = tStep
self.bcIPD = False self.dIdx = 0
self.distHistLength = 15
self.ds = np.empty(self.distHistLength)
RAND = 5 RAND = 0.5
self.SonarSystemUnc = random.uniform(-RAND,RAND) # in cm self.SonarSystemUnc = random.uniform(-RAND,RAND) # in cm
self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds
self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
self.setSpeed(speed) self.setSpeed(speed)
...@@ -59,7 +60,9 @@ class Car(object): ...@@ -59,7 +60,9 @@ class Car(object):
def getSpeed(self): def getSpeed(self):
if self.cSpeed <= 1e-8: if self.cSpeed <= 1e-8:
return 0 return 0
return self.SpeedSystemUnc + self.cSpeed + self.SpeedStatisticalUnc() self.dIdx = (self.dIdx + 1) % self.distHistLength
self.ds[self.dIdx] = self.SpeedSystemUnc + self.cSpeed + self.SpeedStatisticalUnc()
return np.median(self.ds)
def setSpeed(self,value): def setSpeed(self,value):
if value > 1e-8: if value > 1e-8:
...@@ -76,8 +79,8 @@ class Car(object): ...@@ -76,8 +79,8 @@ class Car(object):
except AttributeError: except AttributeError:
setLater = True setLater = True
if not self.plattonPrev == None: if not self.platoonPrev == None:
self.dist = self.SonarSystemUnc + (self.plattonPrev.cPos - self.cPos) + self.SonarStatisticalUnc() self.dist = self.SonarSystemUnc + (self.platoonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
else: else:
self.dist = sys.maxsize self.dist = sys.maxsize
...@@ -86,11 +89,7 @@ class Car(object): ...@@ -86,11 +89,7 @@ class Car(object):
return self.dist return self.dist
def updatePos(self): def updatePos(self):
self.cPos += self.cSpeed/float(tStep) self.cPos += self.cSpeed/float(tps)
def estimateVeloVA(self):
if not self.plattonPrev == None:
return self.getSpeed() + (self.dist - self.oldDist)*self.tStep
def updateSpeed(self): def updateSpeed(self):
if not self.bcIPD: if not self.bcIPD:
...@@ -100,15 +99,12 @@ class Car(object): ...@@ -100,15 +99,12 @@ class Car(object):
PS = self.PS PS = self.PS
v_new = None v_new = None
if not self.plattonPrev == None: if not self.platoonPrev == None:
#self.v_v = self.estimateVeloVA()
#v_v = self.v_v
self.KF.update(np.array([[self.getDistance()]])) self.KF.update(np.array([[self.getDistance()]]))
self.KF.predict() self.KF.predict()
x = self.KF.getValue() x = self.KF.getValue()
d = x[0,0] d = x[0,0]
self.v_v = v_v = v + x[1,0] self.v_v = v_v = max(v + x[1,0], 0)
v_new = self.computeNewSpeed_2(d, v, v_v, IPD, PS) v_new = self.computeNewSpeed_2(d, v, v_v, IPD, PS)
...@@ -145,9 +141,13 @@ class Car(object): ...@@ -145,9 +141,13 @@ class Car(object):
return v_new return v_new
def computeNewSpeed_2(self, d, v, v_v, IPD, PS): def computeNewSpeed_2(self, d, v, v_v, IPD, PS):
# Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
DIST_POW = 2 DIST_POW = 2
SPEED_POW = 0.5 SPEED_POW = 0.5
v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll if abs(v_v) > 1e-8:
v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
else:
v_new = (d - IPD)/IPD
return v_new return v_new
...@@ -166,8 +166,6 @@ def broadcastPlattonSettings(carList, newIPD, newPS): ...@@ -166,8 +166,6 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
def update(carList): def update(carList):
for item in carList: for item in carList:
#if not item.plattonPrev == None:
#item.KF.resetCov()
item.updatePos() item.updatePos()
for item in reversed(carList): for item in reversed(carList):
item.updateSpeed() item.updateSpeed()
...@@ -176,17 +174,15 @@ if __name__ == "__main__": ...@@ -176,17 +174,15 @@ if __name__ == "__main__":
maxT = [20, 40, 80, 110, 130,150,170] # times for events maxT = [20, 40, 80, 110, 130,150,170] # times for events
setIPD = [40, 40, 40, 60, 45, 30, 15] # event of IPD (activ until time event) setIPD = [40, 40, 40, 60, 45, 30, 15] # event of IPD (activ until time event)
setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event) setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event)
# setIPD = [40, 40, 40, 40, 40, 40, 40] # event of IPD (activ until time event)
# setPS = [20, 20, 20, 20, 20, 20, 20] # event of PS (activ until time event)
v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit) v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
pos = [90, 30, 0] # startvektor (Startposition) pos = [90, 30, 0] # startvektor (Startposition)
tStep = 50 tps = 60
cars = [] cars = []
cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tStep)) cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tps))
cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tStep)) cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tps))
#cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tStep)) cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tps))
t = [] t = []
mesIPD= [] mesIPD= []
...@@ -197,7 +193,7 @@ if __name__ == "__main__": ...@@ -197,7 +193,7 @@ if __name__ == "__main__":
y_cSpeed = [[],[],[]] y_cSpeed = [[],[],[]]
y_dist = [[],[],[]] y_dist = [[],[],[]]
y_cDist = [[],[],[]] y_cDist = [[],[],[]]
y_cdist = [[],[],[]] y_cIPD = [[],[],[]]
y_v_v = [[],[],[]] y_v_v = [[],[],[]]
i = 0.0; end = False i = 0.0; end = False
...@@ -220,9 +216,9 @@ if __name__ == "__main__": ...@@ -220,9 +216,9 @@ if __name__ == "__main__":
for k in range(1, len(cars)): for k in range(1, len(cars)):
y_dist[k].append(cars[k].getDistance()) y_dist[k].append(cars[k].getDistance())
y_cDist[k].append(cars[k-1].cPos - cars[k].cPos) y_cDist[k].append(cars[k-1].cPos - cars[k].cPos)
y_cdist[k].append(cars[k].getCIPD()) y_cIPD[k].append(cars[k].getCIPD())
i = i+1/tStep i = i+1/tps
try: try:
update(cars) update(cars)
except UnderCIPDerror: except UnderCIPDerror:
...@@ -232,29 +228,41 @@ if __name__ == "__main__": ...@@ -232,29 +228,41 @@ if __name__ == "__main__":
if end: break if end: break
plt.subplot(131) plt.subplot(221)
plt.plot(t, y_pos[0], 'r') plt.plot(t, y_pos[0], 'r', label='pos LV')
plt.plot(t, y_pos[1], 'b') plt.plot(t, y_pos[1], 'b', label='pos FV1')
# plt.plot(t, y_pos[2], 'g') plt.plot(t, y_pos[2], 'g', label='pos FV2')
plt.title("Position") plt.title("Position")
plt.legend()
plt.subplot(132) plt.subplot(222)
plt.plot(t, mesPS, 'm--') plt.plot(t, mesIPD, 'r--', label='IPD')
plt.plot(t, y_speed[0], 'r--') plt.plot(t, y_dist[1], 'b-', label='assumed dist FV1')
plt.plot(t, y_cSpeed[0], 'r-') plt.plot(t, y_cDist[1], 'b--', label='real dist FV1')
plt.plot(t, y_speed[1], 'b--') plt.plot(t, y_cIPD[1], 'b-.', label='cIPD')
plt.plot(t, y_cSpeed[1], 'b') plt.plot(t, y_dist[2], 'g-', label='assumed dist FV1')
plt.plot(t, y_v_v[1], 'g') plt.plot(t, y_cDist[2], 'g--', label='real dist FV1')
# plt.plot(t, y_speed[2], 'g') plt.plot(t, y_cIPD[2], 'g-.', label='cIPD')
plt.title("Distance")
plt.legend()
plt.subplot(223)
plt.plot(t, mesPS, 'm--', label='PS')
# plt.plot(t, y_speed[0], 'r-', label='assumed speed LV')
plt.plot(t, y_cSpeed[0], 'r--', label='real speed LV')
plt.plot(t, y_v_v[1], 'b:', label='speed estiamtion FV1 for LV')
# plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
# plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
plt.title("Speed") plt.title("Speed")
plt.legend()
plt.subplot(133) plt.subplot(224)
plt.plot(t, mesIPD, 'r--') plt.plot(t, mesPS, 'm--', label='PS')
plt.plot(t, y_dist[1], 'b-') # plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
plt.plot(t, y_cdist[1], 'b--') plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
plt.plot(t, y_cDist[1], 'b--') plt.plot(t, y_v_v[2], 'g:', label='speed estiamtion FV2 for FV1')
# plt.plot(t, y_dist[2], 'g-') # plt.plot(t, y_speed[2], 'g-', label='assumed speed FV1')
# plt.plot(t, y_cdist[2], 'g--') # plt.plot(t, y_cSpeed[2], 'g--', label='real speed FV1')
plt.title("Distance") plt.legend()
plt.show() plt.show()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment