From a85391c7518f464b56753fa1144c521525fb8a7d Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Wed, 6 Dec 2017 22:34:20 +0100
Subject: [PATCH] Integrated Kalman-Filter
---
modules/CACC/.kalmanfilter.py.swp | Bin 0 -> 12288 bytes
modules/CACC/CACC-Module-Test.py | 38 ++++++++++++++----
modules/CACC/kalmanfilter.py | 64 ++++++++++++++++++++++++++++++
modules/CACC/kfConfig.py | 7 ++++
4 files changed, 101 insertions(+), 8 deletions(-)
create mode 100644 modules/CACC/.kalmanfilter.py.swp
create mode 100644 modules/CACC/kalmanfilter.py
create mode 100644 modules/CACC/kfConfig.py
diff --git a/modules/CACC/.kalmanfilter.py.swp b/modules/CACC/.kalmanfilter.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..8c975bcc3f880ec00370de16665fd8fbb3d0897c
GIT binary patch
literal 12288
zcmeI2J8UCG7{}+RE<gZ*M9TyhaisMl<W4%Bgh-Ug=g10jY+p_~K+)Xp*xvJcb~QU2
z*(spl(b9pas400|NkKt_D1ep%foPDR0j{9qKeKD^X7Nc>Na$vzpVm7w-|IhLrperC
zo*W+0YxO3<^AsUh2i4oZd_hPP1BYVh3t#a=m+RRFK8O1^cXpc51LqeLuNNFP4njAX
za-rfzUnQQOOdIXc9kWD*Q>Ogb9rL)w#*&MR<35*)$F*!~8hS~<<Hr8p{(fV^f+<7F
zf#R|r%@<Ma+bUocAc-2*uC0@u7q-oxdgb{I`pnZetuR&rtAJI&Dqt0`3RnfK0#*U5
zz(b^fiY}2iQ12sI&AZwAQt6$2+Lu+pDqt0`3RnfK0#*U5fK|XMU=^?mSOu&C|3d|s
zN61H)3Hfpj$>ab3yIBptJxR!~;2ZEBP~Zd{fh%AGQ1HVOgnSJ?1)qSA!3W?S@HQBL
zJ+KSzuM_eU_z}Dh-U4reSHV@V0Vr4lzdugMSKuC)fLovku7KyjW$^7|$Pau5LLk5i
zcowXKzaAyzXYd{P9J~t#U>CduHo+RWkJ|qPe}M0S*5V%c5ZK2mU=^?mSOu&CRspNP
zf2;t_9?4ZA1&w(ytj|b!wEUf~^7xRBxH@4$!mHYP>pChZt$#Tbd^u)9bm@y}tWf$R
zUv$l^_9c1D)qZ%k^!2Y?iKxyz&)C$v4y{oPc%iChua48-ays?F*4D)=X2_ywI&&6=
z9f!^ft1FT8g{p4WYMZ%>Y_OA$bY9tZny<lW6W3iAi7#wB1=HnZC(WRcYbuYEs?$c!
zCa1s`EEv@zUz}C@#l&dGTuMdBH<&MS<@<SQ7qu|{+nN6^yr%;ukonom>#5Klk9{v;
z!6ZCm(r4m4k1)?8OnH35-TI)Q{Dm;HV#cYIsYx-^p%4h7{7@KmbTcH8$5h%@4$UmC
zo7ts+S>LYiuxxKv2+%4E(8>ikE@ZW8-%V**>*$R$-wTJce;B9J>98&;lHBv%)J*@e
z(Ck@NNrjFA#+6FM2s=CpxG(6jLq{@<W7=DqR;V`@Y^pCcV_7<jW$F4}9>S#3!@d<w
zk9ZtQmcX{wS?n@yNK1c4t0Iwfm~crwpVRY1??KFk#|6D1L-Tv9<h>2=?LuPOd*&^f
z8aH}G*Ta&b`IbCjXG|zKJ-}`Y9S0I6=eUW@3mt8eKZg6`g8Qxv&*3l2|0qklR7W|-
z7M!#SP7aKClQhl3>185j2NX=wLyF=t6Z#0thbpublvc;#nA2POI(e!g6c#)PINj3{
zN@&1EIW^r@`SDw!x06^g;l*csqC&aMdHKUrFxYM=-<`x=F8kaKg}0nFd8#PJniVT_
zGoGI63;3s}rimSj0}#h2dKuk=Got5|?9i`nz|ec?snar7;j7`DyWCZ}m;GrJN<~F7
LjpmfaI8Djlmvko^
literal 0
HcmV?d00001
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index e2f631fa..1e2c89bc 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 00000000..7d9fbadf
--- /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 00000000..d43a0e1d
--- /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]])
--
GitLab