diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index 2cee30ce2b558009dd21f70ccb2cce1712b7cf9d..ba1aa78f5957b3441be9308b42c8205c924385b3 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -6,46 +6,47 @@ matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from kalmanfilter import KalmanFilter
+### is thrown if a Car is too close to its previous platoon member
class UnderCIPDerror(Exception):
pass
class Car(object):
- def __init__(self, prev, speed, position, IPD, PS, tStep):
- self.plattonPrev = prev
+ def __init__(self, prev, speed, position, IPD, PS, tps):
+ 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:
- dt = 1/tStep
+ dt = 1/tps
x = np.array([[0,0]]).T
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]])
Q = np.array([[1,0],[0,1]])
H = np.array([[1,0]])
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.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.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
self.SpeedSystemUnc = random.uniform(-RAND,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.setSpeed(speed)
@@ -59,7 +60,9 @@ class Car(object):
def getSpeed(self):
if self.cSpeed <= 1e-8:
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):
if value > 1e-8:
@@ -76,8 +79,8 @@ class Car(object):
except AttributeError:
setLater = True
- if not self.plattonPrev == None:
- self.dist = self.SonarSystemUnc + (self.plattonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
+ if not self.platoonPrev == None:
+ self.dist = self.SonarSystemUnc + (self.platoonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
else:
self.dist = sys.maxsize
@@ -86,11 +89,7 @@ class Car(object):
return self.dist
def updatePos(self):
- self.cPos += self.cSpeed/float(tStep)
-
- def estimateVeloVA(self):
- if not self.plattonPrev == None:
- return self.getSpeed() + (self.dist - self.oldDist)*self.tStep
+ self.cPos += self.cSpeed/float(tps)
def updateSpeed(self):
if not self.bcIPD:
@@ -100,15 +99,12 @@ class Car(object):
PS = self.PS
v_new = None
- if not self.plattonPrev == None:
- #self.v_v = self.estimateVeloVA()
- #v_v = self.v_v
-
+ if not self.platoonPrev == None:
self.KF.update(np.array([[self.getDistance()]]))
self.KF.predict()
x = self.KF.getValue()
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)
@@ -145,9 +141,13 @@ class Car(object):
return v_new
def computeNewSpeed_2(self, d, v, v_v, IPD, PS):
+ # Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
DIST_POW = 2
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
@@ -166,8 +166,6 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
def update(carList):
for item in carList:
- #if not item.plattonPrev == None:
- #item.KF.resetCov()
item.updatePos()
for item in reversed(carList):
item.updateSpeed()
@@ -176,17 +174,15 @@ if __name__ == "__main__":
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)
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)
pos = [90, 30, 0] # startvektor (Startposition)
- tStep = 50
+ tps = 60
cars = []
- cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tStep))
- cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tStep))
- #cars.append(Car(cars[1], v[2], pos[2], 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],tps))
+ cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tps))
t = []
mesIPD= []
@@ -197,7 +193,7 @@ if __name__ == "__main__":
y_cSpeed = [[],[],[]]
y_dist = [[],[],[]]
y_cDist = [[],[],[]]
- y_cdist = [[],[],[]]
+ y_cIPD = [[],[],[]]
y_v_v = [[],[],[]]
i = 0.0; end = False
@@ -220,9 +216,9 @@ if __name__ == "__main__":
for k in range(1, len(cars)):
y_dist[k].append(cars[k].getDistance())
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:
update(cars)
except UnderCIPDerror:
@@ -232,29 +228,41 @@ if __name__ == "__main__":
if end: break
- plt.subplot(131)
- plt.plot(t, y_pos[0], 'r')
- plt.plot(t, y_pos[1], 'b')
- # plt.plot(t, y_pos[2], 'g')
+ plt.subplot(221)
+ plt.plot(t, y_pos[0], 'r', label='pos LV')
+ plt.plot(t, y_pos[1], 'b', label='pos FV1')
+ plt.plot(t, y_pos[2], 'g', label='pos FV2')
plt.title("Position")
+ plt.legend()
- plt.subplot(132)
- plt.plot(t, mesPS, 'm--')
- plt.plot(t, y_speed[0], 'r--')
- plt.plot(t, y_cSpeed[0], 'r-')
- plt.plot(t, y_speed[1], 'b--')
- plt.plot(t, y_cSpeed[1], 'b')
- plt.plot(t, y_v_v[1], 'g')
- # plt.plot(t, y_speed[2], 'g')
+ plt.subplot(222)
+ plt.plot(t, mesIPD, 'r--', label='IPD')
+ plt.plot(t, y_dist[1], 'b-', label='assumed dist FV1')
+ plt.plot(t, y_cDist[1], 'b--', label='real dist FV1')
+ plt.plot(t, y_cIPD[1], 'b-.', label='cIPD')
+ plt.plot(t, y_dist[2], 'g-', label='assumed dist FV1')
+ plt.plot(t, y_cDist[2], 'g--', label='real dist FV1')
+ 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.legend()
- plt.subplot(133)
- plt.plot(t, mesIPD, 'r--')
- plt.plot(t, y_dist[1], 'b-')
- plt.plot(t, y_cdist[1], 'b--')
- plt.plot(t, y_cDist[1], 'b--')
- # plt.plot(t, y_dist[2], 'g-')
- # plt.plot(t, y_cdist[2], 'g--')
- plt.title("Distance")
+ plt.subplot(224)
+ plt.plot(t, mesPS, 'm--', label='PS')
+ # plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
+ plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
+ plt.plot(t, y_v_v[2], 'g:', label='speed estiamtion FV2 for FV1')
+ # plt.plot(t, y_speed[2], 'g-', label='assumed speed FV1')
+ # plt.plot(t, y_cSpeed[2], 'g--', label='real speed FV1')
+ plt.legend()
plt.show()