Major work on the UKF chapter.
Text is still not organized well, but I got the unscented transform fully documented. Probably needs more citations to the original article, and some revisions - I think there is some duplication of info.
This commit is contained in:
Roger Labbe committed
1 parent
23e283e7fe
commit
e5f968a2e3
2 files changed
+261
-193
No files matched your search
+229
-192
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:508bfe56a37ac55e285aada1ed5172acfa5e46822f874f77b092acceb435750c"
|
||||
"signature": "sha256:a9ba6c3ba612925867d9e360943975b2d0084c56ca20db167d224fb4e3909bf9"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@@ -44,7 +44,7 @@
|
||||
" background: transparent;\n",
|
||||
" color: #000000;\n",
|
||||
" font-weight: 600;\n",
|
||||
" font-size: 13pt;\n",
|
||||
" font-size: 11pt;\n",
|
||||
" font-style: bold;\n",
|
||||
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
|
||||
" }\n",
|
||||
@@ -250,7 +250,7 @@
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x239bb50>"
|
||||
"<IPython.core.display.HTML at 0x2154c50>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -289,9 +289,9 @@
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAr0AAAF9CAYAAAAJJNDxAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAr0AAAF9CAYAAAAJJNDxAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0xa136b00>"
|
||||
"<matplotlib.figure.Figure at 0x30ccf90>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -305,7 +305,7 @@
|
||||
"\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",
|
||||
"\"Enough 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 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."
|
||||
]
|
||||
@@ -325,7 +325,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAqwAAAFdCAYAAADG5D8RAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x3e70c18>"
|
||||
"<matplotlib.figure.Figure at 0x2154890>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -352,7 +352,7 @@
|
||||
"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",
|
||||
"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 our algorithm does not work for the identity transformation then the filter will never converge. In other words, if the input is 1 (for a one dimensional system), the output must also be 1. If the output was different, such as 1.1, then when we fed 1.1 into the transform at the next time step, we'd get out yet another number, maybe 1.23. The filter would run away (diverge). \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",
|
||||
@@ -377,7 +377,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAswAAAFyCAYAAADh+YyCAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0xa583a58>"
|
||||
"<matplotlib.figure.Figure at 0x38c0f50>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -417,7 +417,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAqwAAAFdCAYAAADG5D8RAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x3e707f0>"
|
||||
"<matplotlib.figure.Figure at 0x38e1650>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -427,7 +427,9 @@
|
||||
"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."
|
||||
"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.\n",
|
||||
"\n",
|
||||
"There are many published ways for selecting the sigma points. For now I will stick with the original implementation by Julier and Uhlmann. This method defines a constant kappa ($\\kappa$) which controls how spread out the sigma points are. Their equations for the sigma points are"
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -444,60 +446,38 @@
|
||||
"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",
|
||||
"Assume a n-dimensional state variable $\\mathbf{x}$ with mean $\\mu$ and covariance $\\Sigma$. We want to choose $2n+1$ sigma points to approximate the gaussian distribution of $\\mathbf{x}$.\n",
|
||||
"\n",
|
||||
"Our first sigma point is always going to be the mean of our input. We will call this $\\mathcal{X}_0$. So,\n",
|
||||
"\n",
|
||||
"$$ \\mathcal{X}_0 = \\mu$$\n",
|
||||
"\n",
|
||||
"Tne corresponding weight for this sigma point is\n",
|
||||
"$$\n",
|
||||
"W_0 = \\frac{\\kappa}{n+\\kappa}\n",
|
||||
"$$\n",
|
||||
"where $n$ is the dimension of the problem, and $\\kappa$ is a scaling factor that will be discussed in a moment.\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",
|
||||
"\\mathcal{X}_i &= \\mu + (\\sqrt{(n+\\kappa)\\Sigma})_i\\,\\,\\,\\, &\\text{for}\\text{ i=1 .. n} \\\\\n",
|
||||
"\\mathcal{X}_i &= \\mu - (\\sqrt{(n+\\kappa)\\Sigma})_{i-n}\\,\\,\\,\\,\\, &\\text{for}\\text{ i=(n+1) .. 2n} \\\\\n",
|
||||
"\\text{and the corresponding weights are} \\\\\n",
|
||||
"W_i &= \\frac{1}{2(n+\\kappa)}\\,\\,\\,\\text{for i=1,2..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"
|
||||
"$\\kappa$ (kappa) is a scaling factor that controls how far away from the mean we want the points to be. A larger kappa will choose points further away from the mean, and a smaller kappa will choose points nearer the mean. Julier and Uhlmann suggest using $\\kappa + n = 3$ if the distribution is Gaussian, and perhaps choosing a different value if it is not Gaussian. So in one dimension we get something like the following. Here I have plotted two different choices for kappa to show how kappa affects the distribution of the points."
|
||||
]
|
||||
},
|
||||
{
|
||||
"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()"
|
||||
"ukf_internal.show_sigmas_for_2_kappas()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
@@ -505,9 +485,9 @@
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAswAAAFyCAYAAADh+YyCAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAswAAAFyCAYAAADh+YyCAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzs3Xd8FHX+P/DXZpNNg/QNKaSQSovSlS5FA5ESwDsDp4ge\neoKgQDilnIIFASUgp4ft/F35ngqHBQXRI2ooochRhAAmkMamkewmIT27SXZ+f3AEMJO2KbOz+3o+\nHj4ezuzs7Cvm7eSdyXs/q0hLSxNARERERESibKQOQERERERkztgwExERERG1gA0zEREREVEL2DAT\nEREREbWADTMRERERUQvYMBMRERERtcDkhnn//v2Ijo5GdHQ0kpKSmj2utLQUc+bMwcyZMzFjxgzs\n37+/3ecgIiIiIpKKwpR1mA0GA6ZOnYrdu3dDr9dj/vz5SExMFD22vr4edXV1cHR0RGlpKWJiYnD0\n6FHU19e3+RxERERERFIx6Q7z+fPnER4eDg8PD/j6+sLHxwepqamix9ra2sLR0REAUF5eDpVK1e5z\nEBERERFJxdaUJ+l0OqjVauzcuROurq5Qq9UoKipC3759RY+vqqpCXFwcNBoNEhISYGNj0+5zEBER\nERFJwaSG+aa4uDgAQGJiIhQKRbPHOTs7Y+/evcjIyMDTTz+NUaNGQRCEdp2DiIiIiEgKJjXMarUa\nWq22cVur1UKtVrf6vNDQUPj5+SE9PR3e3t5tOsfFixfh4uJiSkwiIiIiojbT6/UICwtrst+khjkq\nKgpXrlxBSUkJ9Ho9CgsLG0cpEhISoFAosGLFCgBAYWEhVCoV3N3dodVqkZWVhYCAADg7Ozd7jtu5\nuLhgyJAhpsS0OHv37sX06dOljkFmhnVBYlgXJIZ1QWJYF7ecOXNGdL9JDbNKpUJ8fDzmzp0LAFiz\nZk3jYzqd7o5jCwoK8OKLLzZur1q1Cu7u7gDQ7DlInEajkToCmSHWBYlhXZAY1gWJYV20zuQZ5piY\nGMTExDTZv3Hjxju2Bw0ahL1797brHERERERE5oKf9CcjkydPljoCmSHWBYlhXZAY1gWJYV20jg2z\njERGRkodgcwQ64LEsC5IDOuCxLAuWtehZeWoeyUnJ2PMmDFSxyAzw7ogMawLEsO66FyVlZUoKyuT\n/bK4ZWVlcHV1lTpGtxAEAa6urujRo0e7nseGmYiIiKidiouLAQB+fn6yb5j9/PykjtBtBEFoXKHN\n09Ozzc/jSIaM8K4AiWFdkBjWBYlhXXSemw2X3Jtla6NQKODp6Qm9Xt+u57FhJiIiIiJqARtmGUlO\nTpY6Apkh1gWJYV2QGNYFkWnYMBMRERERtYBv+pMRzp6RGNYFiWFdkBjWhXkQBAFarRYAoFarOQct\nA7zDTERERNSJjEYjysvLUV9ff8d+QRCwfft2TJw4EePGjcO4ceMwadIkvP322xAEQaK03au+vh6L\nFi1C//79ERwcjJkzZyI1NVXqWK1iwywjnD0jMawLEsO6IDGsi65VWlqKZ555BqNGjcLIkSNxzz33\n4JFHHkF6ejoA4Nlnn8XmzZtx7tw5FBUVoaioCD///DM2bdqEFStWNJ4nLy8Pa9aswVNPPYWEhASU\nl5eblOfKlSuYOnUqdu3a1SlfX2doaGhASEgIfvjhB2RnZ2PKlCl49NFHpY7VKo5kEBEREXVQSUkJ\nYmNjceHChTv2Z2Vl4dKlS1i9ejX27duH2traJs+tqanBV199hQULFuCjjz5CYmIiCgsLGx//17/+\nhaeeegqLFi1qV6bw8HDY2dlhwoQJpn1RXcDe3h5//OMfG7fnzZuHP/3pTygpKYGHh4eEyVrGhllG\nOHtGYlgXJIZ1QWJYF11n1apVTZrlm7Kzs7F69WqUlZU1+/zr169j4cKFyMnJgcFguOOxq1evYsuW\nLfDz88PMmTPbnKm6uhpVVVXw9vZu83M66q233sL27dub7B85ciQ++eSTJvtPnjwJX19fs26WATbM\nRERERB1SW1uLs2fPtnhMW8Yq8vPzmzTLN5WWluKDDz5oV8N84sQJjBw5EpmZmbhw4QLWrl2L8+fP\nIyMjA7t27cKIESPw+eefY9asWfD398fPP/+M9PR0jBgxAlqtFvb29hg4cKDo/ri4OKSnpzee57PP\nPsPs2bOxbNkyLFu2rE35ysvLsXbtWrz22mtt/pqkwhlmGeHsGYlhXZAY1gWJYV10DZ1O1+LdYwBN\n3gAopqampsXHs7OzW32d2x05cgSVlZXQ6/WYMWMGTp48ierqajzxxBNYsmQJ7r//fhQUFGDIkCEo\nKipCWFgYNBoNYmJi8NBDDyEhIQFarVZ0/6/Pc+3aNQwZMqTN2fR6PR599FHMmjULsbGxbX6eVNgw\nExEREXWAi4sLVCpVi8fY2tq2eIydnV2rr9PQ0CA6A92co0ePIiIiAitXrkReXh4cHR2xb98+9O/f\nH66urqitrUVVVRXUajUmTpyIpKQkTJkyBQCQkpICDw8PTJgwocl+d3f3Zs+zdetWBAYGNvnn4Ycf\nvuPrePLJJxEaGorVq1e3+euREhtmGeHsGYlhXZAY1gWJYV10DRcXF4SHh7d4TEREBEaMGNHs40OG\nDGl11tjLywteXl5tylReXg6j0YjFixdj4MCByM7OxmeffYbi4mIMGDAAAHDo0CEMGzYM33//PQRB\nwKFDhzBq1CgAwKeffoolS5Y0Hnf7/qVLl0Kn04meZ8WKFdBoNE3+uX2ljuXLl0OhUGDLli1t+lrM\nARtmIiIiog564YUX4OfnJ/qYm5sbHn/8cezatQtz5sxBQEBA42OBgYF46KGH8OWXX6J///7Nnt/G\nxgbjx4+HUqlsU57z58/jgQceAAAMHToUFy5cQEBAAObMmYP8/HwkJiZCp9NBoVCgrKwMFRUVKC0t\nxZEjR/CPf/wDQ4cOxfTp01FeXi66v7nztCYnJwcff/wxfvzxRwQHBzfegT5x4kSbvi6pKNLS0sx6\npeycnJx2zcRYsuTkZN4doCZYFySGdUFiWBedJz8/v0mD/P333+O1115Deno6qqurYWdnh9DQUCxY\nsABPPfVU43GlpaU4evQoFAoFRo8eDTc3NwBAUVER5syZg4sXL95xXqVSiXHjxmHnzp1tGt0wxb59\n+3Dq1CmsX7++TfvlTuz7BwBnzpy54xeam7hKBhEREVEnmDx5MiZNmoRjx47h0qVL6N27NyZPntyk\nyXV3d8e0adOaPN/b2xvffPMNtm3bhuTkZNTW1sLFxQWxsbF4/PHHYWvbNW1bWloaduzYgT59+qC8\nvBwuLi4AgMuXL4vut0a8w0xERETUTs3doSR5aO8dZs4wExERERG1gA2zjHD9TBLDuiAxrAsSw7og\nMg0bZiIiC1JSUoOkpFxkZbX8AQhERNR2bJhlhO9sJjGsCwKA3NwKDBqUgr599Zgzxx/Llo1DYKAG\n69efkzoamRFeL4hMw1UyiIhkrrCwCvfcU4GamnGN+wQBqKz0wdtva1FYeBrvvjtUwoRERPLGO8wy\nwtkzEsO6oLi4VNTUDBB9TBDU+PLLYFRWGro5FZkjXi+ITMOGmYhI5tLSWl7aymAIwtq1Kd2UhojI\n8nAkQ0Y4e0ZiWBfWR19vxKWiKmhKa1FWW49eMVVQOqXArocBts51sHWug7HOBvVVKtRXqlBXaYcL\nSgX+fa4Qns52GNDLGT497aX+MkgCvF4QmYYNMxGRmTPUG/FLURXOFVTiXEElruiq0cfDAaEeTnBz\ntIVBa4PaMu//Nccq1FfbwcauAXY96mDrbIBtDz1C7snH9dp6pGqr8f6JPNjb2mCQXw/c5dsDd/v2\nhHcPldRfJpHVEARAq1UAANRqAQqFxIGoVWyYZSQ5OZl3B6gJ1oVlqq034mBGKX5IL0GathpB7g4Y\n5NsDcXf3wkAfZzjaKRuPfafgHArSezU5h6HUEQBga5uFdz/uB7XaCQAgCAI012txrqASJzTl+OCn\nfDjZ2WBEgCum9fNEkLtj93yR1O14vegeRiNQWQk4OQG3f5q1IAB//rM99uxRoaDgxlSsn58Rs2YZ\nsGSJno0zgLy8PDz99NM4e/YswsPDsWPHDvTr16/V502fPh2nT59u/PjwadOmYceOHZ2Wiw0zEZEZ\nySvTY98vWiReKUE/b2fMGqjG3b494axSNvucf/0rEOPGXYHBEC7yaBlGjkyHWj2scY9CoUCQuyOC\n3B0xo78agiAgu7QWh7Ou44X96Qhwc8D0/l4YFeQGWxv+BCdqq9JSBf70J0ecPm2LigrAwQHo168B\n69fXICzMiGefdcLnn6tQW3vr/6uiIhukpSmRmWmDbdturJ+el6fAX/7iAJ1OgcjIBjz5pB4uLu3P\nc+XKFTz77LNYsGABHn744c76MrvU8uXLMWDAAHz22Wd4//338fvf/x7Hjh1r9XkKhQJvvPEGHnnk\nkS7JpUhLSxO65MydJCcnB0OGDJE6BhFRl2kwCjiZU469v2hxRVeD6AgPPNjPC77tmDM+fboQv/1t\nBcrKAmA0+gCogb29BhMnavDxx8Naff5NdQ1GHM0uw9e/aFFQbkBMX0/E9PWCp5OdCV8ZkeXKz8+H\nn9+tN9yWlCgQG9sDFy40vRcZHNyA1aur8fzzzigrE19vwc3NiC+/rMBHHzkgMdEWhYU3f0kWEBRk\nxFNP6bFokb7dOWfMmIG//vWv8Pb2bvdzu1t5eTnCwsJw7tw5+Pr6wmAwICwsDN999x369+/f4nNn\nzJiB3/zmN3j00Ufb9Fq//v7ddObMGQQEBDTZzzvMREQSEQQBydll+Oi/eXCxt8X0/l5YPzkEKtv2\nL2A0dGgvZGT0wtmzRTh48Aq8vFR4+OFQqFTt+yFpp7TBfaHuuC/UHVklNdh7SYcnP/sF40Line truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0xa7d3588>"
|
||||
"<matplotlib.figure.Figure at 0x3bd4c90>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -517,31 +497,16 @@
|
||||
"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",
|
||||
"The remainder of the algorithm follows from our equations above:\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",
|
||||
"$$\\begin{aligned}\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",
|
||||
"\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 `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 `chol()`, which does the same thing."
|
||||
"In other words, we geneate sigma points from an existing state variable from its mean and covariance matrix. We pass those sigma points through the nonlinear function that we are trying to filter. Then we use equations (2) and (3) to regenerate an approximation for the mean and covariance of the output."
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -556,7 +521,7 @@
|
||||
"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 `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 `numpy.matrix` instead. Fortunately, numpy provides `numpy.asmatrix()`. It will convert a variable to `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, `numpy.array`'s, or `numpy.matrix`s into my code and it will work.\n",
|
||||
"So let's just implement this algorithm. First, let's write the code to compute the mean and covariance given the sigma points. \n",
|
||||
"\n",
|
||||
"So we will store the sigma points and weights in matrices, like so:\n",
|
||||
"\n",
|
||||
@@ -564,163 +529,235 @@
|
||||
"\\begin{aligned}\n",
|
||||
"weights &= \n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"w_1&w_2& \\dots & w_n\n",
|
||||
"w_1&w_2& \\dots & w_{2n+1}\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",
|
||||
"\\mathcal{X}_{0,0} & \\mathcal{X}_{0,1} & \\mathcal{X}_{0,2} \\\\\n",
|
||||
"\\mathcal{X}_{1,0} & \\mathcal{X}_{1,1} & \\mathcal{X}_{1,2} \\\\\n",
|
||||
"\\vdots & \\vdots & \\vdots \\\\\n",
|
||||
"\\mathcal{X}_{2n+1,0} & \\mathcal{X}_{2n+1,1} & \\mathcal{X}_{2n+1,2}\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."
|
||||
"In other words, each column contains the $2n+1$ 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 through nth row contains the $\\mu+\\sqrt{(n+\\lambda)\\Sigma}$ terms, and the $n+1$ to $2n$ rows contains the $\\mu-\\sqrt{(n+\\lambda)\\Sigma}$ terms."
|
||||
]
|
||||
},
|
||||
{
|
||||
"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 col in range (kmax):\n",
|
||||
" mu += W[0,col] * Xi[:,col]\n",
|
||||
"\n",
|
||||
" for col in range (kmax):\n",
|
||||
" cov += W[0,col]*(Xi[:,col]-xm) * (Xi[:,col]-xm).T\n",
|
||||
"\n",
|
||||
" return (mu, cov)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 7
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Let's review a bit of numpy nomenclature. The odd looking\n",
|
||||
"Computing the weights in numpy is extremely simple. Recall that\n",
|
||||
"\n",
|
||||
" Xi[:,i]\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"W_0 &= \\frac{\\kappa}{n+\\kappa} \\\\\n",
|
||||
"W_i &= \\frac{1}{2(n+\\kappa)}\\,\\,\\,\\text{for i=1,2..2n}\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"These two lines of code implenent these equations with the `np.full()` method, which creates and fills an array with the same value. Then the value for the mean($W_0$) is computed and overwrites the filled in value. We make $W$ a $(2n+1)\\times1$ dimension array simply because linear algebra with numpy proceeds much more smoothly when all arrays are 2 dimensional, so the one dimensional array `[1,2,3]` is better espressed in numpy as `[[1,2,3]]`.\n",
|
||||
"\n",
|
||||
" W = np.full((2*n+1,1), .5 / (n+kappa))\n",
|
||||
" W[0] = kappa / (n+kappa)\n",
|
||||
" \n",
|
||||
"terms are merely saying 'take the *ith* column of Xi'. So This produces a $1{\\times}n$ column vector, like so:\n",
|
||||
"\n",
|
||||
"The equations for the sigma points are:\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",
|
||||
"\\begin{aligned}\n",
|
||||
"\\mathcal{X}_0 &= \\mu \\\\\n",
|
||||
"\\mathcal{X}_i &= \\mu + \\bigg[\\sqrt{(n+\\kappa)\\Sigma} \\bigg]_i\\,\\,\\,\\, &\\text{for}\\text{ i=1 .. n} \\\\\n",
|
||||
"\\mathcal{X}_i &= \\mu - \\bigg[\\sqrt{(n+\\kappa)\\Sigma}\\bigg]_{i-n}\\,\\,\\,\\,\\, &\\text{for}\\text{ i=(n+1) .. 2n}\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"** AUTHOR'S NOTE: break the function up into several cells, explain each cell (convert to matrix, initialize to zero, sum weights, sum variances. Then put into function. easier to explain to student that way**\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.\n",
|
||||
"The Python for this is not much more difficult once we wrap our heads around the $[\\sqrt{(n+\\kappa)\\Sigma}]_i$ term.\n",
|
||||
"\n",
|
||||
"** Author's note: have not introduced kappa yet. Also, do we want this form (the original implementaiton of UKF, followed by the scaled UKF, or just jump to scaled UKF?**"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def sigma_points (mu, cov, 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",
|
||||
"The term $[\\sqrt{(n+\\kappa)\\Sigma}]_i$ has to be a matrix because $\\Sigma$ is a matrix. The subscript $i$ is choosing the column vector of the matrix. 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",
|
||||
" 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",
|
||||
"\\begin{aligned}\n",
|
||||
"\\text{if }\\Sigma = SS \\\\\n",
|
||||
"\\\\\n",
|
||||
"\\text{then }S = \\sqrt{\\Sigma}\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
" mu = asmatrix(mu)\n",
|
||||
" cov = asmatrix(cov)\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 the matrix S, which when multiplied by its transpose, returns $\\Sigma$:\n",
|
||||
"\n",
|
||||
" n = size(mu)\n",
|
||||
"$$\n",
|
||||
"\\Sigma = SS^T \\\\\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
" # initialize to zero\n",
|
||||
" Xi = asmatrix (zeros((n,2*n+1)))\n",
|
||||
" W = asmatrix (zeros(2*n+1))\n",
|
||||
"This latter method is typically chosen in computational linear algebra because this expression is easy to compute using something called the *Cholesky decomposition*. \n",
|
||||
"Numpy provides this with the `numpy.linalg.cholesky()` method. If your language of choice is Fortran, C, C++, or the like standard libraries such as LAPACK also provide this routine. And, of course, matlab provides `chol()`, which does the same thing.\n",
|
||||
"\n",
|
||||
"This method returns a lower triangular matrix, so we will take the transpose of it so that in our for loop we can access it row-wise as `U[i]`, rather than the more cumbersome column-wise notation `U[i,:]`.\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 col in range (n):\n",
|
||||
" Xi[:,col+1] = mu + U[:,col]\n",
|
||||
" Xi = np.zeros((2*n+1, n))\n",
|
||||
" Xi[0] = X\n",
|
||||
" U = linalg.cholesky((n+kappa)*P).T\n",
|
||||
"\n",
|
||||
" for k in range (n):\n",
|
||||
" Xi[:, n+col+1] = mu - U[:,col]\n",
|
||||
" Xi[k+1] = X + U[k]\n",
|
||||
" Xi[n+k+1] = X - U[k]\n",
|
||||
"\n",
|
||||
"The full listing from the `filterpy.kalman` library follows."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def sigma_points (X, P, kappa):\n",
|
||||
" \"\"\" Computes the sigma points and weights for an unscented Kalman filter\n",
|
||||
" given the mean and covariance of the filter.\n",
|
||||
" kappa is an arbitrary constant\n",
|
||||
" constant. Returns tuple of the sigma points and weights.\n",
|
||||
"\n",
|
||||
" Works with both scalar and array 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",
|
||||
" Parameters\n",
|
||||
" ----------\n",
|
||||
" X An array of the means for each dimension in the problem space.\n",
|
||||
" Can be a scalar if 1D.\n",
|
||||
" examples: 1, [1,2], np.array([1,2])\n",
|
||||
"\n",
|
||||
" P : scalar, or\n",
|
||||
"\n",
|
||||
" Returns\n",
|
||||
" -------\n",
|
||||
" sigmas : np.array, of size (n, 2n+1)\n",
|
||||
" Two dimensional array of sigma points. Each column contains all of\n",
|
||||
" the sigmas for one dimension in the problem space.\n",
|
||||
"\n",
|
||||
" Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}\n",
|
||||
"\n",
|
||||
" weights : 1D np.array, of size (2n+1)\n",
|
||||
" \"\"\"\n",
|
||||
"\n",
|
||||
" if np.isscalar(X):\n",
|
||||
" X = np.array([X])\n",
|
||||
"\n",
|
||||
" if np.isscalar(P):\n",
|
||||
" P = np.array([[P]])\n",
|
||||
"\n",
|
||||
" \"\"\" Xi - sigma points\n",
|
||||
" W - weights\n",
|
||||
" \"\"\"\n",
|
||||
"\n",
|
||||
" n = np.size(X) # dimension of problem\n",
|
||||
"\n",
|
||||
" W = np.full((2*n+1,1), .5 / (n+kappa))\n",
|
||||
" Xi = np.zeros((2*n+1, n))\n",
|
||||
"\n",
|
||||
" # handle values for the mean separately as special case\n",
|
||||
" Xi[0] = X\n",
|
||||
" W[0] = kappa / (n+kappa)\n",
|
||||
"\n",
|
||||
" # implements U'*U = (n+kappa)*P. Returns lower triangular matrix.\n",
|
||||
" # Take transpose so we can access with U[i]\n",
|
||||
" U = linalg.cholesky((n+kappa)*P).T\n",
|
||||
"\n",
|
||||
" for k in range (n):\n",
|
||||
" Xi[k+1] = X + U[k]\n",
|
||||
" Xi[n+k+1] = X - U[k]\n",
|
||||
"\n",
|
||||
" return (Xi, W)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 8
|
||||
"prompt_number": 10
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now let's implement the unscented transform. Recall the equations\n",
|
||||
"$$\\begin{aligned}\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",
|
||||
"We implement the sum of the means with\n",
|
||||
"\n",
|
||||
" X = np.sum (Xi*W, axis=0)\n",
|
||||
"\n",
|
||||
"If you are not a heavy user of numpy this may look foreign to you. Numpy is not just a library that make linear algebra possible; under the hood it is written in C to achieve much faster speeds than Python can reach. A typical speedup is 100x. To get that speedup we must avoid using for loops, and instead use numpy's built in functions to perform calculations. So, instead of writing a for loop to compute the sum, we call the built in `numpy.sum()` method which takes an entire array and computes the sum in C. The `axis` parameter tells sum over which axis to sum the array. For example, if \n",
|
||||
"\n",
|
||||
"$$Xi = \\begin{bmatrix}1&2&3\\\\1&2&3\\\\1&2&3\\end{bmatrix}$$\n",
|
||||
"then \n",
|
||||
"\n",
|
||||
" sum(Xi) == 18\n",
|
||||
" sum(Xi,axis=0) == [3,6,9]\n",
|
||||
" sum(Xi,axis=1) == [6,6,6]\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"All that is left is to compute $\\Sigma = \\sum_i w_i{(\\mathcal{X}_i-\\mu)(\\mathcal{X}_i-\\mu)^T}$\n",
|
||||
"\n",
|
||||
" P = np.zeros((n,n))\n",
|
||||
" for k in range (2*n+1):\n",
|
||||
" s = (Xi[k]-X)[np.newaxis] # needs to be 2D to perform transform\n",
|
||||
" P += W[k]*s*s.T\n",
|
||||
"\n",
|
||||
"This introduces another new feature of numpy. The state variable $X$ is one dimensional, as is $Xi[k]$, so the difference $Xi[k]-X$ is also one dimensional. numpy will not compute the transpose of a 1-D array; it considers the transpose of `[1,2,3]` to be `[1,2,3]`. I consider that a deficiency of numpy, but you have to live with it. So we need to make the array two dimensional, with the second dimension of size 1. You do this in numpy by appending `[np.newaxis]` to the array. For example, `np.array([1,2])[np.newaxis]` returns `array([[1, 2]])`.\n",
|
||||
"\n",
|
||||
"The following code is the implementation from the `filterpy.kalman` library. The function includes the ability to sum a noise covariance into the covariance matrix; this feature will be used to implement the full blown unscented Kalman filter."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"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",
|
||||
" kmax,n = Xi.shape\n",
|
||||
"\n",
|
||||
" X = np.sum (Xi*W, axis=0)\n",
|
||||
" P = np.zeros((n,n))\n",
|
||||
" \n",
|
||||
" for k in range (kmax):\n",
|
||||
" s = (Xi[k]-X)[np.newaxis] # needs to be 2D to perform transform\n",
|
||||
" P += W[k]*s*s.T\n",
|
||||
"\n",
|
||||
" if NoiseCov is not None:\n",
|
||||
" P += NoiseCov\n",
|
||||
"\n",
|
||||
" return (X, P)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 11
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Unscented Kalman Filter"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We are now ready to consider implementing a Kalman filter using the approximations for the mean and covariances afforded by the unscented transform. "
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
|
||||
+32
-1
@@ -8,6 +8,7 @@ import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Ellipse,Arrow
|
||||
import stats
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
|
||||
def arrow(x1,y1,x2,y2):
|
||||
@@ -65,7 +66,37 @@ def show_sigma_selections():
|
||||
|
||||
|
||||
|
||||
|
||||
def show_sigmas_for_2_kappas():
|
||||
# generate the Gaussian data
|
||||
|
||||
xs = np.arange(-4, 4, 0.1)
|
||||
mean = 0
|
||||
sigma = 1.5
|
||||
ys = [stats.gaussian(x, mean, sigma*sigma) for x in xs]
|
||||
|
||||
def sigma_points(mean, sigma, kappa):
|
||||
sigma1 = mean + math.sqrt((1+kappa)*sigma)
|
||||
sigma2 = mean - math.sqrt((1+kappa)*sigma)
|
||||
return mean, sigma1, sigma2
|
||||
|
||||
#generate our samples
|
||||
kappa = 2
|
||||
x0,x1,x2 = sigma_points(mean, sigma, kappa)
|
||||
|
||||
samples = [x0,x1,x2]
|
||||
for x in samples:
|
||||
p1 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='k')
|
||||
|
||||
kappa = -.5
|
||||
x0,x1,x2 = sigma_points(mean, sigma, kappa)
|
||||
|
||||
samples = [x0,x1,x2]
|
||||
for x in samples:
|
||||
p2 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='b')
|
||||
|
||||
plt.legend([p1,p2], ['$kappa$=2', '$kappa$=-0.5'])
|
||||
plt.plot(xs, ys)
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in new issue
Block a user