Constant Heading and Constant Velocity EKF

Extended-Kalman-Filter-CTRV2.py

@@ -1,10 +1,11 @@
 
 # coding: utf-8
 
-# In[109]:
+# In[ ]:
+
 
 import numpy as np
-get_ipython().magic(u'matplotlib inline')
+get_ipython().magic('matplotlib inline')
 import matplotlib.dates as mdates
 import matplotlib.pyplot as plt
 from scipy.stats import norm
@@ -39,14 +40,16 @@
 # 
 # $^{*}$=actually measured values in this implementation example!
 
-# In[110]:
+# In[ ]:
+
 
 numstates=5 # States
 
 
 # We have different frequency of sensor readings.
 
-# In[111]:
+# In[ ]:
+
 
 dt = 1.0/50.0 # Sample Rate of the Measurements is 50Hz
 dtGPS=1.0/10.0 # Sample Rate of GPS is 10Hz
@@ -56,7 +59,8 @@
 # 
 # All symbolic calculations are made with [Sympy](http://nbviewer.ipython.org/github/jrjohansson/scientific-python-lectures/blob/master/Lecture-5-Sympy.ipynb). Thanks!
 
-# In[112]:
+# In[ ]:
+
 
 vs, psis, dpsis, dts, xs, ys, lats, lons = symbols('v \psi \dot\psi T x y lat lon')
 
@@ -72,19 +76,22 @@
 # 
 # This formulas calculate how the state is evolving from one to the next time step
 
-# In[113]:
+# In[ ]:
+
 
 gs
 
 
 # ### Calculate the Jacobian of the Dynamic function $g$ with respect to the state vector $x$
 
-# In[114]:
+# In[ ]:
+
 
 state
 
 
-# In[115]:
+# In[ ]:
+
 
 gs.jacobian(state)
 
@@ -97,13 +104,15 @@
 # 
 # Initialized with $0$ means you are pretty sure where the vehicle starts
 
-# In[116]:
+# In[ ]:
+
 
 P = np.diag([1000.0, 1000.0, 1000.0, 1000.0, 1000.0])
 print(P, P.shape)
 
 
-# In[117]:
+# In[ ]:
+
 
 fig = plt.figure(figsize=(5, 5))
 im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary'))
@@ -136,7 +145,8 @@
 # 
 # "*The state uncertainty model models the disturbances which excite the linear system. Conceptually, it estimates how bad things can get when the system is run open loop for a given period of time.*" - Kelly, A. (1994). A 3D state space formulation of a navigation Kalman filter for autonomous vehicles, (May). Retrieved from http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix=html&identifier=ADA282853
 
-# In[118]:
+# In[ ]:
+
 
 sGPS     = 0.5*8.8*dt**2  # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
 sCourse  = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle
@@ -147,7 +157,8 @@
 print(Q, Q.shape)
 
 
-# In[119]:
+# In[ ]:
+
 
 fig = plt.figure(figsize=(5, 5))
 im = plt.imshow(Q, interpolation="none", cmap=plt.get_cmap('binary'))
@@ -175,7 +186,8 @@
 
 # ## Real Measurements
 
-# In[120]:
+# In[ ]:
+
 
 #path = './../RaspberryPi-CarPC/TinkerDataLogger/DataLogs/2014/'
 datafile = '2014-03-26-000-Data.csv'
@@ -200,14 +212,16 @@
 # 
 # If a GPS measurement is available, the following function maps the state to the measurement.
 
-# In[121]:
+# In[ ]:
+
 
 hs = Matrix([[xs],
              [ys]])
 hs
 
 
-# In[122]:
+# In[ ]:
+
 
 JHs=hs.jacobian(state)
 JHs
@@ -219,7 +233,8 @@
 # 
 # "In practical use, the uncertainty estimates take on the significance of relative weights of state estimates and measurements. So it is not so much important that uncertainty is absolutely correct as it is that it be relatively consistent across all models" - Kelly, A. (1994). A 3D state space formulation of a navigation Kalman filter for autonomous vehicles, (May). Retrieved from http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix=html&identifier=ADA282853
 
-# In[123]:
+# In[ ]:
+
 
 varGPS = 6.0 # Standard Deviation of GPS Measurement
 varspeed = 1.0 # Variance of the speed measurement
@@ -230,7 +245,8 @@
 print(R, R.shape)
 
 
-# In[124]:
+# In[ ]:
+
 
 fig = plt.figure(figsize=(4.5, 4.5))
 im = plt.imshow(R, interpolation="none", cmap=plt.get_cmap('binary'))
@@ -258,15 +274,17 @@
 
 # ## Identity Matrix
 
-# In[125]:
+# In[ ]:
+
 
 I = np.eye(numstates)
 print(I, I.shape)
 
 
 # ## Approx. Lat/Lon to Meters to check Location
 
-# In[126]:
+# In[ ]:
+
 
 RadiusEarth = 6378388.0 # m
 arc= 2.0*np.pi*(RadiusEarth+altitude)/360.0 # m/°
@@ -284,7 +302,8 @@
 
 # ## Initial State
 
-# In[127]:
+# In[ ]:
+
 
 x = np.matrix([[mx[0], my[0], course[0]/180.0*np.pi, speed[0]/3.6+0.001, yawrate[0]/180.0*np.pi]]).T
 print(x, x.shape)
@@ -300,15 +319,17 @@
 
 # ### Put everything together as a measurement vector
 
-# In[128]:
+# In[ ]:
+
 
 measurements = np.vstack((mx, my))
 # Lenth of the measurement
 m = measurements.shape[1]
 print(measurements.shape)
 
 
