Kalman-Filter auf (d,v) reduziert

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

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

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