Expanded description of multiple updates.
Put in context of future EKF localization problem where we perform several updates in a row, one for each landmark.
This commit is contained in:
Roger Labbe committed
1 parent
3796453e1c
commit
a30013c00d
1 file changed
+167
-9
+167
-9
@@ -16,7 +16,7 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": 5,
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
},
|
||||
@@ -237,7 +237,10 @@
|
||||
" },\n",
|
||||
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
|
||||
" \"HTML-CSS\": {\n",
|
||||
" scale:85,\n",
|
||||
" scale:100,\n",
|
||||
" availableFonts: [\"Neo-Euler\"],\n",
|
||||
" preferredFont: \"Neo-Euler\",\n",
|
||||
" webFont: \"Neo-Euler\",\n",
|
||||
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
|
||||
" }\n",
|
||||
" });\n",
|
||||
@@ -247,7 +250,7 @@
|
||||
"<IPython.core.display.HTML object>"
|
||||
]
|
||||
},
|
||||
"execution_count": 1,
|
||||
"execution_count": 5,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
@@ -467,7 +470,7 @@
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"Just a reminder: the superscript $^-$ is used to denote that the value is a prediction, not an estimate. But $\\mathbf{x}$ and $\\mathbf{x}^-$ are the same thing, the *state* of our system, just at different times of the algorithm. I am not entirely pleased with this notation for several reasons. First, it clutters the equations up, making them harder to read. More importantly, that aren't always correct. For example, consider a situation wherre we have no measurement for 1 or more time periods. In that case we just compute the predict step several times in a row without an intervening update step. Thus, $\\mathbf{x}$ is never computed, and the predict step is actually $\\mathbf{x}^-_t = \\mathbf{Fx}^-_{t-1} + \\mathbf{Bu}$. Alternatively, if you have several measurements for one time epoch (from different sensors, say) it is sometimes possible to perform the update once for each measurement instead of trying to incorporate all of the measurements at once. In that case the subsequent updates are not performing the computations on the predicted state, but on the partially updated state. The $^-$ notation does not capture any of these. For most of the book I will dispense with this notation unless I want to call out that I am computing a prediciton. \n",
|
||||
"Just a reminder: the superscript $^-$ is used to denote that the value is a prediction, not an estimate. But $\\mathbf{x}$ and $\\mathbf{x}^-$ are the same thing, the *state* of our system, just at different times of the algorithm. I am not entirely pleased with this notation for several reasons. First, it clutters the equations up, making them harder to read. More importantly, that aren't always correct. For example, consider a situation wherre we have no measurement for 1 or more time periods. In that case we just compute the predict step several times in a row without an intervening update step. Thus, $\\mathbf{x}$ is never computed, and the predict step is actually $\\mathbf{x}^-_t = \\mathbf{Fx}^-_{t-1} + \\mathbf{Bu}$. Alternatively, if you have several measurements for one time epoch (from different sensors, say) it is possible to perform the update once for each measurement instead of trying to incorporate all of the measurements at once. Later in the book we will do this with a localization problem where we have bearing and distance measurements for several landmarks. In that case the subsequent updates are not performing the computations on the predicted state, but on the partially updated state. You can think of this as un update being performed with no movement or time having passed since the last update. The $^-$ notation does not capture any of these. For most of the book I will dispense with this notation unless I want to call out that I am computing a prediciton. \n",
|
||||
"\n",
|
||||
"I will start with the update step, as that is what we started with in the one dimensional Kalman filter case. The first equation is\n",
|
||||
"\n",
|
||||
@@ -480,7 +483,7 @@
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"execution_count": 6,
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
},
|
||||
@@ -489,7 +492,7 @@
|
||||
"data": {
|
||||
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAtcAAAEjCAYAAAD5ZS3PAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text/plain": [
|
||||
"<matplotlib.figure.Figure at 0x518ca90>"
|
||||
"<matplotlib.figure.Figure at 0x8b75c18>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
@@ -720,7 +723,162 @@
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"So we have proven that the multivariate equations are equivalent to the univariate equations when we only have one state variable. I'll close this section by recognizing one quibble - I hand waved my assertion that $H=1$ and $F=1$. In general we know this is not true. For example, a digital thermometer may provide measurement in volts, and we need to convert that to temperature, and we use $H$ to do that conversion. I left that issue out of the last chapter to keep the explanation as simple and streamlined as possible. It is very straightforward to add that generalization to the equations of the last chapter, redo the algebra above, and still have the same results. In practice we do not use the equations in the last chapter to perform Kalman filtering due to the material in the next section which demonstrates how much better the Kalman filter performs when we include unobserved variables. So I prefer to leave the equations from the last chapter in their simplest form so that they economically represent our central ideas without any extra complications."
|
||||
"So we have proven that the multivariate equations are equivalent to the univariate equations when we only have one state variable. I'll close this section by recognizing one quibble - I hand waved my assertion that $H=1$ and $F=1$. In general we know this is not true. For example, a digital thermometer may provide measurement in volts, and we need to convert that to temperature, and we use $H$ to do that conversion. I left that issue out to keep the explanation as simple and streamlined as possible. It is very straightforward to add that generalization to the equations above, redo the algebra, and still have the same results."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Compare Performance to Univariate Kalman Filter"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Let's implement a simple 1-D kalman filter using the Kalman filter from this chapter, and compare its output to the kalman filter from the previous chapter by plotting it. We will use a simple model of tracking an object that starts at x=0 and moves by 1 at each step. We will assume the arbitrary value 5 for the measurement noise and .02 for the process noise.\n",
|
||||
"\n",
|
||||
"First, let's implement the filter from the *Univariate Filter* chapter."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from __future__ import division\n",
|
||||
"import numpy as np\n",
|
||||
"from numpy.random import randn\n",
|
||||
"from filterpy.kalman import KalmanFilter\n",
|
||||
"\n",
|
||||
"# 1-D Kalman filter equations\n",
|
||||
"def predict(pos, variance, movement, movement_variance):\n",
|
||||
" return (pos + movement, variance + movement_variance)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def update (mu1, var1, mu2, var2):\n",
|
||||
" mean = (var1*mu2 + var2*mu1) / (var1+var2)\n",
|
||||
" variance = 1 / (1/var1 + 1/var2)\n",
|
||||
" return (mean, variance)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now, let's implement the same thing using the Kalman filter. I will implement it as a function that returns a KalmanFilter object so that you can run the analysis code several times with the KalmanFilter initialized to the same starting conditions each time."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 8,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from filterpy.kalman import KalmanFilter\n",
|
||||
"\n",
|
||||
"def mkf_filter(R, Q):\n",
|
||||
" f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)\n",
|
||||
" f.P = 500.\n",
|
||||
" f.H = np.array([[1.]])\n",
|
||||
" f.F = np.array([[1.]])\n",
|
||||
" f.B = np.array([[1.]])\n",
|
||||
" f.Q = Q\n",
|
||||
" f.R = R\n",
|
||||
" \n",
|
||||
" return f"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Finally, let's compare the two. I will plot the data from the 1-D Kalman filter as a blue line, and the output of the filter from this chapter as red dots. I wrote it as a function so you can easily modify the parameters and regenerate the plots."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 14,
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuYAAAEjCAYAAAB+VMTJAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text/plain": [
|
||||
"<matplotlib.figure.Figure at 0x929e048>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"data": {
|
||||
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAtIAAAEjCAYAAAAfTOaLAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text/plain": [
|
||||
"<matplotlib.figure.Figure at 0x90e7780>"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"\n",
|
||||
"def plot_kf_compare(x0, p0, R, Q, move):\n",
|
||||
" # storage for filter output\n",
|
||||
" x_uni, x_kf = [], []\n",
|
||||
" p1, p2 = [], []\n",
|
||||
"\n",
|
||||
" # initialize the filters\n",
|
||||
" f = mkf_filter(R, Q)\n",
|
||||
" f.x[0] = 0.\n",
|
||||
" f.P[0, 0] = p0\n",
|
||||
" pos = (x0, p0)\n",
|
||||
" for i in range(50):\n",
|
||||
" z = i*move + randn()\n",
|
||||
" pos = update(pos[0], pos[1], z, R)\n",
|
||||
" f.update(z)\n",
|
||||
"\n",
|
||||
" x_uni.append(pos[0])\n",
|
||||
" x_kf.append(f.x[0])\n",
|
||||
"\n",
|
||||
" p1.append(pos[1])\n",
|
||||
" p2.append(f.P[0, 0])\n",
|
||||
"\n",
|
||||
" u = move + randn()\n",
|
||||
" pos = predict(pos[0], pos[1], u, Q)\n",
|
||||
" f.predict(u=u)\n",
|
||||
"\n",
|
||||
" plt.scatter(range(len(x_kf)), x_kf, c='r', s=80, label='Kalman')\n",
|
||||
" plt.title('State')\n",
|
||||
" plt.plot(x_uni, label='Univariate')\n",
|
||||
" plt.xlim([0, 50])\n",
|
||||
" plt.legend()\n",
|
||||
"\n",
|
||||
" plt.figure()\n",
|
||||
" plt.plot(p1)\n",
|
||||
" plt.scatter(range(len(x_kf)), p2, c='r', s=80)\n",
|
||||
" plt.title('Variance')\n",
|
||||
" plt.xlim([0, 50])\n",
|
||||
" plt.show()\n",
|
||||
" \n",
|
||||
"plot_kf_compare(x0=0., p0=500., R=5., Q=.2, move=1.)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"As you can see, both filters produce the same results. Feel free to vary the initial guess, the measurement noise, and the process noise; so long as you make the same changes to both filters the output should be the same. This is a solid demonstration, albeit not a rigorous proof, that both filters in fact implement the same math for the 1-D case. "
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -837,7 +995,7 @@
|
||||
"source": [
|
||||
"### Taylor Series Expansion\n",
|
||||
"\n",
|
||||
"Broadly speaking there are three ways to find $\\Phi$. The technique most often used with Kalman filters is to use a Taylor-series expansion. The Taylor series represents a function as an infinite sum of terms. The terms are linear, even for a nonlinear function, so we can express any arbitrary nonlinear function using linear algebra. The cost of this choice is that unless we use an infinite number of terms (and who has time for that these days) the value we compute will be approximate rather than exact.\n",
|
||||
"Broadly speaking there are three ways to find $\\Phi$ that are used in Kalman filters. The technique most often used with Kalman filters is to use a Taylor-series expansion. The Taylor series represents a function as an infinite sum of terms. The terms are linear, even for a nonlinear function, so we can express any arbitrary nonlinear function using linear algebra. The cost of this choice is that unless we use an infinite number of terms (and who has time for that these days) the value we compute will be approximate rather than exact.\n",
|
||||
"\n",
|
||||
"For the Kalman filter we will be using a form of the series that uses a matrix. But before we do that, let's work through a couple of examples with real functions since real functions are easier to plot and reason about. The Taylor series for either are nearly identical, so this is a good first step.\n",
|
||||
"For a real (or complex) function the Taylor series of a function $f(x)$ evaluated at $a$ is defined as \n",
|
||||
@@ -2510,7 +2668,7 @@
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.4.1"
|
||||
"version": "3.4.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
||||
Reference in new issue
Block a user