Integrated Kalman-Filter

modules/CACC/CACC-Module-Test.py

+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= []

modules/CACC/kalmanfilter.py

+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

modules/CACC/kfConfig.py

+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]])