created chapter 7
This commit is contained in:
Roger Labbe committed
1 parent
96563286f0
commit
7a56f2263d
2 files changed
+474
-472
No files matched your search
@@ -0,0 +1,474 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:b48c3519700b0fb4870898b3dc76b4f979d302dcdde562eb556ccc8e77d56013"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
"worksheets": [
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 1,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Kalman Filter Math"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"#format the book\n",
|
||||
"%matplotlib inline\n",
|
||||
"from __future__ import division, print_function\n",
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"import sys\n",
|
||||
"sys.path.insert(0,'../') # allow us to format the book\n",
|
||||
"import book_format\n",
|
||||
"book_format.load_style()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"html": [
|
||||
"<style>\n",
|
||||
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
|
||||
"\n",
|
||||
" div.cell{\n",
|
||||
" width: 850px;\n",
|
||||
" margin-left: 0% !important;\n",
|
||||
" margin-right: auto;\n",
|
||||
" }\n",
|
||||
" div.text_cell code {\n",
|
||||
" background: transparent;\n",
|
||||
" color: #000000;\n",
|
||||
" font-weight: 600;\n",
|
||||
" font-size: 11pt;\n",
|
||||
" font-style: bold;\n",
|
||||
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
|
||||
" }\n",
|
||||
" h1 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
"\t}\n",
|
||||
"\t\n",
|
||||
" div.input_area {\n",
|
||||
" background: #F6F6F9;\n",
|
||||
" border: 1px solid #586e75;\n",
|
||||
" }\n",
|
||||
"\n",
|
||||
" .text_cell_render h1 {\n",
|
||||
" font-weight: 200;\n",
|
||||
" font-size: 30pt;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#c76c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 1em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: wrap;\n",
|
||||
" } \n",
|
||||
" h2 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h2 {\n",
|
||||
" font-weight: 200;\n",
|
||||
" font-size: 20pt;\n",
|
||||
" font-style: italic;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#c76c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 1.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" } \n",
|
||||
" h3 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h3 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-size: 18pt;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#d77c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 2em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" h4 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h4 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-size: 16pt;\n",
|
||||
" color:#d77c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 0.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" h5 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h5 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-style: normal;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" font-size: 16pt;\n",
|
||||
" margin-bottom: 0em;\n",
|
||||
" margin-top: 1.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" div.text_cell_render{\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" line-height: 135%;\n",
|
||||
" font-size: 110%;\n",
|
||||
" width:750px;\n",
|
||||
" margin-left:auto;\n",
|
||||
" margin-right:auto;\n",
|
||||
" text-align:justify;\n",
|
||||
" text-justify:inter-word;\n",
|
||||
" }\n",
|
||||
" div.output_subarea.output_text.output_pyout {\n",
|
||||
" overflow-x: auto;\n",
|
||||
" overflow-y: scroll;\n",
|
||||
" max-height: 300px;\n",
|
||||
" }\n",
|
||||
" div.output_subarea.output_stream.output_stdout.output_text {\n",
|
||||
" overflow-x: auto;\n",
|
||||
" overflow-y: scroll;\n",
|
||||
" max-height: 300px;\n",
|
||||
" }\n",
|
||||
" code{\n",
|
||||
" font-size: 70%;\n",
|
||||
" }\n",
|
||||
" .rendered_html code{\n",
|
||||
" background-color: transparent;\n",
|
||||
" }\n",
|
||||
" ul{\n",
|
||||
" margin: 2em;\n",
|
||||
" }\n",
|
||||
" ul li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.5em; \n",
|
||||
" }\n",
|
||||
" ul li li{\n",
|
||||
" padding-left: 0.2em; \n",
|
||||
" margin-bottom: 0.2em; \n",
|
||||
" margin-top: 0.2em; \n",
|
||||
" }\n",
|
||||
" ol{\n",
|
||||
" margin: 2em;\n",
|
||||
" }\n",
|
||||
" ol li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.5em; \n",
|
||||
" }\n",
|
||||
" ul li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.2em; \n",
|
||||
" }\n",
|
||||
" a:link{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" a:visited{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" }\n",
|
||||
" a:hover{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" }\n",
|
||||
" a:focus{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" a:active{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" .rendered_html :link {\n",
|
||||
" text-decoration: underline; \n",
|
||||
" }\n",
|
||||
" .rendered_html :hover {\n",
|
||||
" text-decoration: none; \n",
|
||||
" }\n",
|
||||
" .rendered_html :visited {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .rendered_html :focus {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .rendered_html :active {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .warning{\n",
|
||||
" color: rgb( 240, 20, 20 )\n",
|
||||
" } \n",
|
||||
" hr {\n",
|
||||
" color: #f3f3f3;\n",
|
||||
" background-color: #f3f3f3;\n",
|
||||
" height: 1px;\n",
|
||||
" }\n",
|
||||
" blockquote{\n",
|
||||
" display:block;\n",
|
||||
" background: #fcfcfc;\n",
|
||||
" border-left: 5px solid #c76c0c;\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" width:680px;\n",
|
||||
" padding: 10px 10px 10px 10px;\n",
|
||||
" text-align:justify;\n",
|
||||
" text-justify:inter-word;\n",
|
||||
" }\n",
|
||||
" blockquote p {\n",
|
||||
" margin-bottom: 0;\n",
|
||||
" line-height: 125%;\n",
|
||||
" font-size: 100%;\n",
|
||||
" }\n",
|
||||
"</style>\n",
|
||||
"<script>\n",
|
||||
" MathJax.Hub.Config({\n",
|
||||
" TeX: {\n",
|
||||
" extensions: [\"AMSmath.js\"]\n",
|
||||
" },\n",
|
||||
" tex2jax: {\n",
|
||||
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
|
||||
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
|
||||
" },\n",
|
||||
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
|
||||
" \"HTML-CSS\": {\n",
|
||||
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
|
||||
" }\n",
|
||||
" });\n",
|
||||
"</script>\n"
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f794933e9e8>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 1
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"If you've gotten this far I hope that you are thinking that the Kalman filter's fearsome reputation is somewhat undeserved. Sure, I hand waved some equations away, but I hope implementation has been fairly straightforward for you. The underlying concept is quite straightforward - take two measurements, or a measurement and a prediction, and choose the output to be somewhere between the two. If you believe the measurement more your guess will be closer to the measurement, and if you believe the prediction is more accurate your guess will lie closer it it. That's not rocket science (little joke - it is exactly this math that got Apollo to the moon and back!). \n",
|
||||
"\n",
|
||||
"Well, to be honest I have been choosing my problems carefully. For any arbitrary problem finding some of the matrices that we need to feed into the Kalman filter equations can be quite difficult. I haven't been *too tricky*, though. Equations like Newton's equations of motion can be trivially computed for Kalman filter applications, and they make up the bulk of the kind of problems that we want to solve. If you are a hobbyist, you can safely pass by this chapter for now, and perhaps forever. Some of the later chapters will assume the material in this chapter, but much of the work will still be accessible to you. \n",
|
||||
"\n",
|
||||
"But, I urge everyone to at least read the first section, and to skim the rest. It is not much harder than what you have done - the difficulty comes in finding closed form expressions for specific problems, not understanding the math in this chapter. \n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Modeling a Linear System that Has Noise"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We need to start by understanding the underlying equations and assumptions that the Kalman filter uses. We are trying to model real world phenomena, so what do we have to consider?\n",
|
||||
"\n",
|
||||
"First, each physical system has a process. For example, a car traveling at a certain velocity goes so far in a fixed amount of time, and it's velocity varies as a function of it's acceleration. We describe that behavior with the well known Newtonian equations we learned in high school.\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"v&=at\\\\\n",
|
||||
"d &= \\frac{1}{2}at_2 + v_0t + d_0\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"But, of course we know this is not all that is happening. First, we do not have perfect measures of things like the velocity and acceleration - there is always noise in the measurements, and we have to model that. Second, no car travels on a perfect road. There are bumps that cause the car to slow down, there is wind drag, there are hills that raise and lower the speed. If we do not have explicit knowledge of these factors we lump them all together under the term \"process noise\".\n",
|
||||
"\n",
|
||||
"Trying to model off of those factors explicitly and exactly is impossible for anything but the most trivial problem. I could try to modify Newton's equations for things like bumps in the road, the behavior of the car's suspension system, heck, the effects of hitting bugs with the windshield, but the job would never be done - there would always be more effects to add. What is worse, each of those models would in themselves be a simplification - do I assume the wind is constant, that the drag of the car is the same for all angles of the wind, that the suspension act as perfect springs, and so on?\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"So control theory makes a mathematically correct simplification. We acknowledge that there are many factors that influence the system that we either do not know or that we don't want to have to model. At any time $t$ we say that the actual value (say, the position of our car) is the predicted value plus some unknown process noise:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"x(t) = x_{pred}(t) + noise(t)\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"This is not meant to imply that $noise(t)$ is a function that we can derive analytically or that it is well behaved. If there is a bump in the road at $t=10$ then the noise factor will just incorporate that effect. Again, this is not implying that we model, compute, or even know the value of *noise(t)*, it is merely a statement of fact - we can *always* describe the actual value as the predicted value plus some other value. \n",
|
||||
"\n",
|
||||
"Let's express this in linear algebra. Using the same notation from previous chapters, we can say that our model of the system (without noise) is:\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx$$\n",
|
||||
"\n",
|
||||
"That is, we have a set of linear equations that describe our system. For our car, \n",
|
||||
"$F$ will be the coefficients for Newton's equations of motion. \n",
|
||||
"\n",
|
||||
"Now we need to model the noise. We will just call that *w*, and add it to the equation.\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx + w$$\n",
|
||||
"\n",
|
||||
"Finally, we need to consider inputs into the system. We are dealing with linear problems here, so we will assume that there is some input $u$ into the system, and that we have some linear model that defines how that input changes the system. For example, if you press down on the accelerator in your car the car will accelerate. We will need a matrix $G$ to convert $u$ into the effect on the system. We just add that into our equation:\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx + Gu + w$$\n",
|
||||
"\n",
|
||||
"And that's it. That is the equation that Kalman set out to solve, and he found a way to compute an optimal solution if we assume certain properties of $w$.\n",
|
||||
"\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Walking Through the Kalman Filter Equations"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"I promised that you would not have to understand how to derive Kalman filter equations, and that is true. However, I do think it is worth walking through the equations one by one and becoming familiar with the variables. If this is your first time through the material feel free to skip ahead to the next section. However, you will eventually want to work through this material, so why not now? You will need to have passing familiarity with these equations to read material written about the Kalman filter, as they all presuppose that you are familiar with the equations. I will reiterate them here for easy reference.\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\text{Predict Step}\\\\\n",
|
||||
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{G u}\\;\\;\\;\\;&(1) \\\\\n",
|
||||
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;\\;&(2) \\\\\n",
|
||||
"\\\\\n",
|
||||
"\\text{Update Step}\\\\\n",
|
||||
"\\mathbf{\\gamma} &= \\mathbf{z} - \\mathbf{H x}\\;\\;\\;\\;&(3) \\\\\n",
|
||||
"\\mathbf{K}&= \\mathbf{PH}^T (\\mathbf{HPH}^T + \\mathbf{R})^{-1}\\;\\;\\;\\;&(4) \\\\\n",
|
||||
"\\mathbf{x}&=\\mathbf{x}' +\\mathbf{K\\gamma}\\;\\;\\;\\;&(5) \\\\\n",
|
||||
"\\mathbf{P}&= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;\\;&(6)\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"I will start with the measurement step, as that is what we started with in the one dimensional Kalman filter case. Our first equation is\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf{\\gamma} = \\mathbf{z} - \\mathbf{H x}\\tag{3}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"On the right we have $\\mathbf{Hx}$. That should be recognizable as the measurement function. Multiplying $\\mathbf{H}$ with $\\mathbf{x}$ puts $\\mathbf{x}$ into *measurement space*; in other words, the same basis and units as the sensor's measurements. The variable $\\mathbf{z}$ is just the measurement; it is typical, but not universal to use $\\mathbf{z}$ to denote measurements in the literature ($\\mathbf{y}$ is also sometimes used). Do you remember this chart?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"sys.path.insert(0,'../Chapter06_Multivariate_Kalman_Filter')\n",
|
||||
"import mkf_internal\n",
|
||||
"mkf_internal.show_residual_chart()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAsQAAAGDCAYAAAAs4AbKAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f79302f9780>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 3
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The blue prediction line is the output of $\\mathbf{Hx}$, and the dot labeled \"measurement\" is $\\mathbf{z}$. Therefore, $\\gamma = \\mathbf{z} - \\mathbf{Hx}$ is how we compute the residual, drawn in red. So $\\gamma$ is the residual.\n",
|
||||
"\n",
|
||||
"The next line is the formidable:\n",
|
||||
"\n",
|
||||
"$$\\mathbf{K}= \\mathbf{PH}^T (\\mathbf{HPH}^T + \\mathbf{R})^{-1}\\tag{4}$$\n",
|
||||
"\n",
|
||||
"Unfortunately it is a fair amount of linear algebra to derive this. The derivation can be quite elegant, and I urge you to look it up if you have the mathematical education to follow it. But $\\mathbf{K}$ is just the *Kalman gain* - the ratio of how much measurement vs prediction we should use to create the new estimate. $\\mathbf{R}$ is the *measurement noise*, and $\\mathbf{P}$ is our *uncertainty covariance matrix*.\n",
|
||||
"\n",
|
||||
"So let's work through this expression by expression. Start with $\\mathbf{HPH}^T$. The linear equation $\\mathbf{ABA}^T$ can be thought of as changing the basis of $\\mathbf{B}$ to $\\mathbf{A}$. So $\\mathbf{HPH}^T$ is taking the covariance $\\mathbf{P}$ and putting it in measurement ($\\mathbf{H}$) space. Then, once in measurement space, we can add the measurement noise $\\mathbf{R}$ to it. Hence, the expression for the uncertainty in the measurement is:\n",
|
||||
"\n",
|
||||
"$$(\\mathbf{HPH}^T + \\mathbf{R})^{-1}$$\n",
|
||||
"\n",
|
||||
"Taking the inverse is linear algebra's way of doing $\\frac{1}{x}$. So if you accept my admittedly hand wavey explanation it can be seen to be computing:\n",
|
||||
"\n",
|
||||
"$$ \n",
|
||||
"gain_{measurement\\,space} = \\frac{uncertainty_{prediction}}{uncertainty_{measurement}}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"In other words, the *Kalman gain* equation is doing nothing more than computing a ratio based on how much we trust the prediction vs the measurement. If we are confident in our measurements and unconfident in our predictions $\\mathbf{K}$ will favor the measurement, and vice versa. The equation is complicated because we are doing this in multiple dimensions via matrices, but the concept is simple - scale by a ratio.\n",
|
||||
"\n",
|
||||
"Without going into the derivation of $\\mathbf{K}$, I'll say that this equation is the result of finding a value of $\\mathbf{K}$ that optimizes the *mean-square estimation error*. It does this by finding the minimal values for $\\mathbf{P}$ along it's diagonal. Recall that the diagonal of $P$ is just the variance for each state variable. So, this equation for $\\mathbf{K}$ ensures that the Kalman filter output is optimal. To put this in concrete terms, for our dog tracking problem this means that the estimates for both position and velocity will be optimal - a value of $\\mathbf{K}$ that made the position extremely accurate but the velocity very inaccurate would be rejected in favor of a $\\mathbf{K}$ that made both position and velocity just somewhat accurate."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Our next line is:\n",
|
||||
" $$\\mathbf{x}=\\mathbf{x}' +\\mathbf{K\\gamma}\\tag{5}$$\n",
|
||||
"\n",
|
||||
"This just multiplies the residual by the Kalman gain, and adds it to the state variable. In other words, this is the computation of our new estimate.\n",
|
||||
"\n",
|
||||
"Finally, we have:\n",
|
||||
"\n",
|
||||
"$$\\mathbf{P}=(\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\tag{6}$$\n",
|
||||
"\n",
|
||||
"$I$ is the identity matrix, and is the way we represent $1$ in multiple dimensions. $H$ is our measurement function, and is a constant. So, simplified, this is simply $P = (1-cK)P$. $K$ is our ratio of how much prediction vs measurement we use. So, if $K$ is large then $(1-cK)$ is small, and P will be made smaller than it was. If $K$ is small, then $(1-cK)$ is large, and P will be made larger than it was. So we adjust the size of our uncertainty by some factor of the *Kalman gain*. I would like to draw your attention back to the g-h filter, which included this Python code:\n",
|
||||
"\n",
|
||||
" # update filter \n",
|
||||
" w = w * (1-scale_factor) + z * scale_factor\n",
|
||||
"\n",
|
||||
"This multidimensional Kalman filter equation is partially implementing this calculation for the variance instead of the state variable."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now we have the measurement steps. The first equation is\n",
|
||||
"\n",
|
||||
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Gu}\\tag{1}$$\n",
|
||||
"\n",
|
||||
"This is just our state transition equation which we have already discussed. $\\mathbf{Fx}$ multiplies $\\mathbf{x}$ with the state transition matrix to compute the next state. $G$ and $u$ add in the contribution of the control input $\\mathbf{u}$, if any.\n",
|
||||
"\n",
|
||||
"The final equation is:\n",
|
||||
"$$\\mathbf{P} = \\mathbf{FPF}^T + \\mathbf{Q}\\tag{2}$$\n",
|
||||
"\n",
|
||||
"$\\mathbf{FPF}^T$ is the way we put $\\mathbf{P}$ into the process space using linear algebra so that we can add in the process noise $\\mathbf{Q}$ to it."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Design of the Process Noise Matrix"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,472 +0,0 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:8de0a063add9b29133c1733f57bd0169991ed4e5c592b8968aa06a208803b3c2"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
"worksheets": [
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 1,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Kalman Filter Math"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"#format the book\n",
|
||||
"%matplotlib inline\n",
|
||||
"from __future__ import division, print_function\n",
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"import book_format\n",
|
||||
"book_format.load_style()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"html": [
|
||||
"<style>\n",
|
||||
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
|
||||
"\n",
|
||||
" div.cell{\n",
|
||||
" width: 850px;\n",
|
||||
" margin-left: 0% !important;\n",
|
||||
" margin-right: auto;\n",
|
||||
" }\n",
|
||||
" div.text_cell code {\n",
|
||||
" background: transparent;\n",
|
||||
" color: #000000;\n",
|
||||
" font-weight: 600;\n",
|
||||
" font-size: 11pt;\n",
|
||||
" font-style: bold;\n",
|
||||
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
|
||||
" }\n",
|
||||
" h1 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
"\t}\n",
|
||||
"\t\n",
|
||||
" div.input_area {\n",
|
||||
" background: #F6F6F9;\n",
|
||||
" border: 1px solid #586e75;\n",
|
||||
" }\n",
|
||||
"\n",
|
||||
" .text_cell_render h1 {\n",
|
||||
" font-weight: 200;\n",
|
||||
" font-size: 30pt;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#c76c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 1em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: wrap;\n",
|
||||
" } \n",
|
||||
" h2 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h2 {\n",
|
||||
" font-weight: 200;\n",
|
||||
" font-size: 20pt;\n",
|
||||
" font-style: italic;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#c76c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 1.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" } \n",
|
||||
" h3 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h3 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-size: 18pt;\n",
|
||||
" line-height: 100%;\n",
|
||||
" color:#d77c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 2em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" h4 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h4 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-size: 16pt;\n",
|
||||
" color:#d77c0c;\n",
|
||||
" margin-bottom: 0.5em;\n",
|
||||
" margin-top: 0.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" h5 {\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" }\n",
|
||||
" .text_cell_render h5 {\n",
|
||||
" font-weight: 300;\n",
|
||||
" font-style: normal;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" font-size: 16pt;\n",
|
||||
" margin-bottom: 0em;\n",
|
||||
" margin-top: 1.5em;\n",
|
||||
" display: block;\n",
|
||||
" white-space: nowrap;\n",
|
||||
" }\n",
|
||||
" div.text_cell_render{\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" line-height: 135%;\n",
|
||||
" font-size: 110%;\n",
|
||||
" width:750px;\n",
|
||||
" margin-left:auto;\n",
|
||||
" margin-right:auto;\n",
|
||||
" text-align:justify;\n",
|
||||
" text-justify:inter-word;\n",
|
||||
" }\n",
|
||||
" div.output_subarea.output_text.output_pyout {\n",
|
||||
" overflow-x: auto;\n",
|
||||
" overflow-y: scroll;\n",
|
||||
" max-height: 300px;\n",
|
||||
" }\n",
|
||||
" div.output_subarea.output_stream.output_stdout.output_text {\n",
|
||||
" overflow-x: auto;\n",
|
||||
" overflow-y: scroll;\n",
|
||||
" max-height: 300px;\n",
|
||||
" }\n",
|
||||
" code{\n",
|
||||
" font-size: 70%;\n",
|
||||
" }\n",
|
||||
" .rendered_html code{\n",
|
||||
" background-color: transparent;\n",
|
||||
" }\n",
|
||||
" ul{\n",
|
||||
" margin: 2em;\n",
|
||||
" }\n",
|
||||
" ul li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.5em; \n",
|
||||
" }\n",
|
||||
" ul li li{\n",
|
||||
" padding-left: 0.2em; \n",
|
||||
" margin-bottom: 0.2em; \n",
|
||||
" margin-top: 0.2em; \n",
|
||||
" }\n",
|
||||
" ol{\n",
|
||||
" margin: 2em;\n",
|
||||
" }\n",
|
||||
" ol li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.5em; \n",
|
||||
" }\n",
|
||||
" ul li{\n",
|
||||
" padding-left: 0.5em; \n",
|
||||
" margin-bottom: 0.5em; \n",
|
||||
" margin-top: 0.2em; \n",
|
||||
" }\n",
|
||||
" a:link{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" a:visited{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" }\n",
|
||||
" a:hover{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color: #1d3b84;\n",
|
||||
" }\n",
|
||||
" a:focus{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" a:active{\n",
|
||||
" font-weight: bold;\n",
|
||||
" color:#447adb;\n",
|
||||
" }\n",
|
||||
" .rendered_html :link {\n",
|
||||
" text-decoration: underline; \n",
|
||||
" }\n",
|
||||
" .rendered_html :hover {\n",
|
||||
" text-decoration: none; \n",
|
||||
" }\n",
|
||||
" .rendered_html :visited {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .rendered_html :focus {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .rendered_html :active {\n",
|
||||
" text-decoration: none;\n",
|
||||
" }\n",
|
||||
" .warning{\n",
|
||||
" color: rgb( 240, 20, 20 )\n",
|
||||
" } \n",
|
||||
" hr {\n",
|
||||
" color: #f3f3f3;\n",
|
||||
" background-color: #f3f3f3;\n",
|
||||
" height: 1px;\n",
|
||||
" }\n",
|
||||
" blockquote{\n",
|
||||
" display:block;\n",
|
||||
" background: #fcfcfc;\n",
|
||||
" border-left: 5px solid #c76c0c;\n",
|
||||
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
|
||||
" width:680px;\n",
|
||||
" padding: 10px 10px 10px 10px;\n",
|
||||
" text-align:justify;\n",
|
||||
" text-justify:inter-word;\n",
|
||||
" }\n",
|
||||
" blockquote p {\n",
|
||||
" margin-bottom: 0;\n",
|
||||
" line-height: 125%;\n",
|
||||
" font-size: 100%;\n",
|
||||
" }\n",
|
||||
"</style>\n",
|
||||
"<script>\n",
|
||||
" MathJax.Hub.Config({\n",
|
||||
" TeX: {\n",
|
||||
" extensions: [\"AMSmath.js\"]\n",
|
||||
" },\n",
|
||||
" tex2jax: {\n",
|
||||
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
|
||||
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
|
||||
" },\n",
|
||||
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
|
||||
" \"HTML-CSS\": {\n",
|
||||
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
|
||||
" }\n",
|
||||
" });\n",
|
||||
"</script>\n"
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f677177b748>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 1
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"If you've gotten this far I hope that you are thinking that the Kalman filter's fearsome reputation is somewhat undeserved. Sure, I hand waved some equations away, but I hope implementation has been fairly straightforward for you. The underlying concept is quite straightforward - take two measurements, or a measurement and a prediction, and choose the output to be somewhere between the two. If you believe the measurement more your guess will be closer to the measurement, and if you believe the prediction is more accurate your guess will lie closer it it. That's not rocket science (little joke - it is exactly this math that got Apollo to the moon and back!). \n",
|
||||
"\n",
|
||||
"Well, to be honest I have been choosing my problems carefully. For any arbitrary problem finding some of the matrices that we need to feed into the Kalman filter equations can be quite difficult. I haven't been *too tricky*, though. Equations like Newton's equations of motion can be trivially computed for Kalman filter applications, and they make up the bulk of the kind of problems that we want to solve. If you are a hobbyist, you can safely pass by this chapter for now, and perhaps forever. Some of the later chapters will assume the material in this chapter, but much of the work will still be accessible to you. \n",
|
||||
"\n",
|
||||
"But, I urge everyone to at least read the first section, and to skim the rest. It is not much harder than what you have done - the difficulty comes in finding closed form expressions for specific problems, not understanding the math in this chapter. \n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Modeling a Linear System that Has Noise"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We need to start by understanding the underlying equations and assumptions that the Kalman filter uses. We are trying to model real world phenomena, so what do we have to consider?\n",
|
||||
"\n",
|
||||
"First, each physical system has a process. For example, a car traveling at a certain velocity goes so far in a fixed amount of time, and it's velocity varies as a function of it's acceleration. We describe that behavior with the well known Newtonian equations we learned in high school.\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"v&=at\\\\\n",
|
||||
"d &= \\frac{1}{2}at_2 + v_0t + d_0\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"But, of course we know this is not all that is happening. First, we do not have perfect measures of things like the velocity and acceleration - there is always noise in the measurements, and we have to model that. Second, no car travels on a perfect road. There are bumps that cause the car to slow down, there is wind drag, there are hills that raise and lower the speed. If we do not have explicit knowledge of these factors we lump them all together under the term \"process noise\".\n",
|
||||
"\n",
|
||||
"Trying to model off of those factors explicitly and exactly is impossible for anything but the most trivial problem. I could try to modify Newton's equations for things like bumps in the road, the behavior of the car's suspension system, heck, the effects of hitting bugs with the windshield, but the job would never be done - there would always be more effects to add. What is worse, each of those models would in themselves be a simplification - do I assume the wind is constant, that the drag of the car is the same for all angles of the wind, that the suspension act as perfect springs, and so on?\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"So control theory makes a mathematically correct simplification. We acknowledge that there are many factors that influence the system that we either do not know or that we don't want to have to model. At any time $t$ we say that the actual value (say, the position of our car) is the predicted value plus some unknown process noise:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"x(t) = x_{pred}(t) + noise(t)\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"This is not meant to imply that $noise(t)$ is a function that we can derive analytically or that it is well behaved. If there is a bump in the road at $t=10$ then the noise factor will just incorporate that effect. Again, this is not implying that we model, compute, or even know the value of *noise(t)*, it is merely a statement of fact - we can *always* describe the actual value as the predicted value plus some other value. \n",
|
||||
"\n",
|
||||
"Let's express this in linear algebra. Using the same notation from previous chapters, we can say that our model of the system (without noise) is:\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx$$\n",
|
||||
"\n",
|
||||
"That is, we have a set of linear equations that describe our system. For our car, \n",
|
||||
"$F$ will be the coefficients for Newton's equations of motion. \n",
|
||||
"\n",
|
||||
"Now we need to model the noise. We will just call that *w*, and add it to the equation.\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx + w$$\n",
|
||||
"\n",
|
||||
"Finally, we need to consider inputs into the system. We are dealing with linear problems here, so we will assume that there is some input $u$ into the system, and that we have some linear model that defines how that input changes the system. For example, if you press down on the accelerator in your car the car will accelerate. We will need a matrix $G$ to convert $u$ into the effect on the system. We just add that into our equation:\n",
|
||||
"\n",
|
||||
"$$ f(x) = Fx + Gu + w$$\n",
|
||||
"\n",
|
||||
"And that's it. That is the equation that Kalman set out to solve, and he found a way to compute an optimal solution if we assume certain properties of $w$.\n",
|
||||
"\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Walking Through the Kalman Filter Equations"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"I promised that you would not have to understand how to derive Kalman filter equations, and that is true. However, I do think it is worth walking through the equations one by one and becoming familiar with the variables. If this is your first time through the material feel free to skip ahead to the next section. However, you will eventually want to work through this material, so why not now? You will need to have passing familiarity with these equations to read material written about the Kalman filter, as they all presuppose that you are familiar with the equations. I will reiterate them here for easy reference.\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\text{Predict Step}\\\\\n",
|
||||
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{G u}\\;\\;\\;\\;&(1) \\\\\n",
|
||||
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;\\;&(2) \\\\\n",
|
||||
"\\\\\n",
|
||||
"\\text{Update Step}\\\\\n",
|
||||
"\\mathbf{\\gamma} &= \\mathbf{z} - \\mathbf{H x}\\;\\;\\;\\;&(3) \\\\\n",
|
||||
"\\mathbf{K}&= \\mathbf{PH}^T (\\mathbf{HPH}^T + \\mathbf{R})^{-1}\\;\\;\\;\\;&(4) \\\\\n",
|
||||
"\\mathbf{x}&=\\mathbf{x}' +\\mathbf{K\\gamma}\\;\\;\\;\\;&(5) \\\\\n",
|
||||
"\\mathbf{P}&= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;\\;&(6)\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"I will start with the measurement step, as that is what we started with in the one dimensional Kalman filter case. Our first equation is\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf{\\gamma} = \\mathbf{z} - \\mathbf{H x}\\tag{3}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"On the right we have $\\mathbf{Hx}$. That should be recognizable as the measurement function. Multiplying $\\mathbf{H}$ with $\\mathbf{x}$ puts $\\mathbf{x}$ into *measurement space*; in other words, the same basis and units as the sensor's measurements. The variable $\\mathbf{z}$ is just the measurement; it is typical, but not universal to use $\\mathbf{z}$ to denote measurements in the literature ($\\mathbf{y}$ is also sometimes used). Do you remember this chart?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import mkf_internal\n",
|
||||
"mkf_internal.show_residual_chart()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAsQAAAGDCAYAAAAs4AbKAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f677177b4e0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 2
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The blue prediction line is the output of $\\mathbf{Hx}$, and the dot labeled \"measurement\" is $\\mathbf{z}$. Therefore, $\\gamma = \\mathbf{z} - \\mathbf{Hx}$ is how we compute the residual, drawn in red. So $\\gamma$ is the residual.\n",
|
||||
"\n",
|
||||
"The next line is the formidable:\n",
|
||||
"\n",
|
||||
"$$\\mathbf{K}= \\mathbf{PH}^T (\\mathbf{HPH}^T + \\mathbf{R})^{-1}\\tag{4}$$\n",
|
||||
"\n",
|
||||
"Unfortunately it is a fair amount of linear algebra to derive this. The derivation can be quite elegant, and I urge you to look it up if you have the mathematical education to follow it. But $\\mathbf{K}$ is just the *Kalman gain* - the ratio of how much measurement vs prediction we should use to create the new estimate. $\\mathbf{R}$ is the *measurement noise*, and $\\mathbf{P}$ is our *uncertainty covariance matrix*.\n",
|
||||
"\n",
|
||||
"So let's work through this expression by expression. Start with $\\mathbf{HPH}^T$. The linear equation $\\mathbf{ABA}^T$ can be thought of as changing the basis of $\\mathbf{B}$ to $\\mathbf{A}$. So $\\mathbf{HPH}^T$ is taking the covariance $\\mathbf{P}$ and putting it in measurement ($\\mathbf{H}$) space. Then, once in measurement space, we can add the measurement noise $\\mathbf{R}$ to it. Hence, the expression for the uncertainty in the measurement is:\n",
|
||||
"\n",
|
||||
"$$(\\mathbf{HPH}^T + \\mathbf{R})^{-1}$$\n",
|
||||
"\n",
|
||||
"Taking the inverse is linear algebra's way of doing $\\frac{1}{x}$. So if you accept my admittedly hand wavey explanation it can be seen to be computing:\n",
|
||||
"\n",
|
||||
"$$ \n",
|
||||
"gain_{measurement\\,space} = \\frac{uncertainty_{prediction}}{uncertainty_{measurement}}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"In other words, the *Kalman gain* equation is doing nothing more than computing a ratio based on how much we trust the prediction vs the measurement. If we are confident in our measurements and unconfident in our predictions $\\mathbf{K}$ will favor the measurement, and vice versa. The equation is complicated because we are doing this in multiple dimensions via matrices, but the concept is simple - scale by a ratio.\n",
|
||||
"\n",
|
||||
"Without going into the derivation of $\\mathbf{K}$, I'll say that this equation is the result of finding a value of $\\mathbf{K}$ that optimizes the *mean-square estimation error*. It does this by finding the minimal values for $\\mathbf{P}$ along it's diagonal. Recall that the diagonal of $P$ is just the variance for each state variable. So, this equation for $\\mathbf{K}$ ensures that the Kalman filter output is optimal. To put this in concrete terms, for our dog tracking problem this means that the estimates for both position and velocity will be optimal - a value of $\\mathbf{K}$ that made the position extremely accurate but the velocity very inaccurate would be rejected in favor of a $\\mathbf{K}$ that made both position and velocity just somewhat accurate."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Our next line is:\n",
|
||||
" $$\\mathbf{x}=\\mathbf{x}' +\\mathbf{K\\gamma}\\tag{5}$$\n",
|
||||
"\n",
|
||||
"This just multiplies the residual by the Kalman gain, and adds it to the state variable. In other words, this is the computation of our new estimate.\n",
|
||||
"\n",
|
||||
"Finally, we have:\n",
|
||||
"\n",
|
||||
"$$\\mathbf{P}=(\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\tag{6}$$\n",
|
||||
"\n",
|
||||
"$I$ is the identity matrix, and is the way we represent $1$ in multiple dimensions. $H$ is our measurement function, and is a constant. So, simplified, this is simply $P = (1-cK)P$. $K$ is our ratio of how much prediction vs measurement we use. So, if $K$ is large then $(1-cK)$ is small, and P will be made smaller than it was. If $K$ is small, then $(1-cK)$ is large, and P will be made larger than it was. So we adjust the size of our uncertainty by some factor of the *Kalman gain*. I would like to draw your attention back to the g-h filter, which included this Python code:\n",
|
||||
"\n",
|
||||
" # update filter \n",
|
||||
" w = w * (1-scale_factor) + z * scale_factor\n",
|
||||
"\n",
|
||||
"This multidimensional Kalman filter equation is partially implementing this calculation for the variance instead of the state variable."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now we have the measurement steps. The first equation is\n",
|
||||
"\n",
|
||||
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Gu}\\tag{1}$$\n",
|
||||
"\n",
|
||||
"This is just our state transition equation which we have already discussed. $\\mathbf{Fx}$ multiplies $\\mathbf{x}$ with the state transition matrix to compute the next state. $G$ and $u$ add in the contribution of the control input $\\mathbf{u}$, if any.\n",
|
||||
"\n",
|
||||
"The final equation is:\n",
|
||||
"$$\\mathbf{P} = \\mathbf{FPF}^T + \\mathbf{Q}\\tag{2}$$\n",
|
||||
"\n",
|
||||
"$\\mathbf{FPF}^T$ is the way we put $\\mathbf{P}$ into the process space using linear algebra so that we can add in the process noise $\\mathbf{Q}$ to it."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Design of the Process Noise Matrix"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 2
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in new issue
Block a user