diff --git a/modules/CACC/.kalmanfilter.py.swp b/modules/CACC/.kalmanfilter.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..8c975bcc3f880ec00370de16665fd8fbb3d0897c
Binary files /dev/null and b/modules/CACC/.kalmanfilter.py.swp differ
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index e2f631fa5553de5cd237c223c70572d987436ab6..1e2c89bc4fa08323ca7d45ad915cf1ab7c75a631 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -1,9 +1,10 @@
+import numpy as np
import random
-import IPython
import sys
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
+from kalmanfilter import KalmanFilter
class UnderCIPDerror(Exception):
pass
@@ -12,6 +13,19 @@ class Car(object):
def __init__(self, prev, speed, position, IPD, PS, tStep):
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]])
+
+ 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]])
+
+ self.KF = KalmanFilter(x, P, F, Q, H, R, tStep)
+
+ self.KF.predict()
self.cPos = position
self.IPD = IPD
self.PS = PS
@@ -38,7 +52,7 @@ class Car(object):
return None
def getCIPD(self):
- return 0.01*self.IPD
+ return 0.1*self.IPD
def getSpeed(self):
if self.cSpeed <= 1e-8:
@@ -85,9 +99,14 @@ class Car(object):
v_new = None
if not self.plattonPrev == None:
- # v_v = self.plattonPrev.getSpeed() # TODO estimate this speed
- self.v_v = self.estimateVeloVA()
- v_v = self.v_v
+ #self.v_v = self.estimateVeloVA()
+ #v_v = self.v_v
+
+ 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[2,0]*self.tStep
if checkInbound(d, IPD, 0.01*IPD):
if checkInbound (v, v_v, 0.01*v_v):
if checkInbound(v, PS, 0.01*PS):
@@ -111,9 +130,10 @@ class Car(object):
if self.getCIPD() > d:
print("raise")
raise UnderCIPDerror("under CIPD!")
+ #self.KF.predict()
else: # this is for the LV
- c = 0.8
+ c = 0.5
self.v_v = 0
self.setSpeed(c * v + (1-c) * PS)
@@ -133,6 +153,8 @@ 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()
@@ -146,12 +168,12 @@ if __name__ == "__main__":
v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
pos = [90, 30, 0] # startvektor (Startposition)
- tStep = 30.0
+ tStep = 1000
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(cars[1], v[2], pos[2], setIPD[0], setPS[0],tStep))
t = []
mesIPD= []
diff --git a/modules/CACC/kalmanfilter.py b/modules/CACC/kalmanfilter.py
new file mode 100644
index 0000000000000000000000000000000000000000..7d9fbadff7c1279780bf6ffce0548fb856ac1906
--- /dev/null
+++ b/modules/CACC/kalmanfilter.py
@@ -0,0 +1,64 @@
+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):
+ #print(self.F)
+ self.x = np.dot(self.F, self.x)
+ #print('--')
+ self.P = np.add(np.dot(np.dot(self.F, self.P), self.F.T), self.Q)
+ return None
+
+ def update(self, mesVec):
+ # Innovation
+ #print(mesVec)
+ 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))
+
+ #print('K')
+ K[1,0] = 0.0
+ #print(K)
+ #print('--')
+ 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/kfConfig.py b/modules/CACC/kfConfig.py
new file mode 100644
index 0000000000000000000000000000000000000000..d43a0e1dfe3c614512d118d9c4353bdda310079f
--- /dev/null
+++ b/modules/CACC/kfConfig.py
@@ -0,0 +1,7 @@
+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]])