Added Designing nonlinear filter chapter.
Worked on trying to incorporate air drag, but didn't get far. Need to study more.
This commit is contained in:
Roger Labbe committed
1 parent
0697383a4e
commit
60f1b5a05e
6 files changed
+994
-1039
No files matched your search
+55
-1017
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,697 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:0429c90c9b36cc68cd59fe76a34a2102793651fe7412e585c4ea6398f50fd99a"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
"worksheets": [
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 1,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Designing Nonlinear Kalman Filters"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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()\n",
|
||||
"\n",
|
||||
"#%install_ext secnum.py\n",
|
||||
"#%load_ext secnum\n",
|
||||
"#%secnum"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"html": [
|
||||
"<style>\n",
|
||||
" div.cell{\n",
|
||||
" width: 850px;\n",
|
||||
" margin-left: 0% !important;\n",
|
||||
" margin-right: auto;\n",
|
||||
" }\n",
|
||||
" div.text_cell code {\n",
|
||||
" background: #F6F6F9;\n",
|
||||
" color: #0000FF;\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": 2,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x1e888d0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 2
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Introduction"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"blah blah"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We see that the Kalman filter reasonably tracks the ball. However, as already explained, this is a silly example; we can predict trajectories in a vacuum with arbitrary precision; using a Kalman filter in this example is a needless complication."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Kalman Filter with Air Drag"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"I will dispense with the step 1, step 2, type approach and proceed in a more natural style that you would use in a non-toy engineering problem. We have already developed a Kalman filter that does excellently at tracking a ball in a vacuum, but that does not incorporate the effects of air drag into the model. We know that the process model is implemented with $\\textbf{F}$, so we will turn our attention to that immediately.\n",
|
||||
"\n",
|
||||
"Notionally, the computation that $\\textbf{F}$ computes is\n",
|
||||
"\n",
|
||||
"$$x' = Fx$$\n",
|
||||
"\n",
|
||||
"With no air drag, we had\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf{F} = \\begin{bmatrix}\n",
|
||||
"1 & \\Delta t & 0 & 0 & 0 \\\\\n",
|
||||
"0 & 1 & 0 & 0 & 0 \\\\\n",
|
||||
"0 & 0 & 1 & \\Delta t & \\frac{1}{2}{\\Delta t}^2 \\\\\n",
|
||||
"0 & 0 & 0 & 1 & \\Delta t \\\\\n",
|
||||
"0 & 0 & 0 & 0 & 1\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"which corresponds to the equations\n",
|
||||
"\n",
|
||||
"$$ \n",
|
||||
"\\begin{aligned}\n",
|
||||
"x &= x + v_x \\Delta t \\\\\n",
|
||||
"v_x &= v_x \\\\\n",
|
||||
"\\\\\n",
|
||||
"y &= y + v_y \\Delta t + \\frac{a_y}{2} {\\Delta t}^2 \\\\\n",
|
||||
"v_y &= v_y + a_y \\Delta t \\\\\n",
|
||||
"a_y &= a_y\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"From the section above we know that our new Euler equations must be\n",
|
||||
"\n",
|
||||
"$$ \n",
|
||||
"\\begin{aligned}\n",
|
||||
"x &= x + v_x \\Delta t \\\\\n",
|
||||
"v_x &= v_x \\\\\n",
|
||||
"\\\\\n",
|
||||
"y &= y + v_y \\Delta t + \\frac{a_y}{2} {\\Delta t}^2 \\\\\n",
|
||||
"v_y &= v_y + a_y \\Delta t \\\\\n",
|
||||
"a_y &= a_y\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Realistic 2D Position Sensors"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The position sensor in the last example are not very realistic. In general there is no 'raw' sensor that provides (x,y) coordinates. We have GPS, but GPS already uses a Kalman filter to create a filtered output; we should not be able to improve the signal by passing it through another Kalman filter unless we incorporate additional sensors to provide additional information. We will tackle that problem later. \n",
|
||||
"\n",
|
||||
"Consider the following set up. In an open field we put two transmitters at a known location, each transmitting a signal that we can detect. We process the signal and determine how far we are from that signal, with some noise. First, let's look at a visual depiction of that."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"\n",
|
||||
"circle1=plt.Circle((-4,0),5,color='r',fill=False,linewidth=10, alpha=.7)\n",
|
||||
"circle2=plt.Circle((4,0),5,color='b', fill=False, linewidth=5, alpha=.7)\n",
|
||||
"\n",
|
||||
"fig = plt.gcf()\n",
|
||||
"ax = fig.gca()\n",
|
||||
"\n",
|
||||
"plt.axis('equal')\n",
|
||||
"plt.xlim((-10,10))\n",
|
||||
"plt.ylim((-10,10))\n",
|
||||
"\n",
|
||||
"plt.plot ([-4,0], [0,3], 'r')\n",
|
||||
"plt.plot ([4,0], [0,3], 'b')\n",
|
||||
"plt.text(-4, -.5, \"A\", fontsize=16, horizontalalignment='center')\n",
|
||||
"plt.text(4, -.5, \"B\", fontsize=16, horizontalalignment='center')\n",
|
||||
"#plt.scatter ([-4],[0], 'r')\n",
|
||||
"\n",
|
||||
"ax.add_artist(circle1)\n",
|
||||
"ax.add_artist(circle2)\n",
|
||||
"plt.show()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAXEAAAD9CAYAAABKgkezAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x44c8550>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 113
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Here I have attempted to show transmitter A, drawn in red, at (-4,0) and a second one B, drawn in blue, at (4,0). The red and blue circles show the range from the transmitters to the robot, with the width illustrating the effect of the $1\\sigma$ angular error for each transmitter. Here I have given the red transmitter more error than the blue one. The most probable position for the robot is where the two circles intersect, which I have depicted with the red and blue lines. You will object that we have two intersections, not one, but we will see how we deal with that when we design the measurement function.\n",
|
||||
"\n",
|
||||
"This is a very common sensor set up. Aircraft still use this system to navigate, where it is called DME (Distance Measuring Equipment). Today GPS is a much more common navigation sytem, but I have worked on an aircraft where we integrated sensors like this into our filter along with the GPS, INS, altimeters, etc. We will tackle what is called *multi-sensor fusion* later; for now we will just address this simple configuration.\n",
|
||||
"\n",
|
||||
"The first step is to design our state variables. We will assume that the robot is travelling in a straight direction with constant velocity. This is unlikely to be true for a long period of time, but is acceptable for short periods of time. This does not differ from the previous problem - we will want to track the values for the robot's position and velocity. Hence,\n",
|
||||
"\n",
|
||||
"$$\\mathbf{x} = \n",
|
||||
"\\begin{bmatrix}x\\\\v_x\\\\y\\\\v_y\\end{bmatrix}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The next step is to design the state transistion function. This also will be the same as the previous problem, so without further ado,\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf{x}' = \\begin{bmatrix}1& \\Delta t& 0& 0\\\\0& 1& 0& 0\\\\0& 0& 1& \\Delta t\\\\ 0& 0& 0& 1\\end{bmatrix}\\mathbf{x}$$\n",
|
||||
"\n",
|
||||
"The next step is to design the control inputs. We have none, so we set ${\\mathbf{B}}=0$.\n",
|
||||
"\n",
|
||||
"The next step is to design the measurement function $\\mathbf{z} = \\mathbf{Hx}$. We can model the measurement using the Pythagorean theorem.\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"z_a = \\sqrt{(x-x_A)^2 + (y-y_A)^2} + v_a\\\\[1em]\n",
|
||||
"z_b = \\sqrt{(x-x_B])^2 + (y-y_B)^2} + v_b\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"where $v_a$ and $v_b$ are white noise.\n",
|
||||
"\n",
|
||||
"We see an immediate problem. The Kalman filter is designed for linear equations, and this is obviously nonlinear. In the next chapters we will look at several ways to handle nonlinear problems in a robust way, but for now we will do something simpler. If we know the approximate position of the robot than we can linearize these equations around that point. I could develop the generalized mathematics for this technique now, but instead let me just present the worked example to give context to that development."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Instead of computing $\\mathbf{H}$ we will compute the partial derivative of $\\mathbf{H}$ with respect to the robot's position $\\mathbf{x}$. You are probably familiar with the concept of partial derivative, but if not, it just means how $\\mathbf{H}$ changes with respect to the robot's position.\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial\\mathbf{h}}{\\partial\\mathbf{x}}=\n",
|
||||
"\\small\\begin{bmatrix}\n",
|
||||
"\\frac{(x_r - x_A)}{\\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 & \n",
|
||||
"\\frac{(y_r - y_A)}{\\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 \\\\\n",
|
||||
"\\frac{(x_r - x_B)}{\\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 &\n",
|
||||
"\\frac{(y_r - y_B)}{\\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"In a nutshell, the entry (0,0) contains the difference between the x coordinate of the robot and transmitter A's x coordinate divided by the distance between the robot and A. (2,0) contains the same, except for the y coordintates of the robot and transmitters. The bottom row contains the same computations, except for transmitter B. The 0 entries account for the velocity components of the state variables; naturally the range does notvide us with velocity.\n",
|
||||
"\n",
|
||||
"The values in this matrix change as the robot's position changes, so this is no longer a constant; we will have to recompute it for every time step of the filter.\n",
|
||||
"\n",
|
||||
"If you look at this you may realize that this is just a computation of x/dist and y/dist, so we can switch this to a trigometic form with no loss of generality:\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial\\mathbf{h}}{\\partial\\mathbf{x}}=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"-\\cos{\\theta_A} & 0 & -\\sin{\\theta_A} & 0 \\\\\n",
|
||||
"-\\cos{\\theta_B} & 0 & -\\sin{\\theta_B} & 0\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"However, this raises a huge problem. We are no longer computing $\\mathbf{H}$, but $\\Delta\\mathbf{H}$, the change of $\\mathbf{H}$. If we passed this into our Kalman filter without altering the rest of the design the output would be nonsense. Recall, for example, that we multiply $\\mathbf{Hx}$ to generate the measurements that would result from the given estimate of $\\mathbf{x}$ But now that $\\mathbf{H}$ is linearized around our position it contains the *change* in the measurement function. \n",
|
||||
"\n",
|
||||
"We are forced, therefore, to use the *change* in $\\mathbf{x}$ for our state variables. So we have to go back and redesign our state variables. \n",
|
||||
"\n",
|
||||
">Please note this is a completely normal occurance in designing Kalman filters. The textbooks present examples like this as *fait accompli*, as if it is trivially obvious that the state variables needed to be velocities, not positions. Perhaps once you do enough of these problems it would be trivially obvious, but at that point why are you reading a textbook? I find myself reading through a presentation multiple times, trying to figure out why they made a choice, finally to realize that it is because of the consequences of something on the next page. My presentation is longer, but it reflects what actually happens when you design a filter. You make what seem reasonable design choices, and as you move forward you discover properties that require you to recast your earlier steps. As a result, I am going to somewhat abandon my **step 1**, **step 2**, etc., approach, since so many real problems are not quite that straightforward."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"If our state variables contain the velocities of the robot and not the position then how do we track where the robot is? We can't. Kalman filters that are linearized in this fashion use what is called a *nominal trajectory* - i.e. you assume a position and track direction, and then apply the changes in velocity and acceleration to compute the changes in that trajectory. How could it be otherwise? Recall the graphic showing the intersection of the two range circles - there are two areas of intersection. Think of what this would look like if the two transmitters were very close to each other - the intersections would be two very long cresent shapes. This Kalman filter, as designed, has no way of knowing your true position from only distance measurements to the transmitters. Perhaps your mind is already leaping to ways of working around this problem. If so, stay engaged, as later sections and chapters will provide you with these techniques. Presenting the full solution all at once leads to more confusion than insight, in my opinion. \n",
|
||||
"\n",
|
||||
"So let's redesign our *state transition function*. We are assuming constant velocity and no acceleration, giving state equations of\n",
|
||||
"$$\n",
|
||||
"\\dot{x}' = \\dot{x} \\\\\n",
|
||||
"\\ddot{x}' = 0 \\\\\n",
|
||||
"\\dot{y}' = \\dot{y} \\\\\n",
|
||||
"\\dot{y}' = 0$$\n",
|
||||
"\n",
|
||||
"This gives us the the *state transition function* of\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf{F} = \\begin{bmatrix}0 &1 & 0& 0\\\\0& 0& 0& 0\\\\0& 0& 0& 1\\\\ 0& 0& 0& 0\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"A final complication comes from the measurements that we pass in. $\\mathbf{Hx}$ is now computing the *change* in the measurement from our nominal position, so the measurement that we pass in needs to be not the range to A and B, but the *change* in range from our measured range to our nomimal position. \n",
|
||||
"\n",
|
||||
"There is a lot here to take in, so let's work through the code bit by bit. First we will define a function to compute $\\frac{\\partial\\mathbf{h}}{\\partial\\mathbf{x}}$ for each time step."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def H_of (pos, pos_A, pos_B):\n",
|
||||
" \"\"\" Given the position of our object at 'pos' in 2D, and two transmitters\n",
|
||||
" A and B at positions 'pos_A' and 'pos_B', return the partial derivative\n",
|
||||
" of H\n",
|
||||
" \"\"\"\n",
|
||||
" \n",
|
||||
" from math import sin, cos, atan2\n",
|
||||
"\n",
|
||||
" theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])\n",
|
||||
" theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])\n",
|
||||
"\n",
|
||||
" return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],\n",
|
||||
" [-cos(theta_b), 0, -sin(theta_b), 0]])"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 114
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now we need to create our simulated sensor. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"class DMESensor(object):\n",
|
||||
" def __init__(self, pos_a, pos_b, noise_factor=1.0):\n",
|
||||
" self.A = pos_a\n",
|
||||
" self.B = pos_b\n",
|
||||
" self.noise_factor = noise_factor\n",
|
||||
" \n",
|
||||
" def range_of (self, pos):\n",
|
||||
" \"\"\" returns tuple containing noisy range data to A and B\n",
|
||||
" given a position 'pos'\n",
|
||||
" \"\"\"\n",
|
||||
" \n",
|
||||
" ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)\n",
|
||||
" rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)\n",
|
||||
" \n",
|
||||
" return (ra + random.randn()*self.noise_factor, \n",
|
||||
" rb + random.randn()*self.noise_factor)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 115
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now let's write the code to compute $\\frac{\\partial\\mathbf{h}}{\\partial\\mathbf{x}}$."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def dist(a,b):\n",
|
||||
" return math.sqrt ((a[0]-b[0])**2 + (a[1]-b[1])**2)\n",
|
||||
"\n",
|
||||
"def H_of (pos, pos_A, pos_B):\n",
|
||||
" dist_a = dist(pos, pos_A)\n",
|
||||
" dist_b = dist(pos, pos_B)\n",
|
||||
" \n",
|
||||
" return np.mat([[0, (pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a],\n",
|
||||
" [0, (pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b]])\n",
|
||||
" '''\n",
|
||||
" return np.mat([[(pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a,0],\n",
|
||||
" [(pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b,0]])\n",
|
||||
" '''"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 116
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Finally, we are ready for the Kalman filter code. I will position the transmitters at x=-100 and 100, both with y=-20. This gives me enough space to get good triangulation from both as the robot moves. I will start the robot at (0,0) and move by (1,1) each time step. \n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import math\n",
|
||||
"\n",
|
||||
"pos_a = (100,-20)\n",
|
||||
"pos_b = (-100, -20)\n",
|
||||
"\n",
|
||||
"f1 = KalmanFilter(dim_x=4, dim_z=2)\n",
|
||||
"\n",
|
||||
"f1.F = np.mat ([[0, 1, 0, 0],\n",
|
||||
" [0, 0, 0, 0],\n",
|
||||
" [0, 0, 0, 1],\n",
|
||||
" [0, 0, 0, 0]])\n",
|
||||
"\n",
|
||||
"f1.B = 0.\n",
|
||||
"\n",
|
||||
"f1.R *= 1.\n",
|
||||
"f1.Q *= .1\n",
|
||||
"\n",
|
||||
"f1.x = np.mat([1,0,1,0]).T\n",
|
||||
"f1.P = np.eye(4) * 5.\n",
|
||||
"\n",
|
||||
"# initialize storage and other variables for the run\n",
|
||||
"count = 30\n",
|
||||
"xs, ys = [],[]\n",
|
||||
"pxs, pys = [],[]\n",
|
||||
"\n",
|
||||
"# create the simulated sensor\n",
|
||||
"d = DMESensor (pos_a, pos_b, noise_factor=3.)\n",
|
||||
"\n",
|
||||
"# pos will contain our nominal position since the filter does not\n",
|
||||
"# maintain position.\n",
|
||||
"pos = [0,0]\n",
|
||||
"\n",
|
||||
"for i in range(count):\n",
|
||||
" # move (1,1) each step, so just use i\n",
|
||||
" pos = [i,i]\n",
|
||||
" \n",
|
||||
" # compute the difference in range between the nominal track and measured \n",
|
||||
" # ranges\n",
|
||||
" ra,rb = d.range_of(pos)\n",
|
||||
" rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))\n",
|
||||
" z = np.mat([[ra-rx],[rb-ry]])\n",
|
||||
"\n",
|
||||
" # compute linearized H for this time step\n",
|
||||
" f1.H = H_of (pos, pos_a, pos_b)\n",
|
||||
"\n",
|
||||
" # store stuff so we can plot it later\n",
|
||||
" xs.append (f1.x[0,0]+i)\n",
|
||||
" ys.append (f1.x[2,0]+i)\n",
|
||||
" pxs.append (pos[0])\n",
|
||||
" pys.append(pos[1])\n",
|
||||
" \n",
|
||||
" # perform the Kalman filter steps\n",
|
||||
" f1.predict ()\n",
|
||||
" f1.update(z)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"p1, = plt.plot (xs, ys, 'r--')\n",
|
||||
"p2, = plt.plot (pxs, pys)\n",
|
||||
"plt.legend([p1,p2], ['filter', 'ideal'], 2)\n",
|
||||
"plt.show()\n",
|
||||
"\n"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAXEAAAECCAYAAAAIMefLAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x4a48d50>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 117
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Linearizing the Kalman Filter"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now that we have seen an example of linearizing the Kalman filter we are in a position to better understand the math. \n",
|
||||
"\n",
|
||||
"We start by assuming some function $\\mathbf f$"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
}
|
||||
]
|
||||
}
|
||||
+55
-9
@@ -12,18 +12,34 @@ import numpy.random as random
|
||||
|
||||
class KalmanFilter:
|
||||
|
||||
def __init__(self, dim):
|
||||
""" Create a Kalman filter of dimension 'dim'"""
|
||||
def __init__(self, dim_x, dim_z, use_short_form=False):
|
||||
""" Create a Kalman filter of dimension 'dim', where dimension is the
|
||||
number of state variables.
|
||||
|
||||
use_short_form will force the filter to use the short form equation
|
||||
for computing covariance: (I-KH)P. This is the equation that results
|
||||
from deriving the Kalman filter equations. It is efficient to compute
|
||||
but not numerically stable for very long runs. The long form,
|
||||
implemented in update(), is very slightly suboptimal, and takes longer
|
||||
to compute, but is stable. I have chosen to make the long form the
|
||||
default behavior. If you really want the short form, either call
|
||||
update_short_form() directly, or set 'use_short_form' to true. This
|
||||
causes update() to use the short form. So, if you do it this way
|
||||
you will never be able to use the long form again with this object.
|
||||
"""
|
||||
|
||||
self.x = 0 # state
|
||||
self.P = np.matrix(np.eye(dim)) # uncertainty covariance
|
||||
self.Q = np.matrix(np.eye(dim)) # process uncertainty
|
||||
self.u = np.matrix(np.zeros((dim,1))) # motion vector
|
||||
self.P = np.matrix(np.eye(dim_x)) # uncertainty covariance
|
||||
self.Q = np.matrix(np.eye(dim_x)) # process uncertainty
|
||||
self.u = np.matrix(np.zeros((dim_x,1))) # motion vector
|
||||
self.B = 0
|
||||
self.F = 0 # state transition matrix
|
||||
self.H = 0 # Measurement function (maps state to measurements)
|
||||
self.R = np.matrix(np.eye(1)) # state uncertainty
|
||||
self.I = np.matrix(np.eye(dim))
|
||||
self.R = np.matrix(np.eye(dim_z)) # state uncertainty
|
||||
self.I = np.matrix(np.eye(dim_x))
|
||||
|
||||
if use_short_form:
|
||||
self.update = self.update_short_form
|
||||
|
||||
|
||||
def update(self, Z):
|
||||
@@ -39,16 +55,46 @@ class KalmanFilter:
|
||||
K = self.P * self.H.T * linalg.inv(S) # map system uncertainty into kalman gain
|
||||
|
||||
self.x = self.x + (K*y) # predict new x with residual scaled by the kalman gain
|
||||
|
||||
KH = K*self.H
|
||||
I_KH = self.I - KH
|
||||
self.P = I_KH*self.P * I_KH.T + K*self.R*K.T
|
||||
|
||||
|
||||
def update_short_form(self, Z):
|
||||
"""
|
||||
Add a new measurement to the kalman filter.
|
||||
|
||||
Uses the 'short form' computation for P, which is mathematically
|
||||
correct, but perhaps not that stable when dealing with large data
|
||||
sets. But, it is fast to compute. Advice varies; some say to never
|
||||
use this. My opinion - if the longer form in update() executes too
|
||||
slowly to run in real time, what choice do you have. But that should
|
||||
be a rare case, so the long form is the default use
|
||||
"""
|
||||
|
||||
# measurement update
|
||||
y = Z - (self.H * self.x) # error (residual) between measurement and prediction
|
||||
S = (self.H * self.P * self.H.T) + self.R # project system uncertainty into measurment space + measurement noise(R)
|
||||
|
||||
|
||||
K = self.P * self.H.T * linalg.inv(S) # map system uncertainty into kalman gain
|
||||
|
||||
self.x = self.x + (K*y) # predict new x with residual scaled by the kalman gain
|
||||
|
||||
# and compute the new covariance
|
||||
self.P = (self.I - (K*self.H))*self.P # and compute the new covariance
|
||||
|
||||
|
||||
|
||||
def predict(self):
|
||||
# prediction
|
||||
self.x = (self.F*self.x) + self.u
|
||||
self.x = (self.F*self.x) + self.B * self.u
|
||||
self.P = self.F * self.P * self.F.T + self.Q
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
f = KalmanFilter (dim=2)
|
||||
f = KalmanFilter (dim_x=2, dim_z=2)
|
||||
|
||||
f.x = np.matrix([[2.],
|
||||
[0.]]) # initial state (location and velocity)
|
||||
|
||||
+2
-2
@@ -74,8 +74,8 @@ class BallRungeKutta(object):
|
||||
def step (self, dt):
|
||||
self.x = rk4 (self.x, self.t, dt, fx)
|
||||
self.y = rk4 (self.y, self.t, dt, fy)
|
||||
self.t += dt
|
||||
|
||||
self.t += dt
|
||||
return (self.x, self.y)
|
||||
|
||||
|
||||
def ball_scipy(y0, vel, omega, dt):
|
||||
|
||||
+178
-7
@@ -11,6 +11,7 @@ from math import radians, sin, cos, sqrt, exp
|
||||
import numpy.random as random
|
||||
import matplotlib.markers as markers
|
||||
import matplotlib.pyplot as plt
|
||||
from RungeKutta import *
|
||||
|
||||
|
||||
class BallPath(object):
|
||||
@@ -33,7 +34,89 @@ class BallPath(object):
|
||||
return (x +random.randn()*self.noise[0], y +random.randn()*self.noise[1])
|
||||
|
||||
|
||||
def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
|
||||
|
||||
g = 9.8 # gravitational constant
|
||||
f1 = KalmanFilter(dim_x=5, dim_z=2)
|
||||
|
||||
ay = .5*dt**2
|
||||
f1.F = np.mat ([[1, dt, 0, 0, 0], # x = x0+dx*dt
|
||||
[0, 1, 0, 0, 0], # dx = dx
|
||||
[0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
|
||||
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
|
||||
[0, 0, 0, 0, 1]]) # ddy = -g.
|
||||
|
||||
f1.H = np.mat([
|
||||
[1, 0, 0, 0, 0],
|
||||
[0, 0, 1, 0, 0]])
|
||||
|
||||
f1.R *= r
|
||||
f1.Q *= q
|
||||
|
||||
omega = radians(omega)
|
||||
vx = cos(omega) * v0
|
||||
vy = sin(omega) * v0
|
||||
|
||||
f1.x = np.mat([x,vx,y,vy,-9.8]).T
|
||||
|
||||
return f1
|
||||
|
||||
|
||||
|
||||
def ball_kf_noay(x, y, omega, v0, dt, r=0.5, q=0.02):
|
||||
|
||||
g = 9.8 # gravitational constant
|
||||
f1 = KalmanFilter(dim_x=5, dim_z=2)
|
||||
|
||||
ay = .5*dt**2
|
||||
f1.F = np.mat ([[1, dt, 0, 0, 0], # x = x0+dx*dt
|
||||
[0, 1, 0, 0, 0], # dx = dx
|
||||
[0, 0, 1, dt, 0], # y = y0 +dy*dt
|
||||
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
|
||||
[0, 0, 0, 0, 1]]) # ddy = -g.
|
||||
|
||||
f1.H = np.mat([
|
||||
[1, 0, 0, 0, 0],
|
||||
[0, 0, 1, 0, 0]])
|
||||
|
||||
f1.R *= r
|
||||
f1.Q *= q
|
||||
|
||||
omega = radians(omega)
|
||||
vx = cos(omega) * v0
|
||||
vy = sin(omega) * v0
|
||||
|
||||
f1.x = np.mat([x,vx,y,vy,-9.8]).T
|
||||
|
||||
return f1
|
||||
|
||||
|
||||
def test_kf():
|
||||
dt = 0.1
|
||||
t = 0
|
||||
f1 = ball_kf (0,1, 35, 50, 0.1)
|
||||
f2 = ball_kf_noay (0,1, 35, 50, 0.1)
|
||||
|
||||
path = BallPath( 0, 1, 35, 50, noise=(0,0))
|
||||
path_rk = BallRungeKutta(0, 1, 50, 35)
|
||||
|
||||
xs = []
|
||||
ys = []
|
||||
while f1.x[2,0]>= 0:
|
||||
t += dt
|
||||
f1.predict()
|
||||
f2.predict()
|
||||
#x,y = path.pos_at_t(t)
|
||||
x,y = path_rk.step(dt)
|
||||
xs.append(x)
|
||||
ys.append(y)
|
||||
|
||||
plt.scatter (f1.x[0,0], f1.x[2,0], color='blue',alpha=0.6)
|
||||
plt.scatter (f2.x[0,0], f2.x[2,0], color='red', alpha=0.6)
|
||||
|
||||
plt.plot(xs, ys, c='g')
|
||||
|
||||
|
||||
class BaseballPath(object):
|
||||
def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)):
|
||||
""" Create baseball path object in 2D (y=height above ground)
|
||||
@@ -90,8 +173,9 @@ def test_baseball_path():
|
||||
while ball.y > 0:
|
||||
ball.update (0.1, 0.)
|
||||
plt.scatter (ball.x, ball.y)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def test_ball_path():
|
||||
|
||||
y = 15
|
||||
@@ -103,7 +187,7 @@ def test_ball_path():
|
||||
dt = 1
|
||||
|
||||
|
||||
f1 = KalmanFilter(dim=6)
|
||||
f1 = KalmanFilter(dim_x=6, dim_z=2)
|
||||
dt = 1/30. # time step
|
||||
|
||||
ay = -.5*dt**2
|
||||
@@ -114,7 +198,7 @@ def test_ball_path():
|
||||
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
|
||||
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
|
||||
[0, 0, 0, 0, 0, 1]]) # ddy = -g
|
||||
f1.B = 0.
|
||||
|
||||
|
||||
f1.H = np.mat([
|
||||
[1, 0, 0, 0, 0, 0],
|
||||
@@ -143,8 +227,95 @@ def test_ball_path():
|
||||
plt.scatter(f1.x[0,0],f1.x[3,0], color='green')
|
||||
|
||||
|
||||
|
||||
def drag_force (velocity):
|
||||
""" Returns the force on a baseball due to air drag at
|
||||
the specified velocity. Units are SI
|
||||
"""
|
||||
B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
|
||||
return B_m * velocity
|
||||
|
||||
def update_drag(f, dt):
|
||||
vx = f.x[1,0]
|
||||
vy = f.x[3,0]
|
||||
v = sqrt(vx**2 + vy**2)
|
||||
F = -drag_force(v)
|
||||
print F
|
||||
f.u[0,0] = -drag_force(vx)
|
||||
f.u[1,0] = -drag_force(vy)
|
||||
#f.x[2,0]=F*vx
|
||||
#f.x[5,0]=F*vy
|
||||
|
||||
def test_kf_drag():
|
||||
|
||||
y = 1
|
||||
x = 0
|
||||
omega = 35.
|
||||
noise = [0,0]
|
||||
v0 = 50.
|
||||
ball = BaseballPath (x0=x, y0=y,
|
||||
launch_angle_deg=omega,
|
||||
velocity_ms=v0, noise=noise)
|
||||
#ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
|
||||
|
||||
dt = 1
|
||||
|
||||
|
||||
f1 = KalmanFilter(dim_x=6, dim_z=2)
|
||||
dt = 1/30. # time step
|
||||
|
||||
ay = -.5*dt**2
|
||||
ax = .5*dt**2
|
||||
|
||||
f1.F = np.mat ([[1, dt, ax, 0, 0, 0], # x=x0+dx*dt
|
||||
[0, 1, dt, 0, 0, 0], # dx = dx
|
||||
[0, 0, 1, 0, 0, 0], # ddx = 0
|
||||
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
|
||||
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
|
||||
[0, 0, 0, 0, 0, 1]]) # ddy = -g
|
||||
|
||||
# We will inject air drag using Bu
|
||||
f1.B = np.mat([[0., 0., 1., 0., 0., 0.],
|
||||
[0., 0., 0., 0., 0., 1.]]).T
|
||||
|
||||
f1.u = np.mat([[0., 0.]]).T
|
||||
|
||||
f1.H = np.mat([
|
||||
[1, 0, 0, 0, 0, 0],
|
||||
[0, 0, 0, 1, 0, 0]])
|
||||
|
||||
f1.R = np.eye(2) * 5
|
||||
f1.Q = np.eye(6) * 0.
|
||||
|
||||
omega = radians(omega)
|
||||
vx = cos(omega) * v0
|
||||
vy = sin(omega) * v0
|
||||
|
||||
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
|
||||
|
||||
f1.P = np.eye(6) * 500.
|
||||
|
||||
z = np.mat([[0,0]]).T
|
||||
markers.MarkerStyle(fillstyle='none')
|
||||
|
||||
np.set_printoptions(precision=4)
|
||||
|
||||
t=0
|
||||
while f1.x[3,0] > 0:
|
||||
t+=dt
|
||||
|
||||
#f1.update (z)
|
||||
x,y = ball.update(dt)
|
||||
#x,y = ball.pos_at_t(t)
|
||||
update_drag(f1, dt)
|
||||
f1.predict()
|
||||
print f1.x.T
|
||||
plt.scatter(f1.x[0,0],f1.x[3,0], color='red', alpha=0.5)
|
||||
plt.scatter (x,y)
|
||||
return f1
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_baseball_path()
|
||||
#test_baseball_path()
|
||||
#test_ball_path()
|
||||
|
||||
|
||||
#test_kf()
|
||||
f1 = test_kf_drag()
|
||||
@@ -83,27 +83,30 @@
|
||||
" \n",
|
||||
"EKF and UKF are linear approximations of nonlinear problems. Unless programmed carefully, they are not numerically stable. We discuss some common approaches to this problem.\n",
|
||||
"\n",
|
||||
"[**Chapter 11: Designing Nonlinear Kalman Filters**](http://nbviewer.ipython.org/urls/raw.github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/Designing_Nonlinear_Kalman_Filters.ipynb)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"[**Chapter ??: Least Squares Filters](not implemented)\n",
|
||||
"\n",
|
||||
"Not sure where in order to put this. \n",
|
||||
"\n",
|
||||
"\n",
|
||||
"[**Chapter 11: Smoothing**](not implemented)\n",
|
||||
"[**Chapter 12: Smoothing**](not implemented)\n",
|
||||
" \n",
|
||||
"Kalman filters are recursive, and thus very suitable for real time filtering. However, they work well for post-processing data. We discuss some common approaches.\n",
|
||||
" \n",
|
||||
" \n",
|
||||
"[**Chapter 12: Control Theory**](not implemented)\n",
|
||||
"[**Chapter 13: Control Theory**](not implemented)\n",
|
||||
"\n",
|
||||
"This book focuses on tracking and filtering, but Kalman filters have an input for control. We discuss using Kalman filters to control devices like robots, CNC machinery and so on.\n",
|
||||
" \n",
|
||||
" \n",
|
||||
"[**Chapter 13: Particle Filters**](not implemented)\n",
|
||||
"[**Chapter 14: Particle Filters**](not implemented)\n",
|
||||
" \n",
|
||||
"Particle filters uses a Monte Carlo technique to \n",
|
||||
" \n",
|
||||
" \n",
|
||||
"[**Chapter 14: Multihypothesis Tracking**](not implemented)\n",
|
||||
"[**Chapter 15: Multihypothesis Tracking**](not implemented)\n",
|
||||
" \n",
|
||||
"description\n",
|
||||
"\n",
|
||||
|
||||
Reference in new issue
Block a user