In [392]:
import numpy as np
import matplotlib.dates as mdates
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from scipy.stats import norm
import time
from IPython.display import Image as ImageDisp
from sympy import Symbol, symbols, Matrix, sin, cos, latex, Plot
from sympy.interactive import printing
printing.init_printing()
%pylab inline --no-import-all

Populating the interactive namespace from numpy and matplotlib


# Adaptive Extended Kalman Filter Implementation for Constant Turn Rate and Velocity (CTRV) Vehicle Model with Attitude Estimation in Python¶

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 in loosely coupled way.

## State Vector - Constant Turn Rate and Velocity Vehicle Model (CTRV) + Roll and Pitch Estimation¶

$$x_k= \left[ \begin{matrix} x\\y\\ v \\ \psi\\\phi\\\Theta \end{matrix}\right] = \left[ \begin{matrix} \text{Position x (GNSS)}\\ \text{Position y (GNSS)}\\ \text{Speed (GNSS)} \\ \text{Heading (GNSS)} \\ \text{Pitch (IMU)} \\ \text{Roll (IMU)} \end{matrix}\right]$$

In [393]:
numstates=6 # States

In [394]:
dt    = 1.0/50.0 # Sample Rate of the Measurements is 50Hz
dtGPS = 1.0/10.0 # Sample Rate of GPS is 10Hz


All symbolic calculations are made with Sympy. Thanks!

In [395]:
vs, psis, dpsis, dts, xs, ys, axs, phis, dphis, thetas, dthetas, Lats, Lons = \
symbols('v \psi \dot\psi T x y a_x \phi \dot\phi \Theta \dot\Theta Lat Lon')

As = Matrix([[xs+(vs/dpsis)*(sin(psis+dpsis*dts)-sin(psis))],
[ys+(vs/dpsis)*(-cos(psis+dpsis*dts)+cos(psis))],
[vs + axs*dts],
[psis+dpsis*dts],
[phis+dphis*dts],
[thetas+dthetas*dts]])
state = Matrix([xs,ys,vs,psis,phis,thetas])


## Dynamic Matrix¶

This formulas calculate how the state is evolving from one to the next time step

In [396]:
As

Out[396]:
$$\left[\begin{matrix}x + \frac{v}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right)\\y + \frac{v}{\dot\psi} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right)\\T a_{x} + v\\T \dot\psi + \psi\\T \dot\phi + \phi\\T \dot\Theta + \Theta\end{matrix}\right]$$
In [397]:
print latex(As)

\left[\begin{matrix}x + \frac{v}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right)\\y + \frac{v}{\dot\psi} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right)\\T a_{x} + v\\T \dot\psi + \psi\\T \dot\phi + \phi\\T \dot\Theta + \Theta\end{matrix}\right]


### Calculate the Jacobian of the Dynamic Matrix with respect to the state vector¶

In [398]:
state

Out[398]:
$$\left[\begin{matrix}x\\y\\v\\\psi\\\phi\\\Theta\end{matrix}\right]$$
In [399]:
As.jacobian(state)

