Significant additions to the UKF chapter. Added UKF code.
This commit is contained in:
parent
90e9f57ab6
commit
87e7903e08
@ -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",
|
||||
|
@ -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
84
UKF.py
Normal 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
4
stats.py
4
stats.py
@ -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."""
|
||||
|
@ -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
74
ukf_internal.py
Normal 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()
|
||||
|
Loading…
Reference in New Issue
Block a user