daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added plotting of covariance ellipses to 1D dog tracking, plus explanation.

Browse Source
This commit is contained in:
Roger Labbe 2014-05-12 13:16:17 -07:00
parent bae77c1d02
commit d56fb2a2da
3 changed files with 109 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
Multidimensional Kalman Filters.ipynb
View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:5065da4ac6135de8bf6874ebb71a6914d772f4084a148dbe9d30cddd37f45030"
"signature": "sha256:6db394babf486ee6d5f9640c3e0cef75aa5c8b7ee9cb936b34769b96656384ff"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -936,22 +936,9 @@
"cell_type": "code",
"collapsed": false,
"input": [
"from DogSensor import DogSensor\n",
"\n",
"def dog_tracking_filter(R,Q=0,cov=1.):\n",
" f = KalmanFilter (dim=2)\n",
" f.x = np.matrix([[0], [0]]) # initial state (location and velocity)\n",
" f.F = np.matrix([[1,1],[0,1]]) # state transition matrix\n",
" f.H = np.matrix([[1,0]]) # Measurement function\n",
" f.R = R # measurement uncertainty\n",
" f.P *= cov # covariance matrix \n",
" f.Q = Q\n",
" return f\n",
"\n",
"\n",
"def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):\n",
" dog = DogSensor(velocity=1, noise=noise)\n",
" f = dog_tracking_filter(R=R, Q=Q, cov=4.)\n",
" f = dog_tracking_filter(R=R, Q=Q, cov=20.)\n",
"\n",
" ps = []\n",
" zs = []\n",
@ -959,7 +946,6 @@
" for t in range (count):\n",
" z = dog.sense()\n",
" f.measure (z)\n",
" #print (t,z)\n",
" ps.append (f.x[0,0])\n",
" cov.append(f.P)\n",
" zs.append(z)\n",
@ -970,21 +956,54 @@
" p2, = plt.plot(range(1,count+1),ps, c='b')\n",
" plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2)\n",
" plt.title(title)\n",
" \n",
"\n",
" for i,p in enumerate(cov):\n",
" print(i,p)\n",
" if i==0:\n",
" e = stats.sigma_ellipse (p, i, i)\n",
" stats.plot_sigma_ellipse(e)\n",
" e = stats.sigma_ellipse (p, i+1, ps[i])\n",
" stats.plot_sigma_ellipse(e, axis_equal=False)\n",
" if i == len(cov)-1:\n",
" s = ('$\\sigma^2_{pos} = %.2f$' % p[0,0])\n",
" plt.text (30,1,s,fontsize=18)\n",
" s = ('$\\sigma^2_{vel} = %.2f$' % p[1,1])\n",
" plt.text (30,-4,s,fontsize=18)\n",
" plt.xlim((0,40))\n",
" plt.ylim((0,40))\n",
" plt.axis('equal')\n",
" \n",
" plt.show()\n",
"\n",
" \n",
"plot_track (noise=30, R=5, count=2)"
"\n",
"plot_track (noise=5, R=5, Q=5, count=20, title='R = 5')\n",
"plot_track (noise=5, R=.5, Q=5, count=20, title='R = 0.5')"
],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The output on these is a bit messy, but you should be able to see what is happening. In both plots we are drawing the covariance matrix for each point. We start with the covariance $P=(\\begin{smallmatrix}50&0\\\\0&50\\end{smallmatrix})$, which signifies a lot of uncertainty about our initial belief. After we receive the first measurement the Kalman filter updates this belief, and so the variance is no longer as large. In the top plot the first ellipse (the one on the far left) should be a slighly squashed ellipse. As the filter continues processing the measurements the covariance ellipse quickly shifts shape until it settles down to being a long, narrow ellipse tilted in the direction of movement.\n",
"\n",
"Think about what this means physically. The x-axis of the ellipse denotes our uncertainty in position, and the y-axis our uncertainty in velocity. So, an ellipse that is taller than it is wide signifies that we are more uncertain about the velocity than the position. Conversely, a wide, narrow ellipse shows high uncertainty in position and low uncertainty in velocity. Finally, the amount of tilt shows the amount of correlation between the two variables. \n",
"\n",
"The first plot, with $R=5$, finishes up with an ellipse that is wider than it is tall. If that is not clear I have printed out the variances for the last ellipse in the lower right hand corner. The variance for position is 3.85, and the variance for velocity is 3.0. \n",
"\n",
"In contrast, the second plot, with $R=0.5$, has a final ellipse that is taller than wide. The ellipses in the second plot are all much smaller than the ellipses in the first plot. This stands to reason because a small $R$ implies a small amount of noise in our measurements. Small noise means accurate predictions, and thus a strong belief in our position. \n",
"\n",
"** EXPLAIN WHY SECOND PLOT ELLIPSE IS TALLER THAN WIDE!!!**\n",
"\n",
"Keep looking at these plots until you grasp how the covariance matrix $P$ has a real, physical interpretation. When you start dealing with a, say, $9\\times 9$ matrix it may seem overwhelming - there are 81 numbers to interpret. Just break it down - the diagonal contains the variance for each state variable, and all off diagonal elements are the product of two variances and a scaling factor $p$. You will not be able to plot a $9\\times 9$ matrix on the screen because it would require living in 10-D space, so you have to develop your intution and understanding in this simple, 2-D case. \n",
"\n",
"> **sidebar**: when plotting covariance ellipses, make sure to always use *plt.axis('equal')* in your code. If the axis use different scales the ellipses will be drawn distorted. For example, the ellipse may be drawn as being taller than it is wide, but it may actually be wider than tall.\n",
"\n",
"** I am confused. formula for P suggests correlation is pre-ordained. why would correlation between, say x'' and Z be the same as the correlation between x' and y''? Sure, # will be different, but p is the same for both. I confused.**\n",
"\n",
"** Question: why can't I effect the velocity variance directly? Is it just because it is unobserved **\n",
"\n",
"** question: why are Q and R scalar? Are they always scalar? **\n"
]
},
{
"cell_type": "code",
"collapsed": false,

55
mkf_ellipse_test.py Normal file
View File

@ -0,0 +1,55 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 11 20:47:52 2014
@author: rlabbe
"""
from DogSensor import DogSensor
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import stats
def dog_tracking_filter(R,Q=0,cov=1.):
f = KalmanFilter (dim=2)
f.x = np.matrix([[0], [0]]) # initial state (location and velocity)
f.F = np.matrix([[1,1],[0,1]]) # state transition matrix
f.H = np.matrix([[1,0]]) # Measurement function
f.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):
dog = DogSensor(velocity=1, noise=noise)
f = dog_tracking_filter(R=R, Q=Q, cov=10.)
ps = []
zs = []
cov = []
for t in range (count):
z = dog.sense()
f.measure (z)
#print (t,z)
ps.append (f.x[0,0])
cov.append(f.P)
zs.append(z)
f.predict()
p0, = plt.plot([0,count],[0,count],'g')
p1, = plt.plot(range(1,count+1),zs,c='r', linestyle='dashed')
p2, = plt.plot(range(1,count+1),ps, c='b')
plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2)
plt.title(title)
for i,p in enumerate(cov):
print(i,p)
e = stats.sigma_ellipse (p, i+1, ps[i])
stats.plot_sigma_ellipse(e, axis_equal=False)
plt.xlim((-1,count))
plt.show()
plot_track (noise=30, R=5, Q=2, count=5)

18
stats.py
View File

@ -37,7 +37,7 @@ def multivariate_gaussian(x, mu, cov):
The function gaussian() implements the 1D (univariate)case, and is much
faster than this function.
"""
# force all to numpy.array type
x = np.array(x, copy=False, ndmin=1)
mu = np.array(mu,copy=False, ndmin=1)
@ -54,8 +54,8 @@ def multivariate_gaussian(x, mu, cov):
numerator = np.linalg.solve(cov, err).T.dot(err)
return math.exp(-0.5*(norm_coeff + numerator))
def norm_plot(mean, var):
min_x = mean - var * 1.5
max_x = mean + var * 1.5
@ -75,6 +75,8 @@ def sigma_ellipse(cov, x=0, y=0, sigma=1, num_pts=100):
The ellipse is a 2D numpy array with shape (2, num_pts). Row 0 contains the
x components, and row 1 contains the y coordinates
"""
cov = np.asarray(cov)
L = linalg.cholesky(cov)
t = np.linspace(0, _two_pi, num_pts)
unit_circle = np.array([np.cos(t), np.sin(t)])
@ -85,6 +87,8 @@ def sigma_ellipse(cov, x=0, y=0, sigma=1, num_pts=100):
return (ellipse,x,y)
def sigma_ellipses(cov, x=0, y=0, sigma=[1,2], num_pts=100):
cov = np.asarray(cov)
L = linalg.cholesky(cov)
t = np.linspace(0, _two_pi, num_pts)
unit_circle = np.array([np.cos(t), np.sin(t)])
@ -97,15 +101,17 @@ def sigma_ellipses(cov, x=0, y=0, sigma=[1,2], num_pts=100):
e_list.append (ellipse)
return (e_list,x,y)
def plot_sigma_ellipse(ellipse,title=None):
def plot_sigma_ellipse(ellipse, title=None, axis_equal=True):
""" plots the ellipse produced from sigma_ellipse."""
plt.axis('equal')
if axis_equal:
plt.axis('equal')
e = ellipse[0]
x = ellipse[1]
y = ellipse[2]
plt.plot(e[0], e[1])
plt.plot(e[0], e[1],c='b')
plt.scatter(x,y,marker='+') # mark the center
if title is not None:
plt.title (title)