diff --git a/modules/CACC/car.py b/modules/CACC/car.py
index 62ce856b8747242c36db68e62447cf70bf084c1b..fefed44122d23c5bddde7f0cfc3ea7dd2ef9a739 100644
--- a/modules/CACC/car.py
+++ b/modules/CACC/car.py
@@ -6,18 +6,18 @@ from kalmanfilter import KalmanFilter
class Car(object):
def __init__(self, prev, speed, position, IPD, PS, tps):
- self.platoonPrev = prev # conatining the reference
+ self.platoonPrev = prev # containing the reference
self.cPos = position # inner car values
self.IPD = IPD
self.PS = PS
self.tps = tps
- self.bcIPD = False # below cIPD. If true, sets speed to 0 and skips all further postions and speed updates
+ self.bcIPD = False # below cIPD. If true, sets speed to 0 and skips all further positions and speed updates
# initialise calman filter
if not prev == None:
dt = 1/tps
- # x: current car state x = (distance, velocity)^T. distance means the distance to it's prev platoon member and velocity its relative speed.
+ # x: current car state x = (distance, velocity)^T. Distance means the distance to it's prev platoon member and velocity its relative speed.
x = np.array([[0,0]]).T
# P: covariance matrix
P = np.array([[10,0],[0,10]])
@@ -26,18 +26,20 @@ class Car(object):
F = np.array([[1,dt],[0,1]])
# Q: process error
Q = np.array([[1,0],[0,1]])
- # H: messuare matrix, references the distance
+ # H: measure matrix, references the distance
H = np.array([[1,0]])
- # R: messuarement error
+ # R: measurement error
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)
+ # the sensor data will be median-filtered
+ # therefore a circular buffer is used
+ self.dIdx = 0 # position in the buffer to change next
+ self.distHistLength = 15 # capacity of the buffer
+ self.ds = np.empty(self.distHistLength) # initialise the buffer
# Create randomness for sensors
RAND = 0.5
@@ -77,18 +79,19 @@ class Car(object):
def getDistance(self):
# returns the distance between the car and its previously driving car
- setLater = False
+
+ dist_set = True # assume the distance is known
try:
self.oldDist = self.dist
except AttributeError:
- setLater = True
+ dist_set = False # ... assumtion was false
if not self.platoonPrev == None:
self.dist = self.SonarSystemUnc + (self.platoonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
else:
self.dist = sys.maxsize
- if setLater:
+ if not(dist_set):
self.oldDist = self.dist
return self.dist
diff --git a/modules/CACC/platoonSimulation.py b/modules/CACC/platoonSimulation.py
deleted file mode 100644
index caa315055bad1087b49d955253be44e8518cceaf..0000000000000000000000000000000000000000
--- a/modules/CACC/platoonSimulation.py
+++ /dev/null
@@ -1,247 +0,0 @@
-import sys
-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
index 857cccda1729bb20df5e0b5287c45ce7141f681b..7974cef8b314c5c38e73b29662e09418a782d83c 100644
--- a/modules/CACC/simlifiedSpeedVisualization.py
+++ b/modules/CACC/simlifiedSpeedVisualization.py
@@ -4,21 +4,24 @@ matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
+# This script is used to get an intuition of certain speed controller functions
+# The dependence on the measured distance is investigated here
+
+
+# input to the speed controller:
v = 12
v_v = 10
PS = 10
IPD = 20
+ds = np.linspace(0.*IPD, 1.7*IPD, num = 1000)
IPD_TOL = 0.05*IPD
-DIST_POW = 2
-SPEED_POW = 0.5
-
-ds = np.linspace(0.*IPD, 1.7*IPD, num = 1000)
+# output speed values (2different controllers)
v_newsAdv = []
v_newsLin = []
-def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+def checkInbound(argument, target, delta): # Ueberall ist ein delta von 0.01 hinterlegt
if argument > target + delta:
return False
elif argument < target - delta:
@@ -27,6 +30,8 @@ def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 h
return True
def variante_adv(d):
+ DIST_POW = 2
+ SPEED_POW = 0.5
v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
return v_new
@@ -34,10 +39,12 @@ def variante_lin(d):
v_new = (v_v * (d/(IPD))**1 * (PS/v_v)**0.5)
return v_new
+# compute all values
for d in ds:
v_newsAdv.append(variante_adv(d))
v_newsLin.append(variante_lin(d))
+# plot
plt.plot(ds, v_newsAdv,'b-', label='v_new (exp)')
plt.plot(ds, v_newsLin,'b:', label='v_new (lin)')
plt.plot(ds, [v_v for d in ds],'tab:orange', label='v_v')