-# In[129]:
+# In[ ]:
+
 
 # Preallocation for Plotting
 x0 = []
@@ -359,7 +380,8 @@ def savestates(x, Z, P, K):
 
 # $$x_k= \begin{bmatrix} x \\ y \\ \psi \\ v \\ \dot\psi \end{bmatrix} = \begin{bmatrix} \text{Position X} \\ \text{Position Y} \\ \text{Heading} \\ \text{Velocity} \\ \text{Yaw Rate} \end{bmatrix} =  \underbrace{\begin{matrix}x[0] \\ x[1] \\ x[2] \\ x[3] \\ x[4]  \end{matrix}}_{\textrm{Python Nomenclature}}$$
 
-# In[130]:
+# In[ ]:
+
 
 for filterstep in range(m):
 
@@ -430,14 +452,10 @@ def savestates(x, Z, P, K):
     savestates(x, Z, P, K)
 
 
-# In[ ]:
-
-
-
-
 # ## Lets take a look at the filter performance
 
-# In[131]:
+# In[ ]:
+
 
 def plotP():
     fig = plt.figure(figsize=(16,9))
@@ -455,12 +473,14 @@ def plotP():
 
 # ### Uncertainties in $P$
 
-# In[132]:
+# In[ ]:
+
 
 plotP()
 
 
-# In[133]:
+# In[ ]:
+
 
 fig = plt.figure(figsize=(6, 6))
 im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary'))
@@ -491,7 +511,8 @@ def plotP():
 
 # ### Kalman Gains
 
-# In[134]:
+# In[ ]:
+
 
 fig = plt.figure(figsize=(16,9))
 plt.step(range(len(measurements[0])),Kx, label='$x$')
@@ -510,7 +531,8 @@ def plotP():
 
 # ## State Vector
 
-# In[135]:
+# In[ ]:
+
 
 def plotx():
     fig = plt.figure(figsize=(16,16))
@@ -547,19 +569,22 @@ def plotx():
     plt.savefig('Extended-Kalman-Filter-CTRV-State-Estimates.png', dpi=72, transparent=True, bbox_inches='tight')
 
 
-# In[136]:
+# In[ ]:
+
 
 plotx()
 
 
 # ## Position x/y
 
-# In[137]:
+# In[ ]:
+
 
 #%pylab --no-import-all
 
 
-# In[138]:
+# In[ ]:
+
 
 def plotxy():
 
@@ -589,14 +614,16 @@ def plotxy():
     #plt.savefig('Extended-Kalman-Filter-CTRV-Position.png', dpi=72, transparent=True, bbox_inches='tight')
 
 
-# In[139]:
+# In[ ]:
+
 
 plotxy()
 
 
 # ### Detailed View
 
-# In[140]:
+# In[ ]:
+
 
 def plotxydetails():
     fig = plt.figure(figsize=(12,9))
@@ -640,7 +667,8 @@ def plotxydetails():
     plt.legend(loc='best')
 
 
-# In[141]:
+# In[ ]:
+
 
 plotxydetails()
 
@@ -653,7 +681,8 @@ def plotxydetails():
 
 # ### Convert back from Meters to Lat/Lon (WGS84)
 
-# In[142]:
+# In[ ]:
+
 
 latekf = latitude[0] + np.divide(x1,arc)
 lonekf = longitude[0]+ np.divide(x0,np.multiply(arc,np.cos(latitude*np.pi/180.0)))
@@ -664,7 +693,8 @@ def plotxydetails():
 # Coordinates and timestamps to be used to locate the car model in time and space
 # The value can be expressed as yyyy-mm-ddThh:mm:sszzzzzz, where T is the separator between the date and the time, and the time zone is either Z (for UTC) or zzzzzz, which represents ±hh:mm in relation to UTC.
 
-# In[143]:
+# In[ ]:
+
 
 import datetime
 car={}
@@ -678,12 +708,14 @@ def plotxydetails():
     car["gps"].append((longitude[i], latitude[i], 0))
 
 
-# In[144]:
+# In[ ]:
+
 
 from simplekml import Kml, Model, AltitudeMode, Orientation, Scale, Style, Color
 
 
-# In[145]:
+# In[ ]:
+
 
 # The model path and scale variables
 car_dae = r'https://raw.githubusercontent.com/balzer82/Kalman/master/car-model.dae'
@@ -730,7 +762,8 @@ def plotxydetails():
 kml.savekmz("Extended-Kalman-Filter-CTRV.kmz")
 
 
-# In[146]:
+# In[ ]:
+
 
 print('Exported KMZ File for Google Earth')
 

README.md

@@ -65,6 +65,12 @@ Situation covered: You have an velocity sensor which measures the vehicle speed
 
 [View IPython Notebook](https://github.com/balzer82/Kalman/blob/master/Extended-Kalman-Filter-CTRV.ipynb?create=1) ~ [See Vimeo](https://vimeo.com/88057157)
 
+### Extended Kalman Filter with Constant Heading and Constant Velocity (CHCV) Model
+
+Situation covered: You have the position (x & y) from a GPS sensor and extimating the heading direction (ψ) and the velocity (v).
+
+[View IPython Notebook](https://github.com/balzer82/Kalman/blob/master/Extended-Kalman-Filter-CHCV.ipynb?create=1)
+
 ### Extended Kalman Filter with Constant Turn Rate and Acceleration (CTRA) Model
 
 Situation covered: You have an acceleration and velocity sensor which measures the vehicle longitudinal acceleration and speed (v) in heading direction (ψ) and a yaw rate sensor (ψ˙) which all have to fused with the position (x & y) from a GPS sensor.