diff --git a/modules/CACC/.kalmanfilter.py.swp b/modules/CACC/.kalmanfilter.py.swp
deleted file mode 100644
index af88080ce0232adb046cd99d6a41b2aed11b01e6..0000000000000000000000000000000000000000
Binary files a/modules/CACC/.kalmanfilter.py.swp and /dev/null differ
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index 1e2c89bc4fa08323ca7d45ad915cf1ab7c75a631..19dae0ba2cff19f058cfa83576ba672ec75b6d55 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -15,13 +15,18 @@ class Car(object):
self.plattonPrev = prev
if not prev == None:
dt = 1/tStep
- x = np.array([[0,0,0]]).T
- P = np.array([[100,0,0],[0,100,0],[0,0,100]])
+ x = np.array([[0,0]]).T
+ P = np.array([[10,0],[0,10]])
- F = np.array([[1,0,1],[1,0,0],[dt, -dt, 0]])
- Q = np.array([[1,0,0],[0,1,0],[0,0,1]])
- H = np.array([[1,0,0]])
- R = np.array([[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)
@@ -40,11 +45,8 @@ class Car(object):
self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds
- # self.SpeedSystemUnc = random.uniform(0,0) # in meter per seconds
- # self.SpeedStatisticalUnc = lambda : random.gauss(0,0) # in meter per seconds
self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
- # self.EngineSystemUnc = random.uniform(0,0) # in meter per seconds
self.setSpeed(speed)
self.getDistance()
@@ -106,7 +108,7 @@ class Car(object):
self.KF.predict()
x = self.KF.getValue()
d = x[0,0]
- self.v_v = v_v = v + x[2,0]*self.tStep
+ self.v_v = v_v = v + x[1,0]
if checkInbound(d, IPD, 0.01*IPD):
if checkInbound (v, v_v, 0.01*v_v):
if checkInbound(v, PS, 0.01*PS):
@@ -130,7 +132,6 @@ class Car(object):
if self.getCIPD() > d:
print("raise")
raise UnderCIPDerror("under CIPD!")
- #self.KF.predict()
else: # this is for the LV
c = 0.5
@@ -153,8 +154,8 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
def update(carList):
for item in carList:
- if not item.plattonPrev == None:
- item.KF.resetCov()
+ #if not item.plattonPrev == None:
+ #item.KF.resetCov()
item.updatePos()
for item in reversed(carList):
item.updateSpeed()
@@ -168,7 +169,7 @@ if __name__ == "__main__":
v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
pos = [90, 30, 0] # startvektor (Startposition)
- tStep = 1000
+ tStep = 50
cars = []
cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tStep))
diff --git a/modules/CACC/kalmanfilter.py b/modules/CACC/kalmanfilter.py
index 0c8de9847579457381f501e296069538453b5f53..fc9d0dc4e515432ad28246ff9bad1ce97d767d0e 100644
--- a/modules/CACC/kalmanfilter.py
+++ b/modules/CACC/kalmanfilter.py
@@ -44,11 +44,7 @@ class KalmanFilter(object):
# Kalman - Gain
K = np.dot(np.dot(self.P, self.H.T), np.linalg.pinv(S))
- # Zweite Zeile 0.0 setzen, damit sie den alten Abstand nicht verändert!!!
- # !!!
- K[1,0] = 0.0
- # !!!
-
+ # 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
diff --git a/modules/CACC/kfConfig.py b/modules/CACC/kfConfig.py
deleted file mode 100644
index d43a0e1dfe3c614512d118d9c4353bdda310079f..0000000000000000000000000000000000000000
--- a/modules/CACC/kfConfig.py
+++ /dev/null
@@ -1,7 +0,0 @@
-import numpy as np
-
-# Zustandsvektor [d_c, d_c-1, v_r]
-x = np.array([[0, 0, 0, 0]]).T
-
-# Kovarianzmatrix (hohe Varianzen fuer schnelle Aenderungen)
-P = np.array([[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]])