Significant additions to the UKF chapter. Added UKF code.

This commit is contained in:
Roger Labbe 2014-05-28 19:16:41 -07:00
parent 90e9f57ab6
commit 87e7903e08
7 changed files with 601 additions and 32 deletions

View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:e2baeba25a3174dd9f44aa961606fbda4ef44c61e180085a8179c69f0e69bad4"
"signature": "sha256:47e47ce6c78a985606fd325605b42a0e55e381111a625eb69820fb2771542ce2"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -53,7 +53,7 @@
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 36pt;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
@ -63,7 +63,6 @@
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-indent:1em;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
@ -71,8 +70,8 @@
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 1.5em;\n",
" margin-top: 0.5em;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" } \n",
@ -243,13 +242,13 @@
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 2,
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x19b9390>"
"<IPython.core.display.HTML at 0x1ae78d0>"
]
}
],
"prompt_number": 2
"prompt_number": 1
},
{
"cell_type": "heading",

View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:254e89db7aa7d5d122fc4ca120623bf57e0bbb51df939e2b783e03617022220b"
"signature": "sha256:00dc5e666bed6e6a433a1c086c374cef621d05ba381473abe5a36db5a5cd4ef7"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -53,7 +53,7 @@
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 36pt;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
@ -70,8 +70,8 @@
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 1.5em;\n",
" margin-top: 0.5em;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" } \n",
@ -244,7 +244,7 @@
"output_type": "pyout",
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x127a950>"
"<IPython.core.display.HTML at 0x1353950>"
]
}
],

84
UKF.py Normal file
View File

@ -0,0 +1,84 @@
# -*- coding: utf-8 -*-
"""
Created on Wed May 28 17:40:19 2014
@author: rlabbe
"""
from numpy import matrix, zeros, asmatrix, size
from numpy.linalg import cholesky
def sigma_points (mu, cov, kappa):
""" Computes the sigma points and weights for an unscented Kalman filter.
xm are the means, and P is the covariance. kappa is an arbitrary constant
constant. Returns tuple of the sigma points and weights.
This is the original algorithm as published by Julier and Uhlmann.
Later algorithms introduce more parameters - alpha, beta,
Works with both scalar and array inputs:
sigma_points (5, 9, 2) # mean 5, covariance 9
sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
"""
mu = asmatrix(mu)
cov = asmatrix(cov)
n = size(mu)
# initialize to zero
Xi = asmatrix (zeros((n,2*n+1)))
W = asmatrix (zeros(2*n+1))
# all weights are 1/ 2(n+kappa)) except the first one.
W[0,1:] = 1. / (2*(n+kappa))
W[0,0] = float(kappa) / (n + kappa)
# use cholesky to find matrix square root of (n+kappa)*cov
# U'*U = (n+kappa)*P
U = asmatrix (cholesky((n+kappa)*cov))
# mean is in location 0.
Xi[:,0] = mu
for k in range (n):
Xi[:,k+1] = mu + U[:,k]
for k in range (n):
Xi[:, n+k+1] = mu - U[:,k]
return (Xi, W)
def unscented_transform (Xi, W, NoiseCov=None):
""" computes the unscented transform of a set of signma points and weights.
returns the mean and covariance in a tuple
"""
W = asmatrix(W)
Xi = asmatrix(Xi)
n, kmax = Xi.shape
# initialize results to 0
mu = matrix (zeros((n,1)))
cov = matrix (zeros((n,n)))
for k in range (kmax):
mu += W[0,k] * Xi[:,k]
for k in range (kmax):
cov += W[0,k]*(Xi[:,k]-mu) * (Xi[:,k]-mu).T
return (mu, cov)
xi = matrix ([[1,2,3,4,5,6,7],[1,2,3,4,5,6,7],[1,2,3,4,5,6,7]])
mu = matrix ([1,2,3,4,5,6,7])
m,c = unscented_transform(xi, mu)
print m
print c

File diff suppressed because one or more lines are too long

View File

@ -61,7 +61,7 @@ def norm_plot(mean, var):
max_x = mean + var * 1.5
xs = np.arange(min_x, max_x, 0.1)
ys = [gaussian(x,23,5) for x in xs]
ys = [gaussian(x,mean,var) for x in xs]
plt.plot(xs,ys)
@ -106,7 +106,7 @@ def plot_covariance_ellipse (cov, x=0, y=0, sigma=1,title=None, axis_equal=True)
"""
e = sigma_ellipse (cov, x, y, sigma)
plot_sigma_ellipse(e, title, axis_equal)
def plot_sigma_ellipse(ellipse, title=None, axis_equal=True):
""" plots the ellipse produced from sigma_ellipse."""

View File

@ -19,7 +19,7 @@
.text_cell_render h1 {
font-weight: 200;
font-size: 36pt;
font-size: 30pt;
line-height: 100%;
color:#c76c0c;
margin-bottom: 0.5em;
@ -36,8 +36,8 @@
font-style: italic;
line-height: 100%;
color:#c76c0c;
margin-bottom: 1.5em;
margin-top: 0.5em;
margin-bottom: 0.5em;
margin-top: 1.5em;
display: block;
white-space: nowrap;
}

74
ukf_internal.py Normal file
View File

@ -0,0 +1,74 @@
# -*- coding: utf-8 -*-
"""
Created on Tue May 27 21:21:19 2014
@author: rlabbe
"""
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse,Arrow
import stats
import numpy as np
def arrow(x1,y1,x2,y2):
return Arrow(x1,y1, x2-x1, y2-y1, lw=2, width=0.1, ec='k', color='k')
def show_2d_transform():
ax=plt.gca()
ax.add_artist(Ellipse(xy=(2,5), width=2, height=3,angle=70,linewidth=1,ec='k'))
ax.add_artist(Ellipse(xy=(7,5), width=2.2, alpha=0.3, height=3.8,angle=150,linewidth=1,ec='k'))
ax.add_artist(arrow(2, 5, 6, 4.8))
ax.add_artist(arrow(1.5, 5.5, 7, 3.8))
ax.add_artist(arrow(2.3, 4.1, 8, 6))
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
plt.axis('equal')
plt.xlim(0,10); plt.ylim(0,10)
plt.show()
def show_3_sigma_points():
xs = np.arange(-4, 4, 0.1)
var = 1.5
ys = [stats.gaussian(x, 0, var) for x in xs]
samples = [0, 1.2, -1.2]
for x in samples:
plt.scatter ([x], [stats.gaussian(x, 0, var)], s=80)
plt.plot(xs, ys)
plt.show()
def show_sigma_selections():
ax=plt.gca()
ax.add_artist(Ellipse(xy=(2,5), alpha=0.5, width=2, height=3,angle=0,linewidth=1,ec='k'))
ax.add_artist(Ellipse(xy=(5,5), alpha=0.5, width=2, height=3,angle=0,linewidth=1,ec='k'))
ax.add_artist(Ellipse(xy=(8,5), alpha=0.5, width=2, height=3,angle=0,linewidth=1,ec='k'))
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
plt.scatter([1.5,2,2.5],[5,5,5],c='k', s=50)
plt.scatter([2,2],[4.5, 5.5],c='k', s=50)
plt.scatter([4.8,5,5.2],[5,5,5],c='k', s=50)
plt.scatter([5,5],[4.8, 5.2],c='k', s=50)
plt.scatter([7.2,8,8.8],[5,5,5],c='k', s=50)
plt.scatter([8,8],[4,6],c='k' ,s=50)
plt.axis('equal')
plt.xlim(0,10); plt.ylim(0,10)
plt.show()
if __name__ == '__main__':
show_sigma_selections()