Out[399]:
$$\left[\begin{matrix}1 & 0 & \frac{1}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & \frac{v}{\dot\psi} \left(- \cos{\left (\psi \right )} + \cos{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & 1 & \frac{1}{\dot\psi} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right) & \frac{v}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 1\end{matrix}\right]$$

It has to be computed on every filter step because it consists of state variables.

In [400]:
print latex(As.jacobian(state))

\left[\begin{matrix}1 & 0 & \frac{1}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & \frac{v}{\dot\psi} \left(- \cos{\left (\psi \right )} + \cos{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & 1 & \frac{1}{\dot\psi} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right) & \frac{v}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 1\end{matrix}\right]


It has to be computed on every filter step because it consists of state variables.

## Real Measurements from Low Budget Hardware (IMU & GPS)¶

In [401]:
#path = './../RaspberryPi-CarPC/TinkerDataLogger/DataLogs/2014/'
#datafile = path+'2014-04-13-002-Data.csv'
datafile = '2014-04-23-GPS-IMU-Data.csv'

date, \
timem, \
millis, \
ax, \
ay, \
az, \
rollrate, \
pitchrate, \
yawrate, \
roll, \
pitch, \
yaw, \
speed, \
course, \
latitude, \
longitude, \
altitude, \
pdop, \
hdop, \
vdop, \
epe, \
fix, \
satellites_view, \
satellites_used, \
temp = np.loadtxt(datafile, delimiter=',', unpack=True, skiprows=1)


Read '2014-04-23-GPS-IMU-Data.csv' successfully.


### Static Gains¶

They are estimated through several measurements in static condition

In [402]:
#pitch = pitch + 1.405
#ax = ax + 0.33
ax = ax + 0.5
rollrate = rollrate + 1.42
yawrate = yawrate + 0.07
pitchrate = pitchrate + 2.17


### Course Correction¶

In [403]:
# A course of 0° means the Car is traveling north bound
# and 90° means it is traveling east bound.
# In the Calculation following, East is Zero and North is 90°
# We need an offset.
course =(-course+90.0)


### Clamping of Low Speed Values¶

In [404]:
# clamp speed and yawrate to zero while standing still
#speed[speed<1.0]=0.0
#yawrate[speed<1.0]=0.0


### Map¶

In [405]:
# Display GPS Heatmap from Disk
gpsheatmap = ImageDisp(filename='2014-04-23-GPS-IMU-Data-Heatmap.png')
gpsheatmap

Out[405]:

Map tiles by Stamen Design, under CC BY 3.0. Data by OpenStreetMap, under CC BY SA.

## Real Measurements from GNSS Hardware as Ground Truth¶

In [406]:
datafile = '2014-04-23-GNSSGroundTruth.csv'

LatDD, LonDD = np.loadtxt(datafile, delimiter=',', unpack=True, skiprows=1)


Read '2014-04-23-GNSSGroundTruth.csv' successfully.


## Roll-/Pitch-/Yawrate¶

In [407]:
plt.figure(figsize=(14,4))
#plt.plot(roll)
plt.plot(pitchrate, label='Pitchrate $\dot\phi$')
plt.plot(rollrate, label='Rollrate $\dot \Theta$')
plt.plot(yawrate, label='Yawrate $\dot \psi$')
plt.xlabel('Filterstep')
plt.ylabel('Turnrate $(^\circ/s)$')
plt.legend(loc='best')

Out[407]:
<matplotlib.legend.Legend at 0x112ec8790>

### Amplitude Spectrum of Turnrates¶

In [408]:
hann = np.hanning(len(pitchrate))
Yax = np.fft.fft(ax*hann)
Ypitchrate = np.fft.fft(pitchrate*hann)
Yrollrate = np.fft.fft(rollrate*hann)
Yyawrate = np.fft.fft(yawrate*hann)

N   = len(Ypitchrate)/2+1
Xfft = np.linspace(0, 1.0/dt/2, N)

In [409]:
plt.figure(figsize=(8,3))
plt.plot(Xfft, 2.0*np.abs(Yax[:N])/N, label='Acceleration', alpha=0.9)
plt.plot(Xfft, 2.0*np.abs(Ypitchrate[:N])/N, label='Pitchrate', alpha=0.9)
plt.plot(Xfft, 2.0*np.abs(Yrollrate[:N])/N, label='Rollrate', alpha=0.9)
plt.plot(Xfft, 2.0*np.abs(Yyawrate[:N])/N, label='Yawrate', alpha=0.9)
#plt.xlim(0, 1)
plt.axvline(5, color='grey', label='Cutoff Frequency', alpha=0.4)
plt.legend()
plt.ylabel('Amplitude $^\circ/s /Hz$')
plt.xlabel('Frequency $Hz$')

Out[409]:
<matplotlib.text.Text at 0x11506cbd0>

### 5th Order Butterworth LowPass¶

In [410]:
cutoff = 5. # Hz
fs = 1/dt
nyq = fs/2.
filterorder = 5

from scipy import signal
b,a = signal.filter_design.butter(filterorder,cutoff/nyq)

axLowpass = signal.lfilter(b,a, ax)
pitchrateLowpass = signal.lfilter(b,a, pitchrate)
rollrateLowpass = signal.lfilter(b,a, rollrate)
yawrateLowpass = signal.lfilter(b,a, yawrate)

In [411]:
plt.figure(figsize=(14,8))
plt.subplot(411)
plt.plot(ax, label='Acceleration')
plt.plot(axLowpass, label='Acceleration %iHz Lowpass-filtered' % cutoff)
plt.ylabel('Acceleration $(m/s^2)$')
plt.legend(loc='best')

plt.subplot(412)
plt.plot(pitchrate, label='Pitchrate')
plt.plot(pitchrateLowpass, label='Pitchrate %iHz Lowpass-filtered' % cutoff)
plt.ylabel('Pitchrate $(^\circ/s)$')
plt.legend(loc='best')

plt.subplot(413)
plt.plot(rollrate, label='Rollrate')
plt.plot(rollrateLowpass, label='Rollrate %iHz Lowpass-filtered' % cutoff)
plt.ylabel('Rollrate $(^\circ/s)$')
plt.legend(loc='best')

plt.subplot(414)
plt.plot(yawrate, label='Yawrate')
plt.plot(yawrateLowpass, label='Yawrate %iHz Lowpass-filtered' % cutoff)
plt.ylabel('Yawrate $(^\circ/s)$')
plt.legend(loc='best')

plt.xlabel('Filterstep')

Out[411]:
<matplotlib.text.Text at 0x113326890>

### Determine natural Movement Turnrate-Acceleration¶

In [412]:
plt.figure(figsize=(14,4))
plt.plot(np.diff(axLowpass)/dt, label='Jerk $\ddot v$')
plt.plot(np.diff(pitchrateLowpass)/dt, label='Pitchrate-Acceleration $\ddot\phi$')
plt.plot(np.diff(rollrateLowpass)/dt, label='Rollrate-Acceleration $\ddot \Theta$')
plt.plot(np.diff(yawrateLowpass)/dt, label='Yawrate-Acceleration $\ddot \psi$')
plt.xlabel('Filterstep')
plt.ylabel('Turnrate Accelerations $(^\circ/s^2)$')
plt.legend()

Out[412]:
<matplotlib.legend.Legend at 0x118fb1310>
In [413]:
print('max Jerk: %.1f m/s3' % np.max(np.abs(np.diff(axLowpass)/dt)))
print('max Pitchrate-Acceleration: %.1f °/s2' % np.max(np.abs(np.diff(pitchrateLowpass)/dt)))
print('max Rollrate-Acceleration: %.1f °/s2' % np.max(np.abs(np.diff(rollrateLowpass)/dt)))
print('max Yawrate-Acceleration: %.1f °/s2' % np.max(np.abs(np.diff(yawrateLowpass)/dt)))

max Jerk: 36.2 m/s3
max Pitchrate-Acceleration: 82.3 °/s2
max Rollrate-Acceleration: 62.0 °/s2
max Yawrate-Acceleration: 33.6 °/s2


## Control Input¶

In [414]:
control = Matrix([axs, dpsis, dphis, dthetas])
control

Out[414]:
$$\left[\begin{matrix}a_{x}\\\dot\psi\\\dot\phi\\\dot\Theta\end{matrix}\right]$$
In [415]:
print latex(control)

\left[\begin{matrix}a_{x}\\\dot\psi\\\dot\phi\\\dot\Theta\end{matrix}\right]


### Calculate the Jacobian of the Dynamic Matrix with Respect to the Control Input¶

In [416]:
JGs = As.jacobian(control)
JGs

Out[416]:
$$\left[\begin{matrix}0 & \frac{T v}{\dot\psi} \cos{\left (T \dot\psi + \psi \right )} - \frac{v}{\dot\psi^{2}} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & \frac{T v}{\dot\psi} \sin{\left (T \dot\psi + \psi \right )} - \frac{v}{\dot\psi^{2}} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\T & 0 & 0 & 0\\0 & T & 0 & 0\\0 & 0 & T & 0\\0 & 0 & 0 & T\end{matrix}\right]$$
In [417]:
print latex(JGs)

\left[\begin{matrix}0 & \frac{T v}{\dot\psi} \cos{\left (T \dot\psi + \psi \right )} - \frac{v}{\dot\psi^{2}} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\0 & \frac{T v}{\dot\psi} \sin{\left (T \dot\psi + \psi \right )} - \frac{v}{\dot\psi^{2}} \left(\cos{\left (\psi \right )} - \cos{\left (T \dot\psi + \psi \right )}\right) & 0 & 0\\T & 0 & 0 & 0\\0 & T & 0 & 0\\0 & 0 & T & 0\\0 & 0 & 0 & T\end{matrix}\right]


## Process Noise Covariance Matrix $Q$¶

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: "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. The $Q$ matrix can be assumed diagonal, and its elements set to the predicted magnitude of the truncated terms in the constant velocity model. They can arise from:

• disturbances such as terrain following loads
• neglected control inputs such as sharp turns, braking or accelerating
• neglected derivatives in the dead reckoning model
• neglected states"
In [418]:
jerkmax = 300.0    # m/s3

rollrateaccmax =  200.0 *np.pi/180.0 # rad/s2
yawrateaccmax  =  80.0  *np.pi/180.0 # rad/s2

In [419]:
print('Sigma ax: %.2f m/s2' % (dt * jerkmax))
print('Sigma yaw: %.3f 1/s' % (dt * yawrateaccmax))
print('Sigma pitch: %.3f 1/s' % (dt * pitchrateaccmax))
print('Sigma roll: %.3f 1/s' % (dt * rollrateaccmax))

Sigma ax: 6.00 m/s2
Sigma yaw: 0.028 1/s
Sigma pitch: 0.070 1/s
Sigma roll: 0.070 1/s

In [420]:
Q = np.diagflat([[(dt * jerkmax)**2],            # acceleration
[(dt * yawrateaccmax)**2],           # yawrate
[(dt * pitchrateaccmax)**2],         # pitchrate
[(dt * rollrateaccmax)**2]])         # rollrate

In [421]:
fig = plt.figure(figsize=(numstates, numstates))
im = plt.imshow(Q, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Process Noise Covariance Matrix $Q$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(6))
# set the locations and labels of the yticks
plt.yticks(np.arange(5), \
('$a_x$', '$\dot \psi$', '$\dot \phi$', '$\dot \Theta$'),\
fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(6))
# set the locations and labels of the yticks
plt.xticks(np.arange(5), \
('$a_x$', '$\dot \psi$', '$\dot \phi$', '$\dot \Theta$'),\
fontsize=22)

plt.xlim([-0.5,3.5])
plt.ylim([3.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
plt.colorbar(im, cax=cax)

plt.tight_layout()


### Estimated Position Error¶

$EPE \sim \mathrm{HDOP} \cdot \mathrm{URA}(1 \sigma)$

In [422]:
plt.figure(figsize=(16,3))
plt.plot(epe, label='$EPE$ from GNSS modul', marker='*', markevery=50)
#plt.plot(speed)
plt.ylabel('$EPE$ in $(m)$')
plt.xlabel('Filterstep $k$')
plt.xlim(0,6000)
plt.legend(loc='best')
plt.savefig('Extended-Kalman-Filter-CTRV-EPE.eps', bbox_inches='tight')


### Lat/Lon to Meters¶

In [423]:
R = 6378388.0 + altitude # m
arc= 2.0*np.pi*R/360.0 # m/°

dx = arc * np.cos(latitude*np.pi/180.0) * np.hstack((0.0, np.diff(longitude))) # in m
dy = arc * np.hstack((0.0, np.diff(latitude))) # in m

mx = np.cumsum(dx)
my = np.cumsum(dy)

ds = np.sqrt(dx**2+dy**2)

GPS=np.hstack((True, (np.diff(ds)>0.0).astype('bool'))) # GPS Trigger for Kalman Filter

In [424]:
print('One degree of Lon is %.2fkm at %.1fm altitude.' % (arc[0]/1000.0, altitude[0]))

One degree of Lon is 111.33km at 117.7m altitude.


## Measurement Noise Covariance Matrix $R$ (Adaptive)¶

"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 [425]:
state

Out[425]:
$$\left[\begin{matrix}x\\y\\v\\\psi\\\phi\\\Theta\end{matrix}\right]$$
In [426]:
R = np.diagflat([[(12.0)**2],      # x
[(12.0)**2],           # y
[(1.0)**2],  # v
[(0.5)**2],  # pitch
[(0.5)**2]]) # roll

In [427]:
fig = plt.figure(figsize=(numstates, numstates))
im = plt.imshow(R, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Measurement Noise Covariance Matrix $R$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

plt.xlim([-0.5,5.5])
plt.ylim([5.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
plt.colorbar(im, cax=cax)

plt.tight_layout()


### Position¶

$R$ is just initialized here. In the Kalman Filter Step it will calculated dynamically with the $EPE$ (Estimated Position Error) from the GPS signal as well as depending on the $speed$, like proposed in [Wender, S. (2008). Multisensorsystem zur erweiterten Fahrzeugumfelderfassung. Retrieved from http://vts.uni-ulm.de/docs/2008/6605/vts_6605_9026.pdf P.108].

$\sigma_p^2 = c \cdot \sigma_\text{speed}^2 + \sigma_\text{EPE}^2$

with

$\sigma_\text{speed} = (v+\epsilon)^{-\xi}$

$\sigma_\text{EPE} = \zeta \cdot EPE$

In [428]:
epsilon = 0.1
xi      = 500.0
zeta    = 50.0

spspeed=xi/((speed/3.6)+epsilon)
spepe=zeta*epe
sp = (spspeed)**2 + (spepe)**2

In [429]:
plt.figure(figsize=(6,2))
plt.semilogy(spspeed**2, label='$\sigma_{x/y}$ from speed', marker='*', markevery=150)
plt.semilogy(spepe**2, label='$\sigma_{x/y}$ from EPE', marker='x', markevery=150)
plt.semilogy(sp, label='Res.', marker='o', markevery=150)
plt.ylabel('Values for $R$ Matrix')
plt.xlabel('Filterstep $k$')
plt.xlim(0,6000)
#plt.legend(loc='best')
plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3,


### Attitude¶

Because the estimation of Roll and Pitch is only valid for quasistatic situations (which is not valid for a moving vehicle), the values for the measured rotations are dynamically chosen.

Uncertainty should be high when car is moving and very low, when the vehicle is standing still

$\sigma_\Theta=\sigma_\psi=\left[\rho+\gamma\cdot a\right]^2$

In [430]:
rho = 200.0
gamma=500.0

sroll = (rho + gamma*ay)**2
spitch= (rho + gamma*ax)**2

In [431]:
plt.figure(figsize=(6,2))
plt.semilogy(sroll, label='$\sigma_{\Theta}$', marker='o', markevery=150, alpha=0.8)
plt.semilogy(spitch, label='$\sigma_{\phi}$', marker='*', markevery=150, alpha=0.9)
plt.ylabel('Values for $R$ Matrix')
plt.xlabel('Filterstep $k$')
plt.xlim(0,6000)
plt.legend(bbox_to_anchor=(0.0, 1.02, 1., .102), loc=3,


## Measurement Function $h$¶

Matrix H is the Jacobian of the Measurement function h with respect to the state.

#### If GPS Measurement is available¶

In [432]:
hs = Matrix([[xs],[ys],[vs],[psis],[phis],[thetas]])
Hs=hs.jacobian(state)
Hs

Out[432]:
$$\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 1\end{matrix}\right]$$

Else set the elements to zero.

### Identity Matrix¶

In [433]:
I = np.eye(numstates)
print(I, I.shape)

(array([[ 1.,  0.,  0.,  0.,  0.,  0.],
[ 0.,  1.,  0.,  0.,  0.,  0.],
[ 0.,  0.,  1.,  0.,  0.,  0.],
[ 0.,  0.,  0.,  1.,  0.,  0.],
[ 0.,  0.,  0.,  0.,  1.,  0.],
[ 0.,  0.,  0.,  0.,  0.,  1.]]), (6, 6))


## Initial State¶

In [434]:
state

Out[434]:
$$\left[\begin{matrix}x\\y\\v\\\psi\\\phi\\\Theta\end{matrix}\right]$$
In [435]:
x = np.matrix([[mx[0], my[0], speed[0]/3.6, course[0]/180.0*np.pi+0.05, 0.0, 0.0]]).T
print(x, x.shape)

U=float(np.cos(x[3])*x[2])
V=float(np.sin(x[3])*x[2])

plt.quiver(x[0], x[1], U, V)
plt.scatter(float(x[0]), float(x[1]), s=100)
plt.title('Initial Location')
plt.axis('equal')

(matrix([[ 0.        ],
[ 0.        ],
[ 9.46111111],
[-3.45689007],
[ 0.        ],
[ 0.        ]]), (6, 1))

Out[435]:
$$\begin{pmatrix}-0.00015, & 0.00015, & -0.0003, & 0.0003\end{pmatrix}$$

## Initial Uncertainty¶

Initialized with $0$ means you are pretty sure where the vehicle starts and in which direction it is heading. Initialized with high values means, that you trust the measurements first, to align the state vector $x$ with them.

In [436]:
P = 1e5*np.eye(numstates)
print(P.shape)

fig = plt.figure(figsize=(numstates, numstates))
im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Covariance Matrix $P$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

plt.xlim([-0.5,5.5])
plt.ylim([5.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
plt.colorbar(im, cax=cax)

plt.tight_layout()

(6, 6)


## Put everything together as a measurement vector¶

In [437]:
state

Out[437]:
$$\left[\begin{matrix}x\\y\\v\\\psi\\\phi\\\Theta\end{matrix}\right]$$
In [438]:
measurements = np.vstack((mx, my, \
speed/3.6, \
(((course+180.0)%360.0-180.0))/180.0*np.pi, \
pitch/180.0*np.pi, \
roll/180.0*np.pi))
# Lenth of the measurement
m = measurements.shape[1]
print(measurements.shape)

(6, 6014)

In [439]:
# Preallocation for Plotting
x0 = []
x1 = []
x2 = []
x3 = []
x4 = []
x5 = []
x6 = []
x7 = []
x8 = []
Zx = []
Zy = []
P0 = []
P1 = []
P2 = []
P3 = []
P4 = []
P5 = []
P6 = []
P7 = []
P8 = []
K0 = []
K1 = []
K2 = []
K3 = []
K4 = []
K5 = []
dstate=[]

In [440]:
starttime = time.time()


# Extended Kalman Filter Step¶

$$x_k= \left[ \begin{matrix} x\\y\\ v \\ \psi\\\phi\\\Theta \end{matrix}\right] = \left[ \begin{matrix} \text{Position x} \\ \text{Position y} \\ \text{Speed} \\ \text{Heading} \\ \text{Pitch} \\ \text{Roll} \end{matrix}\right] = \underbrace{\begin{matrix}x[0] \\ x[1] \\ x[2] \\ x[3] \\ x[4] \\ x[5] \end{matrix}}_{\textrm{Python Nomenclature}}$$

In [441]:
for filterstep in range(m):

axc = -ax[filterstep]
yawc = yawrate[filterstep]/180.0*np.pi
pitc = pitchrate[filterstep]/180.0*np.pi
rolc = rollrate[filterstep]/180.0*np.pi

# Time Update (Prediction)
# ========================
# see "Dynamic Matrix"
if yawc==0.0: # Driving straight
x[0] = x[0] + x[2]*dt * np.cos(x[3])
x[1] = x[1] + x[2]*dt * np.sin(x[3])
x[2] = x[2] + axc*dt
x[3] = x[3]
x[4] = x[4] + pitc*dt
x[5] = x[5] + rolc*dt
yawc = 0.00000001 # to avoid numerical issues in Jacobians
dstate.append(0)
else: # otherwise
x[0] = x[0] + (x[2]/yawc) * (np.sin(yawc*dt+x[3]) - np.sin(x[3]))
x[1] = x[1] + (x[2]/yawc) * (-np.cos(yawc*dt+x[3])+ np.cos(x[3]))
x[2] = x[2] + axc*dt
x[3] = (x[3] + yawc*dt + np.pi) % (2.0*np.pi) - np.pi
x[4] = x[4] + pitc*dt
x[5] = x[5] + rolc*dt
dstate.append(1)

# Calculate the Jacobian of the Dynamic Matrix A
# see "Calculate the Jacobian of the Dynamic Matrix with respect to the state vector"
a13 = float((1.0/yawc) * (np.sin(yawc*dt+x[3]) - np.sin(x[3])))
a14 = float((x[2]/yawc)* (np.cos(yawc*dt+x[3]) - np.cos(x[3])))
a23 = float((1.0/yawc) * (-np.cos(yawc*dt+x[3]) + np.cos(x[3])))
a24 = float(x[2]/yawc) * (np.sin(yawc*dt+x[3]) - np.sin(x[3]))
JA = np.matrix([[1.0, 0.0, a13, a14, 0.0, 0.0],
[0.0, 1.0, a23, a24, 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, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

# Calculate the Jacobian of the Control Input G
# see "Calculate the Jacobian of the Dynamic Matrix with Respect to the Control"
g12 = float((dt*x[2]/yawc)*np.cos(yawc*dt+x[3]) - x[2]/yawc**2*(np.sin(yawc*dt+x[3])-np.sin(x[3])))
g22 = float((dt*x[2]/yawc)*np.sin(yawc*dt+x[3]) - x[2]/yawc**2*(-np.cos(yawc*dt+x[3])+np.cos(x[3])))
JG = np.matrix([[0.0, g12, 0.0, 0.0],
[0.0, g22, 0.0, 0.0],
[dt, 0.0, 0.0, 0.0],
[0.0, dt, 0.0, 0.0],
[0.0, 0.0, dt, 0.0],
[0.0, 0.0, 0.0, dt]])

# Project the error covariance ahead
P = JA*P*JA.T + JG*Q*JG.T

# Measurement Update (Correction)
# ===============================
hx = np.matrix([[float(x[0])],
[float(x[1])],
[float(x[2])],
[float(x[3])],
[float(x[4])],
[float(x[5])]])
# Because GPS is sampled with 10Hz and the other Measurements, as well as
# the filter are sampled with 50Hz, one have to wait for correction until
# there is a new GPS Measurement
if GPS[filterstep]:
# Calculate the Jacobian of the Measurement Function
# see "Measurement Matrix H"
JH = np.matrix([[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, 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, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
else:
# Calculate the Jacobian of the Measurement Function
# see "Measurement Matrix H"
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, 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, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

R[0,0] = sp[filterstep]       # x
R[1,1] = sp[filterstep]       # y
R[2,2] = spspeed[filterstep]  # v
R[3,3] = spspeed[filterstep]  # course
R[4,4] = spitch[filterstep]   # pitch
R[5,5] = sroll[filterstep]    # roll

S = JH*P*JH.T + R
K = (P*JH.T) * np.linalg.inv(S)

# Update the estimate via
z = measurements[:,filterstep].reshape(JH.shape[0],1)
y = z - (hx)                         # Innovation or Residual
x = x + (K*y)

# Update the error covariance
P = (I - (K*JH))*P

# Save states for Plotting
x0.append(float(x[0]))
x1.append(float(x[1]))
x2.append(float(x[2]))
x3.append(float(x[3]))
x4.append(float(x[4]))
x5.append(float(x[5]))

P0.append(float(P[0,0]))
P1.append(float(P[1,1]))
P2.append(float(P[2,2]))
P3.append(float(P[3,3]))
P4.append(float(P[4,4]))
P5.append(float(P[5,5]))

#Zx.append(float(z[0]))
#Zy.append(float(z[1]))

K0.append(float(K[0,0]))
K1.append(float(K[1,0]))
K2.append(float(K[2,0]))
K3.append(float(K[3,0]))
K4.append(float(K[4,0]))

In [442]:
print('One Filterstep took %.4fs (average) on MacBook Pro 2.5GHz Intel i5' % ((time.time() - starttime)/m))

One Filterstep took 0.0017s (average) on MacBook Pro 2.5GHz Intel i5


# Plots¶

In [443]:
%pylab inline --no-import-all

Populating the interactive namespace from numpy and matplotlib


## Uncertainties of Matrix $P$¶

In [444]:
fig = plt.figure(figsize=(16,9))
plt.semilogy(range(m),P0, label='$x$')
plt.step(range(m),P1, label='$y$')
plt.step(range(m),P2, label='$v$')
plt.step(range(m),P3, label='$\psi$')
plt.step(range(m),P4, label='$\phi$')
plt.step(range(m),P5, label='$\Theta$')

plt.xlabel('Filter Step [k]')
plt.ylabel('')
plt.xlim(0,6000)
plt.title('Uncertainty (Elements from Matrix $P$)')
#plt.legend(loc='best',prop={'size':22})
plt.legend(bbox_to_anchor=(0., 0.91, 1., .06), loc=3,
plt.savefig('Covariance-Matrix-Verlauf.eps', bbox_inches='tight')

In [445]:
fig = plt.figure(figsize=(numstates, numstates))
im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Covariance Matrix $P$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(np.arange(6), \
('$x$', '$y$', '$v$', '$\psi$', '$\phi$', '$\Theta$'),\
fontsize=22)

plt.xlim([-0.5,5.5])
plt.ylim([5.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())