diff --git a/modules/CACC/.kalmanfilter.py.swp b/modules/CACC/.kalmanfilter.py.swp index 8c975bcc3f880ec00370de16665fd8fbb3d0897c..af88080ce0232adb046cd99d6a41b2aed11b01e6 100644 Binary files a/modules/CACC/.kalmanfilter.py.swp and b/modules/CACC/.kalmanfilter.py.swp differ diff --git a/modules/CACC/kalmanfilter.py b/modules/CACC/kalmanfilter.py index 7d9fbadff7c1279780bf6ffce0548fb856ac1906..0c8de9847579457381f501e296069538453b5f53 100644 --- a/modules/CACC/kalmanfilter.py +++ b/modules/CACC/kalmanfilter.py @@ -30,15 +30,12 @@ class KalmanFilter(object): 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 @@ -47,10 +44,11 @@ class KalmanFilter(object): # Kalman - Gain K = np.dot(np.dot(self.P, self.H.T), np.linalg.pinv(S)) - #print('K') + # Zweite Zeile 0.0 setzen, damit sie den alten Abstand nicht verändert!!! + # !!! 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