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

Significant additions to the UKF chapter. Added UKF code.

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Designing_Kalman_Filters.ipynb
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",

10
Multidimensional_Kalman_Filters.ipynb
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

440
Unscented_Kalman_Filter.ipynb
View File

File diff suppressed because one or more lines are too long

4
stats.py
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."""

6
styles/custom2.css
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()