GPS toggle und .kmz

Extended-Kalman-Filter-CTRV.py

@@ -36,7 +36,7 @@
 
 # <markdowncell>
 
-# Situation covered: You have an velocity sensor which measures the vehicle speed ($v$) in heading direction ($\psi$) and a yaw rate sensor ($\dot \psi$) which both have to fused with the position ($x$ & $y$) from a GPS sensor.
+# Situation covered: You have a velocity sensor, which measures the vehicle speed ($v$) in heading direction ($\psi$) and a yaw rate sensor ($\dot \psi$) which both have to fused with the position ($x$ & $y$) from a GPS sensor.
 
 # <headingcell level=2>
 
@@ -237,6 +237,8 @@
 # <markdowncell>
 
 # Matrix $J_H$ is the Jacobian of the Measurement function $h$ with respect to the state. Function $h$ can be used to compute the predicted measurement from the predicted state.
+# 
+# If a GPS measurement is available, the following function maps the state to the measurement.
 
 # <codecell>
 
@@ -251,12 +253,9 @@
 JHs=hs.jacobian(state)
 JHs
 
-# <codecell>
+# <markdowncell>
 
-JH = np.matrix([[1.0, 0.0, 0.0, 0.0, 0.0],
-                [0.0, 1.0, 0.0, 0.0, 0.0],
-                [0.0, 0.0, 0.0, 1.0, 0.0],
-                [0.0, 0.0, 0.0, 0.0, 1.0]])
+# If no GPS measurement is available, simply set the corresponding values in $J_h$ to zero.
 
 # <headingcell level=2>
 
@@ -444,6 +443,17 @@
                     [float(x[3])],
                     [float(x[4])]])
 
+    if GPS[filterstep]:
+        JH = np.matrix([[1.0, 0.0, 0.0, 0.0, 0.0],
+                        [0.0, 1.0, 0.0, 0.0, 0.0],
+                        [0.0, 0.0, 0.0, 1.0, 0.0],
+                        [0.0, 0.0, 0.0, 0.0, 1.0]])
+    else:
+        JH = np.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
+                        [0.0, 0.0, 0.0, 0.0, 0.0],
+                        [0.0, 0.0, 0.0, 1.0, 0.0],
+                        [0.0, 0.0, 0.0, 0.0, 1.0]])        
+
     S = JH*P*JH.T + R
     K = (P*JH.T) * np.linalg.inv(S)
 
@@ -481,7 +491,7 @@
 
 # <headingcell level=3>
 
-# Uncertainty
+# Uncertainties
 
 # <codecell>
 
@@ -689,7 +699,7 @@
 
 # <codecell>
 
-from simplekml import Kml, Model, AltitudeMode, Orientation, Scale
+from simplekml import Kml, Model, AltitudeMode, Orientation, Scale, Style, Color
 
 # <codecell>
 
@@ -708,34 +718,30 @@
 # Create the track
 trk = kml.newgxtrack(name="EKF", altitudemode=AltitudeMode.clamptoground,
                      description="State Estimation from Extended Kalman Filter with CTRV Model")
-gps = kml.newgxtrack(name="GPS", altitudemode=AltitudeMode.clamptoground,
-                     description="Original GPS Measurements")
 
 # Attach the model to the track
 trk.model = model_car
-gps.model = model_car
-
 trk.model.link.href = car_dae
-gps.model.link.href = car_dae
 
 # Add all the information to the track
 trk.newwhen(car["when"])
 trk.newgxcoord(car["coord"])
 
-gps.newwhen(car["when"][::5])
-gps.newgxcoord((car["gps"][::5]))
-
 # Style of the Track
 trk.iconstyle.icon.href = ""
 trk.labelstyle.scale = 1
 trk.linestyle.width = 4
 trk.linestyle.color = '7fff0000'
 
-gps.iconstyle.icon.href = ""
-gps.labelstyle.scale = 0
-gps.linestyle.width = 3
-gps.linestyle.color = '7fffffff'
+# Add GPS measurement marker
+fol = kml.newfolder(name="GPS Measurements")
+sharedstyle = Style()
+sharedstyle.iconstyle.icon.href = 'http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png'
 
+for m in range(len(latitude)):
+    if GPS[m]:
+        pnt = fol.newpoint(coords = [(longitude[m],latitude[m])])
+        pnt.style = sharedstyle
 
 # Saving
 #kml.save("Extended-Kalman-Filter-CTRV.kml")