diff --git a/.gitignore b/.gitignore
index f09c3f70a70b8d49563cefbe57dcb2825aa41316..badcad4dc02c5bc8c33679972672151b9a7e0e42 100644
--- a/.gitignore
+++ b/.gitignore
@@ -43,4 +43,6 @@
doc/**/*.pdf
*.synctex.gz
+*.swp
testenv/
+__pycache__
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index e2c8c8db58e4aaad8bd6e9f3cc0e01a818baf418..ba1aa78f5957b3441be9308b42c8205c924385b3 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -1,96 +1,163 @@
+import numpy as np
import random
+import sys
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
+from kalmanfilter import KalmanFilter
-class Car(object):
- cSpeed = None
- cPos = None
- cDist = None
+### is thrown if a Car is too close to its previous platoon member
+class UnderCIPDerror(Exception):
+ pass
- cIPD = None
- IPD = None
- PS = None
+class Car(object):
- def __init__(self, prev, speed, position, IPD, PS):
- self.plattonPrev = prev
- self.cSpeed = speed
+ def __init__(self, prev, speed, position, IPD, PS, tps):
+ self.platoonPrev = prev
self.cPos = position
- self.updateDis()
- self.cIPD = 0.01*IPD # noch keine cIPD definition existent
- self.IPD = IPD
- self.PS = PS
-
+ self.IPD = IPD
+ self.PS = PS
+ self.tps = tps
+
self.bcIPD = False
+
+ # initialise calman filter
+ if not prev == None:
+ dt = 1/tps
+ x = np.array([[0,0]]).T
+ P = np.array([[10,0],[0,10]])
+
+ 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, tps)
+
+ self.KF.predict()
+
+ self.dIdx = 0
+ self.distHistLength = 15
+ self.ds = np.empty(self.distHistLength)
+
+ 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)
+ self.getDistance()
+ self.v_v = self.cSpeed
return None
def getCIPD(self):
- return self.cIPD
+ return 0.1*self.IPD
- def getPos(self):
- return self.cPos
-
def getSpeed(self):
- return self.cSpeed
-
+ if self.cSpeed <= 1e-8:
+ return 0
+ 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:
+ self.cSpeed = value
+ self.speed = self.EngineSystemUnc + value
+ else:
+ self.cSpeed = 0
+ self.speed = 0
+
def getDistance(self):
- return self.cDist
+ setLater = False
+ try:
+ self.oldDist = self.dist
+ except AttributeError:
+ setLater = True
+
+ if not self.platoonPrev == None:
+ self.dist = self.SonarSystemUnc + (self.platoonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
+ else:
+ self.dist = sys.maxsize
+
+ if setLater:
+ self.oldDist = self.dist
+ return self.dist
def updatePos(self):
- self.cPos = self.cPos + self.cSpeed/16 #Ticklaenge. Kann zusammen mit dem i in den Updates angepasst werden. Das Produkt beider muss 1 sein
-
- def updateDis(self):
- if not self.plattonPrev == None:
- self.cDist = - self.cPos + self.plattonPrev.getPos()#* random.uniform(0.999, 1.001) # bis zu 0.001 relative Abweichung auf die Entfernungsmessung
+ self.cPos += self.cSpeed/float(tps)
def updateSpeed(self):
if not self.bcIPD:
- d_c = self.cDist
- v_c = self.cSpeed
+ d = self.getDistance() # sensor data
+ v = self.getSpeed() # sesnor data
IPD = self.IPD
- PS = self.PS
- v_n = None
-
- if not self.plattonPrev == None:
- v_v = self.plattonPrev.getSpeed() #* random.uniform(0.9, 1.1) # bis zu 0.1 relative Abweichung auf die Geschwindigkeitsmessung
- if self.checkInbound(d_c, IPD, 0.01*IPD):
- if self.checkInbound (v_c, v_v, 0.01*v_v):
- if self.checkInbound(v_c, PS, 0.01*PS):
- v_n = PS
- else:
- if v_c > PS:
- v_n = v_v - abs(PS-v_v)/4
- else:
- v_n = v_v + abs(PS-v_v)/4
- else:
- if v_c > v_v:
- v_n = v_v - abs(PS-v_v)/4
- else:
- v_n = v_v + abs(PS-v_v)/4
- else:
- v_n = min((v_v * (d_c/IPD)**2), v_c*1.1) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ PS = self.PS
+ v_new = None
+
+ 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 = max(v + x[1,0], 0)
+
+ v_new = self.computeNewSpeed_2(d, v, v_v, IPD, PS)
inertia = 0.8 # in [0,1] und gibt die Traegheit des Fahrzeuges an
- self.cSpeed = inertia * v_c + (1- inertia) * v_n
+ self.setSpeed(inertia * v + (1- inertia) * v_new)
+
+ if self.getCIPD() > d:
+ print("raise")
+ raise UnderCIPDerror("under CIPD!")
+
+ else: # this is for the LV
+ c = 0.5
+ self.v_v = 0
+ self.setSpeed(c * v + (1-c) * PS)
- self.cIPD = self.IPD * 0.01 #too low
- if self.cIPD > self.cDist:
- self.cSpeed = 0
- self.bcIPD = True
- self.cIPD = -10 #Zur Visualisierung
+
+ def computeNewSpeed_1(self, d, v, v_v, IPD, PS):
+ if checkInbound(d, IPD, 0.01*IPD):
+ if checkInbound (v, v_v, 0.01*v_v):
+ if checkInbound(v, PS, 0.01*PS):
+ v_new = PS
+ else:
+ if v > PS:
+ v_new = v_v - abs(PS-v_v)/4
+ else:
+ v_new = v_v + abs(PS-v_v)/4
else:
- v_v = self.cSpeed
- c = 0.8
- self.cSpeed = c * v_c + (1-c) * PS
-
- def checkInbound(self, argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
- if argument > target + delta:
- return False
- elif argument < target - delta:
- return False
+ if v > v_v:
+ v_new = v_v - abs(PS-v_v)/4
+ else:
+ v_new = v_v + abs(PS-v_v)/4
+ else:
+ v_new = (v_v * (d/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ 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
+ if abs(v_v) > 1e-8:
+ v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
else:
- return True
+ v_new = (d - IPD)/IPD
+ return v_new
+
+def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+ if argument > target + delta:
+ return False
+ elif argument < target - delta:
+ return False
+ else:
+ return True
def broadcastPlattonSettings(carList, newIPD, newPS):
for item in carList:
@@ -100,7 +167,6 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
def update(carList):
for item in carList:
item.updatePos()
- item.updateDis()
for item in reversed(carList):
item.updateSpeed()
@@ -110,12 +176,13 @@ if __name__ == "__main__":
setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event)
v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
- pos = [70, 30, 0] # startvektor (Startposition)
+ pos = [90, 30, 0] # startvektor (Startposition)
+ tps = 60
cars = []
- cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0]))
- cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0]))
- cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0]))
+ 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= []
@@ -123,10 +190,13 @@ if __name__ == "__main__":
y_pos = [[],[],[]]
y_speed = [[],[],[]]
+ y_cSpeed = [[],[],[]]
y_dist = [[],[],[]]
- y_cdist = [[],[],[]]
+ y_cDist = [[],[],[]]
+ y_cIPD = [[],[],[]]
+ y_v_v = [[],[],[]]
- i = 0.0
+ i = 0.0; end = False
for j in range(0, len(maxT)):
IPD = setIPD[j]
PS = setPS[j]
@@ -138,39 +208,61 @@ if __name__ == "__main__":
mesPS.append(PS)
for k in range(0, len(cars)):
- y_pos[k].append(cars[k].getPos())
+ y_pos[k].append(cars[k].cPos)
y_speed[k].append(cars[k].getSpeed())
+ y_cSpeed[k].append(cars[k].cSpeed)
+ y_v_v[k].append(cars[k].v_v)
for k in range(1, len(cars)):
y_dist[k].append(cars[k].getDistance())
- y_cdist[k].append(cars[k].getCIPD())
+ y_cDist[k].append(cars[k-1].cPos - cars[k].cPos)
+ y_cIPD[k].append(cars[k].getCIPD())
- i = i+0.0625
- update(cars)
+ i = i+1/tps
+ try:
+ update(cars)
+ except UnderCIPDerror:
+ print("catch")
+ end = True
+ break
+ if end: break
- plt.subplot(131)
- plt.plot(t, y_pos[0], 'r', label='LV')
- plt.plot(t, y_pos[1], 'b', label='FV 1')
- plt.plot(t, y_pos[2], 'g', label='FV 2')
- plt.legend()
+ 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, 'r--', label='PS')
- plt.plot(t, y_speed[0], 'r-', label='LV')
- plt.plot(t, y_speed[1], 'b-', label='FV 1')
- plt.plot(t, y_speed[2], 'g-', label='FV 2')
+ 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--', label='IPD')
- plt.plot(t, y_dist[1], 'b-', label='FV1 - LV - Distance')
- plt.plot(t, y_cdist[1], 'b--', label='FV1 - cIPD)')
- plt.plot(t, y_dist[2], 'g-', label='FV2 - FV1 - Distance')
- plt.plot(t, y_cdist[2], 'g--', label='FV2 - cIPD')
+ 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.title("Distance")
plt.show()
diff --git a/modules/CACC/kalmanfilter.py b/modules/CACC/kalmanfilter.py
new file mode 100644
index 0000000000000000000000000000000000000000..fc9d0dc4e515432ad28246ff9bad1ce97d767d0e
--- /dev/null
+++ b/modules/CACC/kalmanfilter.py
@@ -0,0 +1,58 @@
+import numpy as np
+
+class KalmanFilter(object):
+ def __init__(self, x, P, F, Q, H, R, ticksPerSecond):
+ self.tps = ticksPerSecond
+ dt = 1/ticksPerSecond
+
+ # Zustandsvektor
+ self.x = x
+
+ # Kovarianzmatrix (hohe Werte fuer schnelle Aenderungen)
+ self.P = P
+ self.originP = P
+
+
+ # Dynamikmatrix
+ self.F = F
+
+ # Prozessrausch-Matrix (Kovarianzmatrix)
+ self.Q = Q
+
+ # Messmatrix um die Messung auf die relavanten Daten zu reduzieren
+ self.H = H
+
+ # Messrausch-Kovarianzmatrix (nur fuer die zu messenden Groessen)
+ self.R = R
+
+ # dynamisch - gross Q, klein R
+ # glaettung - klein Q, gross R
+ return None
+
+ def predict(self):
+ self.x = np.dot(self.F, self.x)
+ self.P = np.add(np.dot(np.dot(self.F, self.P), self.F.T), self.Q)
+ return None
+
+ def update(self, mesVec):
+ # Innovation
+ y = np.add(mesVec.T, - np.dot(self.H, self.x))
+
+ # Residualkovarianz
+ S = np.add(np.dot(np.dot(self.H, self.P), self.H.T), self.R)
+
+ # Kalman - Gain
+ K = np.dot(np.dot(self.P, self.H.T), np.linalg.pinv(S))
+
+ # update the predecited values
+ self.x = np.add(self.x, np.dot(K, y))
+ self.P = np.add(self.P, - np.dot(np.dot(K,S),K.T))
+ return None
+
+ def resetCov(self):
+ self.P = self.originP
+ return None
+
+
+ def getValue(self):
+ return self.x
diff --git a/modules/CACC/platoonSimulation.py b/modules/CACC/platoonSimulation.py
new file mode 100644
index 0000000000000000000000000000000000000000..367bca46a7ace3505d053650b6a0933546f6f5c3
--- /dev/null
+++ b/modules/CACC/platoonSimulation.py
@@ -0,0 +1,248 @@
+import sys
+import IPython
+import bisect
+import time
+import math
+import random
+import collections
+
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+class CrashException(Exception):
+ pass
+
+class Vehicle(object):
+ def __init__(self,name,typ,road,initPos,initVelo):
+
+ self.name = name
+ self.typ = typ
+ self.road = road
+
+ self.velos = []
+ self.posis = []
+ self.dists = []
+ self.velosVA = []
+
+ self.posi = 1.0*initPos # to make it a float value
+ if self.typ == "stone":
+ self.velo = 0
+ else:
+ self.velo = initVelo
+
+ self.engInAcc = (100 + random.randint(-3,3))/100.0 # constant inaccuracy for my own engine
+ self.sensorInAcc = (100 + random.randint(-3,3))/100.0
+
+ @property
+ def velo(self):
+ return self.velos[-1]
+ @velo.setter
+ def velo(self,value):
+ self.velos.append(value)
+
+ @property
+ def posi(self):
+ return self.posis[-1]
+ @posi.setter
+ def posi(self,value):
+ self.posis.append(value)
+
+ @property
+ def dist(self):
+ return self.dists[-1]
+ @dist.setter
+ def dist(self,value):
+ self.dists.append(value)
+
+ @property
+ def veloVA(self):
+ return self.velosVA[-1]
+ @veloVA.setter
+ def veloVA(self,value):
+ self.velosVA.append(value)
+
+ def __str__(self):
+ retStr = "---"
+ retStr += self.name + ".posi: " + str(self.posi) + "\n"
+ retStr += self.name + ".velo: " + str(self.velo) + "\n"
+ retStr += self.name + ".dist: " + str(self.dist) + "\n"
+ retStr += self.name + ".veloVA: " + str(self.veloVA) + "\n"
+ retStr += self.name + ".engInAcc: " + str(self.engInAcc) + "\n"
+ retStr += self.name + ".sensorInAcc: " + str(self.sensorInAcc) + "\n"
+ retStr += "---\n"
+ return retStr
+
+ def update(self, tStep):
+
+ oldVelo = self.velo
+
+ # get sensor data
+ self.updateSensorData(tStep)
+
+ # udapte position
+ self.posi += tStep*(oldVelo + self.velo)/2
+
+ # decide on values
+ if self.typ == "fv":
+ if abs(self.dist - IPD) > IPD_TOL:
+ self.velo = self.engInAcc*min(self.veloVA, PS)*(self.dist / IPD)
+ else:
+ self.velo = self.engInAcc*PS
+
+ if self.typ == "lv":
+ if self.dist < IPD:
+ # self.velo = max(self.velo - 1, 0)
+ self.velo = 0
+ else:
+ self.velo = self.engInAcc*PS
+
+ def updateSensorData(self,tStep):
+ try:
+ oldDist = self.dist
+ except IndexError:
+ oldDist = self.sensorInAcc*self.road.getDistance(self)
+
+ self.dist = self.sensorInAcc*self.road.getDistance(self)
+ self.veloVA = self.velo + (self.dist - oldDist)/tStep
+ return
+
+ def __le__(self, other):
+ return self.posi <= other.posi
+
+ def __lt__(self, other):
+ return self.posi < other.posi
+
+
+class Road(object):
+ def __init__(self):
+ self.vehicles = []
+
+ def __str__(self):
+ retStr = ""
+ for vehicle in self.vehicles:
+ retStr += vehicle.__str__()
+ return retStr
+
+ def placeVehicle(self, vehicle):
+ bisect.insort(self.vehicles, vehicle)
+
+ def update(self,tStep):
+ for vIdx, vehicle in enumerate(self.vehicles):
+ vehicle.update(tStep)
+ if vIdx < len(self.vehicles)-1:
+ if vehicle.posi >= self.vehicles[vIdx+1].posi:
+ raise CrashException("Crash!")
+
+ def draw(self):
+ minPosi = self.vehicles[0].posi
+ maxPosi = self.vehicles[-1].posi
+ relPos = [int((vehicle.posi - minPosi)/(maxPosi - minPosi)*100) for vehicle in self.vehicles]
+
+ drawing = ""
+ relDrawProg = 0
+ for vIdx, vehicle in enumerate(self.vehicles):
+ spaceToFill = relPos[vIdx] - relDrawProg
+ distStr = "<" + str(int(self.vehicles[vIdx-1].dist)).zfill(3) + ">"
+ if spaceToFill >= len(distStr):
+ frontspace = int(math.ceil((spaceToFill - len(distStr))/2.0))
+ backspace = int(math.floor((spaceToFill - len(distStr))/2.0))
+ for _ in range(frontspace):
+ drawing += " "; relDrawProg += 1
+ drawing += distStr; relDrawProg += len(distStr)
+ for _ in range(backspace):
+ drawing += " "; relDrawProg += 1
+ else:
+ for _ in range(spaceToFill):
+ drawing += " "
+ relDrawProg += 1
+ drawing += vehicle.name
+ relDrawProg += 1
+
+ print("-----------------------------------------------------------------------------------------------------")
+ print(drawing)
+ print("-----------------------------------------------------------------------------------------------------")
+
+ def getDistance(self,vehicle):
+ vIdx = self.vehicles.index(vehicle)
+ if vIdx == len(self.vehicles)-1:
+ return sys.maxsize
+ else:
+ return self.vehicles[vIdx + 1].posi - self.vehicles[vIdx].posi
+
+ def getVeloVA(self, vehicle):
+ vIdx = self.vehicles.index(vehicle)
+ if vIdx == len(self.vehicles)-1:
+ return sys.maxsize
+ else:
+ return self.vehicles[vIdx + 1].velo
+
+
+# GLOBALS
+IPD = 15*100 # in millimetres
+IPD_TOL = 1*100 # in millimetres
+
+PS = 15 # in millimetres per milliseconds
+PS_TOL = 1 # in millimetres per milliseconds
+
+SENSOR_UPDATE_TIME = .5 # in milliseconds
+
+if __name__ == "__main__":
+
+ tStart = 0; tStep = 1; tEnd = 500
+
+ road = Road()
+ lv = Vehicle("L","lv",road,100*100,15); road.placeVehicle(lv)
+ fv1 = Vehicle("1","fv",road,75*100,20); road.placeVehicle(fv1)
+ # fv2 = Vehicle("2","fv",road,50*100,15); road.placeVehicle(fv2)
+
+ # startStone = Vehicle("s","stone",road,0*100,0); road.placeVehicle(startStone)
+ # endStone = Vehicle("S","stone",road,400*100,0); road.placeVehicle(endStone)
+
+ t = tStart;
+ ts = []
+ while True:
+
+ try:
+ road.update(SENSOR_UPDATE_TIME)
+ except CrashException:
+ print("CRASH!")
+ break
+
+ t += SENSOR_UPDATE_TIME; ts.append(t)
+ # print("t = ", t)
+ # road.draw()
+ # time.sleep(0.2)
+
+ if t > tEnd:
+ break
+ plt.subplot(131)
+ plt.plot([tStart] + ts, lv.posis, 'm')
+ plt.plot([tStart] + ts, fv1.posis, 'g')
+ # plt.plot([tStart] + ts, fv2.posis, 'b')
+ plt.title("Position")
+ plt.ylim(7000,18000)
+
+ plt.subplot(132)
+ plt.plot(ts, [PS for t in ts],'r', ts,[PS-PS_TOL for t in ts],'r--', ts,[PS+PS_TOL for t in ts],'r--')
+ plt.plot([tStart] + ts, lv.velos, 'm--')
+ plt.plot([tStart] + ts, [d/lv.engInAcc for d in lv.velos], 'm')
+ plt.plot([tStart] + ts, fv1.velos, 'g--')
+ plt.plot([tStart] + ts, [d/fv1.engInAcc for d in fv1.velos], 'g')
+ plt.plot(ts, fv1.velosVA, 'm.')
+ # plt.plot([tStart] + ts, fv2.velos, 'b--')
+ # plt.plot([tStart] + ts, [d/fv2.engInAcc for d in fv2.velos], 'b')
+ plt.title("Speed")
+ plt.ylim(13,26)
+
+ plt.subplot(133)
+ plt.plot(ts,[IPD for t in ts],'r', ts,[IPD-IPD_TOL for t in ts],'r--', ts,[IPD+IPD_TOL for t in ts],'r--')
+ plt.plot(ts, fv1.dists, 'g--')
+ plt.plot(ts, [d/fv1.sensorInAcc for d in fv1.dists], 'g')
+ # plt.plot(ts, fv2.dists, 'b--')
+ # plt.plot(ts, [d/fv2.sensorInAcc for d in fv2.dists], 'b')
+ plt.title("Distance")
+ plt.ylim(1300,2600)
+
+ plt.show()
+
diff --git a/modules/CACC/simlifiedSpeedVisualization.py b/modules/CACC/simlifiedSpeedVisualization.py
new file mode 100644
index 0000000000000000000000000000000000000000..95c5a704d71e3b2a6f2285d1d238907f306b9721
--- /dev/null
+++ b/modules/CACC/simlifiedSpeedVisualization.py
@@ -0,0 +1,78 @@
+import numpy as np
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+
+v = 20
+v_v = 0.5
+PS = 20
+IPD = 20
+
+IPD_TOL = 0.05*IPD
+
+DIST_POW = 2
+SPEED_POW = 0.5
+
+ds = np.linspace(0.*IPD, 1.5*IPD, num = 1000)
+v_news = []
+
+def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+ if argument > target + delta:
+ return False
+ elif argument < target - delta:
+ return False
+ else:
+ return True
+
+def variante_1(d):
+ v_new = None
+ if checkInbound(d, IPD, IPD_TOL):
+ if checkInbound (v, v_v, 0.01*v_v):
+ if checkInbound(v, PS, 0.01*PS):
+ v_new = PS
+ else:
+ if v > PS:
+ v_new = v_v - abs(PS-v_v)/4
+ else:
+ v_new = v_v + abs(PS-v_v)/4
+ else:
+ if v > v_v:
+ v_new = v_v - abs(PS-v_v)/4
+ else:
+ v_new = v_v + abs(PS-v_v)/4
+ else:
+ v_new = (v_v * (d/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ return v_new
+
+def variante_2(d):
+ v_new = None
+ if d < IPD - IPD_TOL:
+ v_new = (v_v * (d/(IPD - IPD_TOL))**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ elif d > IPD + IPD_TOL:
+ v_new = (v_v * (d/(IPD + IPD_TOL))**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ else:
+ v_new = v_v
+ return v_new
+
+def variante_3(d):
+ v_new = (v_v * (d/(IPD))**DIST_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ return v_new
+
+def variante_4(d):
+ v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ return v_new
+
+for d in ds:
+ v_new = variante_4(d)
+ v_news.append(v_new)
+
+plt.plot(ds, v_news,'b', label='v_new')
+plt.plot(ds, [v_v for d in ds],'tab:orange', label='v_v')
+plt.plot(ds, [PS for d in ds], 'g', label='PS')
+plt.plot(ds, [v for d in ds], 'b--', label='v')
+plt.plot([IPD, IPD], [0, max(v_news)], 'r', label='IPD')
+plt.plot([IPD - IPD_TOL, IPD - IPD_TOL], [0, max(v_news)], 'r--', label='IPD-')
+plt.plot([IPD + IPD_TOL, IPD + IPD_TOL], [0, max(v_news)], 'r--', label='IPD+')
+plt.legend()
+plt.show()