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 committed 2014-05-28 19:16:41 -07:00
1 parent 90e9f57ab6
commit 87e7903e08
7 files changed
+601 -32
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Designing_Kalman_Filters.ipynb
+7 -8
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",
Multidimensional_Kalman_Filters.ipynb
+5 -5
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>"
]
}
],
UKF.py
+84
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
Unscented_Kalman_Filter.ipynb
+426 -14
View File
@@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:5478490f2384cc166dd45368779e2c93fa7ab3ef0d0a4ef4c3f103c9c96f1a9a"
"signature": "sha256:388572360fc3834b2b47f1ac29bc861fe9c4c097c10c029badd3c3aab4674e6c"
},
"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": 1,
"prompt_number": 2,
"text": [
"<IPython.core.display.HTML at 0x1e308d0>"
"<IPython.core.display.HTML at 0x14b4190>"
]
}
],
"prompt_number": 1
"prompt_number": 2
},
{
"cell_type": "markdown",
@@ -258,7 +257,7 @@
"In the previous chapter we developed the Extended Kalman Filter to allow us to use the Kalman filter with nonlinear problems. It is by far the most commonly used Kalman filter. However, it requires that you be able to analytically derive the Jacobian blah blah limp prose.\n",
"\n",
"\n",
"However, for many problems finding the Jacobian is either very difficult or impossible. Futhermore, being an approximation, the EKF can diverge. For all these reasons there is a need for a different way to approximate the Gaussian being passed through a nonlinear transfer function. Recall the result of this transform:"
"However, for many problems finding the Jacobian is either very difficult or impossible. Futhermore, being an approximation, the EKF can diverge. For all these reasons there is a need for a different way to approximate the Gaussian being passed through a nonlinear transfer function. In the last chapter I showed you this plot:"
]
},
{
@@ -269,7 +268,7 @@
"from numpy.random import normal\n",
"import numpy as np\n",
"\n",
"data = normal(loc=0.0, scale=1, size=5000000)\n",
"data = normal(loc=0.0, scale=1, size=500000)\n",
"\n",
"def g(x):\n",
" return (np.cos(4*(x/2+0.7)))*np.sin(0.3*x)-1.6*x\n",
@@ -282,9 +281,9 @@
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAr0AAAF9CAYAAAAJJNDxAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"png": "iVBORw0KGgoAAAANSUhEUgAAAW4AAAENCAYAAAAvwo97AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x3160910>"
"<matplotlib.figure.Figure at 0x2524fd0>"
]
}
],
@@ -294,9 +293,422 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"For the Kalman filter predict-update cycle to work, we need an approximation of the mean and variance of the Gaussian being passed through the transfer function. The EKF solves this by performing a Taylor Series expansion...\n",
"\n"
"I generated this by taking 500,000 samples from the input, passing it through the nonlinear tranform, and building a histogram of the result. From that histogram we can then compute a mean and a variance that we compared to the output of the EKF.\n",
"\n",
"It has perhaps occured to you that this sampling process constitutes a solution to our problem. This is called a 'monte carlo' approach, and it used by some Kalman filter designs, such as the *Ensemble filter*. Sampling requires no specialized knowledge programming, and does not require a closed form solution. No matter how nonlinear or poorly behaved the tranfer function is, as long as we sample with enough points we will build an accurate output distribution.\n",
"\n",
"\"Enought points\" is the rub. The graph above was created with 500,000 points, and the output is still not smooth. You wouldn't need to use that many points to get a reasonable estimate of the mean and variance, but it will require many points. What's worse, this is only for 1 dimension. In general, the number of points required increases by the power of the number of dimensions. If you need 50 points for 1 dimension, you need 50^2 for two dimensions, 50^3 for three dimensions, and so on. So while this approach does work, it is very computationally expensive. The Unscented Kalman filter, the topic of this chapter, uses a somewhat similar technique but reduces the amount of computation needed by a drastic amount. \n",
"\n",
"It is somewhat hard to understand some aspects of this problem by looking at the histogram, so consider this alternative representation, this time for 2 variables/dimensions."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import ukf_internal\n",
"ukf_internal.show_2d_transform()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAV0AAADtCAYAAAAcNaZ2AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x14b8410>"
]
}
],
"prompt_number": 4
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here on the left we show an ellipse depicting the $1\\sigma$ distribution of two variables. The arrows show how three randomly sampled points might be transformed by some arbitrary nonlinear function to a new distribution. The ellipse on the right is drawn semi-transparently to indicate that it is an *estimate* of the mean and variance of this collection of points - if we were to sample, say, a million points the shape of the points might be very far from an ellipse. "
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Choosing Sigma Points"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So what would be fewest number of sampled points that we can use, and what kinds of constraints does this problem formulation put on the points? We will assume that we have no special knowledge about the nonlinear tranform as we want to find a generalized algorithm. For reasons that come clear in the next section, we will call these points *sigma points*.\n",
"\n",
"Let's consider the simplest possible case, and see if it offers any insight. The simplest possible system is *identity* - the transformation does not alter the input. It should be clear that if this does not work then the filter will never converge.\n",
"\n",
"The fewest number of points that we can use is one per dimension. This is the number that the linear Kalman filter uses. The input to a Kalman filter for the distribution $\\mathcal{N}(\\mu,\\sigma^2)$ is just $\\mu$ itself. So while this works for the linear case, it is not a good answer for the nonlinear case.\n",
"\n",
"If we were to pass some value $\\mu+\\Delta$ instead, the *identity* system would not converge, so this is not a possible algorithm. Since we cannot set our one point sample to $\\mu$, or any value that is not $\\mu$, we must conclude that a one point sample will not work.\n",
"\n",
"So, what is the next lowest number we can choose? Consider the fact that Gaussians are symmetric, and that we probably want to always have one of our sample points be the mean of the input. Two points would require us to select the mean, and then one other point. That one other point would introduce an asymmetry in our input that we probably don't want. I recognize that this is rather vague, but I don't want to spend a lot of time on a scheme that doesn't work. \n",
"\n",
"The next lowest number is 3 points. 3 points allows us to select the mean, and then one point on each side of the mean, as depicted on the chart below."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"ukf_internal.show_3_sigma_points()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAX0AAAECCAYAAAASDQdFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x2781810>"
]
}
],
"prompt_number": 5
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For this to work for all *identity* cases we will want the sums of the weights to equal one. We can always come up with counterexamples, but in general if the sum is greater or less than one the sampling will not yield the correct output. Given that, we then have to select *sigma points* $\\mathcal{X}$ and their corresponding weights so that they compute to the mean and variance of the input Gaussian. So we can write\n",
"\n",
"$$\\begin{aligned}\n",
"1 &= \\sum_i{w_i}\\;\\;\\;&(1) \\\\\n",
"\\mu &= \\sum_i w_i\\mathcal{X}_i\\;\\;\\;&(2) \\\\\n",
"\\Sigma &= \\sum_i w_i{(\\mathcal{X}_i-\\mu)(\\mathcal{X}_i-\\mu)^T}\\;\\;\\;&(3)\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"If we look at this is should be clear that there is no one unique answer - the problem is unconstrained. For example, if you choose a smaller weight for the point at the mean for the input, you could compensate by choosing larger weights for the rest of the $\\mathcal{X}$, and vice versa if you chose a larger weight for it. Indeed, these equations do not require that any of the points be the mean of the input at all, though it seems 'nice' to do so, so to speak.\n",
"\n",
"Methods for selecting these sigma points is it own topic. In the next section I will develop the most typically used method in practice. It has the virtue of requiring only 3 sigma points per dimension, which is far lower than we might expect to provide good results. Despite the low number of points, the computations for the weight selections are very easy and efficient, and the numerical performance of the filter is as good as, and usually better than the EKF.\n",
"\n",
"But before we go on I want to make sure the idea is clear. We are choosing 3 points for each dimension in our covariances. That choice is *entirely deterministic*. Below are three different examples for the same covariance ellipse."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"ukf_internal.show_sigma_selections()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAV0AAADtCAYAAAAcNaZ2AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x25bcb90>"
]
}
],
"prompt_number": 6
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Note that while I chose the points to lie along the major and minor axis of the ellipse, nothing in the constraints above require me to do that; however, it is fairly typical to do this. Furthermore, in each case I show the points evenly spaced; again, the constraints above do not require that. However, the technique that we develop in the next section *does* do this. It is a reasonable choice, after all; if we want to accurately sample our input it makes sense to sample in a symmetric manner."
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"The Unscented Transform"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So our desire is to have an algorithm for selecting sigma points based on some criteria. Maybe we know something about our nonlinear problem, and we know we want our sigma points to be very close together, or very far apart. Or through experimentation we decide that a certain choice of basis vectors from our hyperellipse are the best axis to choose our sigma points from. But we want this to be an algorithm - we don't want to have to hard code in a specific selection algorithm for each different problem. So we are going to want to be able to set some parameters to tell the algorithm how to automatically select the points and weights for us. That may seem a bit abstract, so let's just launch into it, and try to develop an intuitive understanding as we go.\n",
"\n",
"Our first choice is always going to be the mean of our input. We will number this $\\mathcal{X}_0$. So,\n",
"\n",
"$$ \\mathcal{X}_0 = \\mu$$\n",
"\n",
"So for each dimension we need to select 2 more points. We want them to be symmetric around the mean so that for the linear case they cancel out and we are just left with the mean as the result. Here is how we are going to do that:\n",
"\n",
"\n",
"$$ \n",
"\\begin{aligned}\n",
"\\mathcal{X}_i &= \\mu + (\\sqrt{(n+\\lambda)\\Sigma})_i\\,\\,\\,\\, &\\text{for i=1..n} \\\\\n",
"\\mathcal{X}_i &= \\mu - (\\sqrt{(n+\\lambda)\\Sigma})_{i-n}\\,\\,\\,\\, &\\text{for i=n+1..2n}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"Let's talk through this. For the moment, just think of this in one dimension, so we can ignore the subscript $i$. $n$ is just our dimensionality, and $\\lambda$ is a scaling factor that controls how far away from the mean we want the points to be. A larger lambda will choose points further away from the mean, and a smaller lambda will choose points nearer the mean. So in one dimension we get something like:\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import stats\n",
"import math\n",
"\n",
"# generate the Gaussian data\n",
"\n",
"xs = np.arange(-4, 4, 0.1)\n",
"mean = 0\n",
"sigma = 1.5\n",
"ys = [stats.gaussian(x, mean, sigma*sigma) for x in xs]\n",
"\n",
"def sigma_points (mean, sigma, lambda_):\n",
" sigma1 = mean + math.sqrt ((1+lambda_)*sigma) \n",
" sigma2 = mean - math.sqrt ((1+lambda_)*sigma) \n",
" return mean, sigma1, sigma2\n",
" \n",
"#generate our samples\n",
"lambda_ = 1.4\n",
"x0,x1,x2 = sigma_points (mean,sigma,lambda_)\n",
"\n",
"samples = [x0,x1,x2]\n",
"for x in samples:\n",
" p1 = plt.scatter ([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='k')\n",
"\n",
"lambda_ = 0.5\n",
"x0,x1,x2 = sigma_points (mean,sigma,lambda_)\n",
"\n",
"samples = [x0,x1,x2]\n",
"for x in samples:\n",
" p2 = plt.scatter ([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='b')\n",
"\n",
"plt.legend([p1,p2],['$\\lambda$=1.4', '$\\lambda$=0.5'])\n",
"plt.plot(xs, ys)\n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAX0AAAECCAYAAAASDQdFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x2783310>"
]
}
],
"prompt_number": 7
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here I have plotted two different choices for lambda - 05 and 1.4 - to show how lambd affects the distribution of the points. \n",
"\n",
"Perhaps the reason for calling these *sigma points* is clear. While we are not selecting exactly $1\\sigma$ for the position of our points, we are scaling by some factor of $\\sigma$.\n",
"\n",
"\n",
"On to larger dimensions ($n>1$). The term $(\\sqrt{(n+\\lambda)\\Sigma})_i$ has to be a matrix because $\\Sigma$ is a matrix. The subscript $i$ is choosing the column vector of the matrix. And thus the square root is not the square root of a scalar, but of a matrix.\n",
"\n",
"What is the 'square root of a matrix'? The usual definition is that the square root of a matrix $\\Sigma$ is just the matrix $S$ that, when multiplied by itself, yields $\\Sigma$.\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\text{if }\\Sigma = SS \\\\\n",
"\\\\\n",
"\\text{then }S = \\sqrt{\\Sigma}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"However, there is an alternative definition, and we will chose that because it has numerical properties that makes it much easier for us to compute its value. We can alternatively define the square root as a matrix S, when multiplied by its transpose, returns $\\Sigma$:\n",
"$$\n",
"\\Sigma = SS^T \\\\\n",
"$$\n",
"\n",
"If this makes you uncomfortable, I advise not worrying about it. We are just defining an algorithm for choosing our sigma points; if the 'square' root in method of computation ($SS$) is slightly different than the other case ($SS^T), it shouldn't really matter. Plus, this still works for the one dimensional case, as the transpose of a scalar is just the scalar. \n",
"\n",
"I will derive the math for computing this in a later section in this chapter. For now I will say that this is a well worn area of linear algebra, and performing this 'square root' uses something called the Cholesky decomposition. This is a library procedure that is available in any linear algebra library. For example, numpy provides it in $\\verb ,numpy.linalg.cholesky(),$, and we will be using that function in our code. If your language of choice is Fortran, C, C++, or the like the standard libraries like LAPACK also provide this routine. And, of course, matlab provides $\\tt chol()$, which does the same thing."
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Implementation"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So let's just implement this algorithm. First, let's write the code to compute the mean and covariance given the sigma points. Unfortunately, this requires a short diversion into the intricacies of numpy's data types. Through historical accidents, numpy provides several data structures to represent matrices, and they do not 'play well' with each other. Most sources recommend using the type $\\verb,numpy.array,$, however, it is quite cumbersome to use for intensive linear algebra. Since everything I do in this book is linear algebra, I choose to use $\\verb,numpy.matrix,$ instead. Fortunately, numpy provides $\\verb,numpy.asmatrix(),$. It will convert a variable to $\\verb,numpy.matrix,$ if it is not already one; better yet, it will turn a scalar into a $1{\\times}1$ matrix. The result is that we can pass scalars, $\\verb,numpy.array,$'s, or $\\verb,numpy.matrix,$s into my code and it will work.\n",
"\n",
"So we will store the sigma points and weights in matrices, like so:\n",
"\n",
"$$ \n",
"\\begin{aligned}\n",
"weights &= \n",
"\\begin{bmatrix}\n",
"w_1&w_2& \\dots & w_n\n",
"\\end{bmatrix} \n",
"\\\\\n",
"sigmas &= \n",
"\\begin{bmatrix}\n",
"\\mathcal{X}_{0,0} & \\mathcal{X}_{0,1} & \\dots & \\mathcal{X}_{0,n} \\\\\n",
"\\mathcal{X}_{1,0} & \\mathcal{X}_{1,1} & \\dots & \\mathcal{X}_{1,n} \\\\\n",
"\\mathcal{X}_{2,0} & \\mathcal{X}_{2,1} & \\dots & \\mathcal{X}_{2,n}\n",
"\\end{bmatrix}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"In other words, each column contains the 3 sigma points for one dimension in our problem. The $0th$ sigma point is always the mean, so first row of sigma's contains the mean of each of our dimensions. The second row contains the $\\mu+(\\sqrt{(n+\\lambda)\\Sigma})$ term, and the third row contains the $\\mu-(\\sqrt{(n+\\lambda)\\Sigma})$ term."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import numpy as np\n",
"\n",
"def unscented_transform (Xi, W, NoiseCov=None):\n",
" \"\"\" computes the unscented transform of a set of signma points and weights.\n",
" returns the mean and covariance in a tuple\n",
" \"\"\"\n",
" W = np.asmatrix(W)\n",
" Xi = np.asmatrix(Xi)\n",
"\n",
" n, kmax = Xi.shape\n",
"\n",
" # initialize results to 0\n",
" mu = np.mat (np.zeros((n,0)))\n",
" cov = np.mat (np.zeros((n,n)))\n",
"\n",
" for k in range (kmax):\n",
" mu += W[0,k] * Xi[:,k]\n",
"\n",
" for k in range (kmax):\n",
" cov += W[0,k]*(Xi[:,k]-xm) * (Xi[:,k]-xm).T\n",
"\n",
" return (mu, cov)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 8
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's review a bit of numpy nomenclature. The odd looking\n",
"\n",
" Xi[:,i]\n",
" \n",
"term is merely saying 'take the *ith* column of Xi'. So This produces a $1{\\times}n$ column vector, like so:\n",
"\n",
"$$\n",
"\\begin{bmatrix}\n",
"\\mathcal{X}_{0,i} \\\\\n",
"\\mathcal{X}_{1,i} \\\\\n",
"\\mathcal{X}_{2,i} \\\\\n",
"\\vdots \\\\\n",
"\\mathcal{X}_{n,i} \n",
"\\end{bmatrix}\n",
"$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This is confusing if you haven't seen it before so I will belabor this point. Read this as $\\tt Xi[A,B]$, where $\\tt A$ indexes the row, and $\\tt B$ the column. In this case the row value is ':', which is Python's way to taking a *slice*. For example, for a one dimensional list $\\tt x$, $\\tt x[3:5]$ returns a list containing $\\tt x[3]$ and $\\tt x[4]$. If you leave one of the slice elements out it just selects everything on that side of the slice, so $\\tt x[3:]$ would return a list of elements number 2 to the last element in $\\tt x$. Therefore, $\\tt x[:]$ just returns $\\tt x$ itself - it asks for all elements in $\\tt x$. So, in a form like $\\tt Xi[:,i]$ we take all rows from the ith column; effectively, a 1xn column vector. \n",
"\n",
"\n",
"We write this way in numpy for two reasons. First, it is very close to the mathematical notation our equations use. Second, and more importantly, when you write this way numpy uses C routines to implement the functionality, making the code much faster. Avoiding for loops in numerical code is 'Pythonic'. If you are used to packages like Matlab you are already familar with notation; if you are coming from a language like C recognize I could have written this loop:\n",
"\n",
" for k in range (kmax):\n",
" mu += W[0,k] * Xi[:,k]\n",
" \n",
"as:\n",
"\n",
" for k in range (kmax):\n",
" for i in range (n):\n",
" mu += W[0,k] * Xi[i,k]\n",
"\n",
"\n",
"\n",
"So if you look at this code you should see that it is just performing the computations\n",
"$$\\begin{aligned}\n",
"\\mu &= \\sum_i w_i\\mathcal{X}_i \\\\\n",
"\\Sigma &= \\sum_i w_i{(\\mathcal{X}_i-\\mu)(\\mathcal{X}_i-\\mu)^T}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"Even if you are not a strong numpy programmer it should be reasonably clear that this is not a difficult bit of code. Furthermore, as with our previous Kalman filter codes, we just need to write it once - we will call this function for *any* problem we are solving, and it will perform the Unscented transform for us."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We are half done! Now we need to write the function that selects the sigma points based on this term $\\sqrt{(n+\\lambda)\\Sigma}$. It is a bit longer, but still straightforward."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"def sigma_points (mu, P, kappa):\n",
" \"\"\" Computes the sigma points and weights for an unscented Kalman filter.\n",
" xm are the means, and P is the covariance. kappa is an arbitrary constant\n",
" constant. Returns tuple of the sigma points and weights.\n",
"\n",
" Works with scalar, array, and matrix inputs:\n",
" sigma_points (5, 9, 2) # mean 5, covariance 9\n",
" sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I\n",
" \"\"\"\n",
"\n",
" mu = asmatrix(mu)\n",
" cov = asmatrix(cov)\n",
"\n",
" n = size(mu)\n",
"\n",
" # initialize to zero\n",
" Xi = asmatrix (zeros((n,2*n+1)))\n",
" W = asmatrix (zeros(2*n+1))\n",
"\n",
"\n",
" # all weights are 1/ 2(n+kappa)) except the first one.\n",
" W[0,1:] = 1. / (2*(n+kappa))\n",
" W[0,0] = float(kappa) / (n + kappa)\n",
"\n",
"\n",
" # use cholesky to find matrix square root of (n+kappa)*cov\n",
" # U'*U = (n+kappa)*P\n",
" U = asmatrix (cholesky((n+kappa)*cov))\n",
"\n",
" # mean is in location 0.\n",
" Xi[:,0] = mu\n",
"\n",
" for k in range (n):\n",
" Xi[:,k+1] = mu + U[:,k]\n",
"\n",
" for k in range (n):\n",
" Xi[:, n+k+1] = mu - U[:,k]\n",
"\n",
" return (Xi, W)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 10
}
],
"metadata": {}
stats.py
+2 -2
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."""
styles/custom2.css
+3 -3
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;
}
ukf_internal.py
+74
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()