From 521becf01af3d71faa1bec1eded9e66dcaa4d016 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Mon, 11 Dec 2017 11:47:10 +0100
Subject: [PATCH] Kalman-Filter auf (d,v) reduziert
---
modules/CACC/.kalmanfilter.py.swp | Bin 12288 -> 0 bytes
modules/CACC/CACC-Module-Test.py | 29 +++++++++++++++--------------
modules/CACC/kalmanfilter.py | 6 +-----
modules/CACC/kfConfig.py | 7 -------
4 files changed, 16 insertions(+), 26 deletions(-)
delete mode 100644 modules/CACC/.kalmanfilter.py.swp
delete mode 100644 modules/CACC/kfConfig.py
diff --git a/modules/CACC/.kalmanfilter.py.swp b/modules/CACC/.kalmanfilter.py.swp
deleted file mode 100644
index af88080ce0232adb046cd99d6a41b2aed11b01e6..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 12288
zcmeI2&x<2P6vu1Ui>t1RD|oG#O9pz9jI#@ipg7EsogvJU`LQF8%2<<rNy>DpLv?k`
z%wavO`X?xR5HJ1%t9bSxp4@}rO$1$Uf}$S8li;_iJ4q+Zf+s<&hEKY?s@|*jzV*6b
zm^!H+Ztw7G)jGrVBx66ERE~c8ov}I^wpB+eX=uAmw)cV8_QuV%wfgXZ{gaMc1&6iG
zxSbA2nWVOF(om*@+E(1|iPXdcVPw+oQPL1SO=`Yfk~WmA<XwX}Oe0Ea8|xbzwZ4c3
z0wG6+bai+-nbqID0$u@@n$F6#OKk1ARrjfvpDpprPu=vwcm=!yUIDLwSHLUa74Qmp
z1-t_PBL&QGiM@?{FXTDDnBSKc?)j^~c?G-zUIDLwSHLUa74Qmp1-t@Y0k42pz$@?{
zR6vA`ef}_G-&{oS`2YXp@Bd$(VC-k`HFzHwa0qt56;J{keE&FO_rRy%6L1&Y0q=o#
z!2wtYFM&TUG4><44?Y0zfZO0Ta21pQ2N%Kp$1n!m2DiW)pb4G^m%*QpVm@#e+ySqE
z7WnHC#(o9gf_vaA@F6$>H^2_K3Z4NEfeT;>{Dl0i2H$}%!Drwj;4iO$SHLUa74Qmp
z1-t_PvI0E6G?`Q@o>0`Oj@e@C!utblLhA4?nL`n!RI$F6pGQ6m);D5B(>6C@i@%V~
z9BbK?s^xmEFHH%Vjd(Qm^^aVsVO4~ov#GWMzQWNE#-@_rn!)~Z(5xOTFV9CYMifQU
zaWFA#2K*GGr^Cy<R4Ps1ny;<~_19rs$01jqU-LuC{OB#hL05i6G9o(=`I<9Q0T0DM
z8lFhvAt^3!wp8);lf(!WawXe6!$+jQ_*jKFms*^4I#V+XyO3a}X^=^}g+yH<kyIk;
zR)<oJD*J`p%e?8%wL+)sLaJGo`!hY}1$6$ma{n!O&l=oB?s0FXY?<#-BEwWf{dgp_
z6zXg?!|7~>0VRjjt{xPYYi^qHT*TW-!JCn>ax&ObNr#~@S<wYN^M>NHtKoj^Ltf+O
z*I&N!=VaPg%(OAfba$4i3pcMJyBO|$Vmr>p*+7SyLfOvx?dM@5v)6gim3cQ3WK62M
zSe|}FQt{n@cXga3e2+Oh{FHfks!&Hb)Qn+SFNS5~_hwUMiv!V%raP1*TBNXDX-<|1
zC#1C;^NLC}@1&%8i26KBt*!~76cxX&WA|M6Dzf#0_pL%;)_d+P9XL0(#pWZ@j(gV>
ziIGsY?l-Z5%FY9c(leaIrb0P7_q%YvTX5gf@frN(@$ckeFXXWpV*^ea1t*(MynYsD
zVZU-S=35d>vTX@+XZ3D3tyvSkqDAp~agX@R_BeT_Arw{=;qza&5^88bYB4lhKeO!@
z+eYekf@DN}6YFWr(}&Q&V6|psyPvd3?^8Qg;k4H!V1^0CoHN1}<MFXQfPZ}Ksx{!)
z@~~l|Xi+`*!1sa$JJf4C5~#gw%V@Zx@Rj)FO==rk%W^P`wc#oq3{Sa8uz#|@0pt2D
A3IG5A
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index 1e2c89bc..19dae0ba 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/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))
diff --git a/modules/CACC/kalmanfilter.py b/modules/CACC/kalmanfilter.py
index 0c8de984..fc9d0dc4 100644
--- a/modules/CACC/kalmanfilter.py
+++ b/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
diff --git a/modules/CACC/kfConfig.py b/modules/CACC/kfConfig.py
deleted file mode 100644
index d43a0e1d..00000000
--- a/modules/CACC/kfConfig.py
+++ /dev/null
@@ -1,7 +0,0 @@
-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