daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Copy editing.

Browse Source
This commit is contained in:
Roger Labbe 2016-01-30 08:29:22 -08:00
parent ac2c27119b
commit 57dda86f18
7 changed files with 261 additions and 413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
09-Nonlinear-Filtering.ipynb
View File

@ -310,13 +310,8 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"## Introduction"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Introduction\n",
"\n",
"The Kalman filter that we have developed uses linear equations, and so the filter can only handle linear problems. But the world is nonlinear, and so the classic filter that we have been studying to this point can have very limited utility. \n",
"\n",
"There can be nonlinearity in the process model. Suppose we wanted to track the motion of a weight on a spring, such as an automobile's suspension. The equation for the motion with $m$ being the mass, $k$ the spring constant, and $c$ the damping force is \n",
@ -333,9 +328,11 @@
"\n",
"It is almost true to state that the only equation anyone knows how to solve is $\\mathbf{Ax}=\\mathbf{b}$. We only really know how to do linear algebra. I can give you any linear set of equations and you can either solve it or prove that it haas no solution. \n",
"\n",
"Anyone with formal education in math or physics has spent years learning various analytic ways to solve integrals, differential equations and so on. Yet even trivial physical systems produce equations that cannot be solved analytically. I can take an equation that you are able to integrate, insert a $\\log$ term, and render it insolvable. It is stuff like this that leads to jokes about physicists stating \"assume a spherical cow on a frictionless surface in a vacuum...\". Without making extreme simplifications most physical problems do not have analytic solutions.\n",
"Anyone with formal education in math or physics has spent years learning various analytic ways to solve integrals, differential equations and so on. Yet even trivial physical systems produce equations that cannot be solved analytically. I can take an equation that you are able to integrate, insert a $\\log$ term, and render it insolvable. This leads to jokes about physicists stating \"assume a spherical cow on a frictionless surface in a vacuum...\". Without making extreme simplifications most physical problems do not have analytic solutions.\n",
"\n",
"How do we do things like model airflow over an aircraft in a computer, or predict weather, or track missiles with a Kalman filter? We retreat to what we know: $\\mathbf{Ax}=\\mathbf{b}$. We find some way to linearize the problem, turning it into a set of linear equations, and then use linear algebra software packages to compute an approximate solution. Linearizing a nonlinear problem gives us inexact answers, and in a recursive algorithm like a Kalman filter or weather tracking system these small errors can sometimes reinforce each other at each step, quickly causing the algorithm to spit out nonsense. \n",
"How do we do things like model airflow over an aircraft in a computer, or predict weather, or track missiles with a Kalman filter? We retreat to what we know: $\\mathbf{Ax}=\\mathbf{b}$. We find some way to linearize the problem, turning it into a set of linear equations, and then use linear algebra software packages to compute an approximate solution. \n",
"\n",
"Linearizing a nonlinear problem gives us inexact answers, and in a recursive algorithm like a Kalman filter or weather tracking system these small errors can sometimes reinforce each other at each step, quickly causing the algorithm to spit out nonsense. \n",
"\n",
"What we are about to embark upon is a difficult problem. There is not one obvious, correct, mathematically optimal solution anymore. We will be using approximations, we will be introducing errors into our computations, and we will forever be battling filters that *diverge*, that is, filters whose numerical errors overwhelm the solution. \n",
"\n",
@ -346,24 +343,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"## The Problem with Nonlinearity"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"As asserted in the introduction the only math you really know how to do is linear math, that is, equations of the form \n",
"## The Problem with Nonlinearity\n",
"\n",
"$$ A\\mathbf{x}=\\mathbf{b}$$\n",
"\n",
"That may strike you as hyperbole. After all, in this book we have integrated a polynomial to get distance from velocity and time. We know how to integrate polynomials, and so we are able to find the closed form equation for distance given velocity and time:\n",
"\n",
"$$\\int{(vt+v_0)}\\,dt = \\frac{a}{2}t^2+v_0t+d_0$$\n",
"\n",
"That's a nonlinear equation that we know how to solve. But it is also a very special form. You spent a lot of time, probably at least a year, learning how to integrate various terms, and you still can not integrate some arbitrary equation - no one can. We don't know how. If you took freshman Physics you perhaps remember homework involving sliding frictionless blocks on a perfect plane and other toy problems. At the end of the course you were almost entirely unequipped to solve real world problems because the real world is nonlinear, and you were taught linear, closed forms of equations. It made the math tractable, but mostly useless. \n",
"\n",
"The mathematics of the Kalman filter is beautiful in part due to the Gaussian equation being so special. It is nonlinear, but when we add and multiply them using linear algebra we get another Gaussian equation as a result. That is very rare. $\\sin{x}*\\sin{y}$ does not yield a $\\sin$ as an output.\n",
"The mathematics of the Kalman filter is beautiful in part due to the Gaussian equation being so special. It is nonlinear, but when we add and multiply them we get another Gaussian as a result. That is very rare. $\\sin{x}*\\sin{y}$ does not yield a $\\sin$ as an output.\n",
"\n",
"What I mean by linearity may be obvious, but there are some subtleties. The mathematical requirements are twofold:\n",
"\n",
@ -385,25 +367,20 @@
"\n",
"$$L(f(t)) = f(t) + 1$$\n",
"\n",
"is not linear because $L(0) = 1$! Be careful!"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## An Intuitive Look at the Problem"
"is not linear because $L(0) = 1$. Be careful!"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## An Intuitive Look at the Problem\n",
"\n",
"I particularly like the following way of looking at the problem, which I am borrowing from Dan Simon's *Optimal State Estimation* [[1]](#[1]). Consider a tracking problem where we get the range and bearing to a target, and we want to track its position. The reported distance is 50 km, and the reported angle is 90$^\\circ$. Assume that the errors in both range and angle are distributed in a Gaussian manner. Given an infinite number of measurements what is the expected value of the position?\n",
"\n",
"I have been recommending using intuition to gain insight, so let's see how it fares for this problem (hint: nonlinear problems are *not* intuitive). We might reason that since the mean of the range will be 50 km, and the mean of the angle will be 90$^\\circ$, that clearly the answer will be x=0 km, y=50 km.\n",
"I have been recommending using intuition to gain insight, so let's see how it fares for this problem. We might reason that since the mean of the range will be 50 km, and the mean of the angle will be 90$^\\circ$, that the answer will be x=0 km, y=50 km.\n",
"\n",
"Well, let's plot that and find out. Here are 3000 points plotted with a normal distribution of the distance of 0.4 km, and the angle having a normal distribution of 0.35 radians. We compute the average of the all of the positions, and display it as a star. Our intuition is displayed with a large circle."
"Let's plot that and find out. Here are 3000 points plotted with a normal distribution of the distance of 0.4 km, and the angle having a normal distribution of 0.35 radians. We compute the average of the all of the positions, and display it as a star. Our intuition is displayed with a large circle."
]
},
{
@ -454,14 +431,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"## The Effect of Nonlinear Functions on Gaussians"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Gaussians are not closed under an arbitrary nonlinear function. Recall the equations of the Kalman filter - at each evolution (prediction) we pass the Gaussian representing the state through the process function to get the Gaussian at time $k$. Our process function was always linear, so the output was always another Gaussian. Let's look at that on a graph. I will take an arbitrary Gaussian and pass it through the function $f(x) = 2x + 1$ and plot the result. We know how to do this analytically, but lets use sampling. I will generate 500,000 points with a normal distribution, pass them through $f(x)$, and plot the results. I do it this way because the next example will be nonlinear, and we will have no way to compute this analytically."
"## The Effect of Nonlinear Functions on Gaussians\n",
"\n",
"Gaussians are not closed under an arbitrary nonlinear function. Recall the equations of the Kalman filter - at each evolution we pass the Gaussian representing the state through the process function to get the Gaussian at time $k$. Our process function was always linear, so the output was always another Gaussian. Let's look at that on a graph. I will take an arbitrary Gaussian and pass it through the function $f(x) = 2x + 1$ and plot the result. We know how to do this analytically, but lets use sampling. I will generate 500,000 points with a normal distribution, pass them through $f(x)$, and plot the results. I do it this way because the next example will be nonlinear, and we will have no way to compute this analytically."
]
},
{
@ -569,7 +541,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"This result may be somewhat surprising to you. The function looks \"fairly\" linear - it is pretty close to a straight line, but the probability distribution of the output is completely different from a Gaussian. Recall the equations for multiplying two univariate Gaussians:\n",
"This result may be somewhat surprising to you. The function looks \"fairly\" linear, but the probability distribution of the output is completely different from a Gaussian. Recall the equations for multiplying two univariate Gaussians:\n",
"\n",
"$$\\begin{aligned}\n",
"\\mu &=\\frac{\\sigma_1^2 \\mu_2 + \\sigma_2^2 \\mu_1} {\\sigma_1^2 + \\sigma_2^2} \\\\\n",
@ -730,7 +702,8 @@
"cell_type": "code",
"execution_count": 10,
"metadata": {
"collapsed": false
"collapsed": false,
"scrolled": true
},
"outputs": [
{
@ -918,7 +891,7 @@
"The costs should be clear. It is computationally expensive to test tens of thousands of points for every step in the filter. But modern CPUs are very fast, and this is a good problem for GPUs because the part of the algorithm is parallelizable. Another cost is that the answer is not mathematical. With a Kalman filter my covariance matrix gives me important information about the amount of error in the estimate. The particle filter does not give me a rigorous way to compute this. Finally, the output of the filter is a cloud of points; I then have to figure out how to interpret it. Usually you will be doing something like taking the mean and standard deviations of the points, but this is a difficult problem. There are still many points that do not 'belong' to a tracked object, so you first have to run some sort of clustering algorithm to first find the points that seem to be tracking an object, and then you need another algorithm to produce an state estimate from those points. None of this is intractable, but it is all quite computationally expensive. \n",
"\n",
"\n",
"Finally, we have a new algorithm called the *unscented Kalman filter* (UKF) that does not require you to find analytic solutions to nonlinear equations, and yet almost always performs better than the EKF. It does especially well with highly nonlinear problems - problems where the EKF has significant difficulties. Designing the filter is extremely easy. Some will say the jury is still out on the UKF, but to my mind the UKF is superior in almost every way to the EKF. I suggest that the UKF should be the starting point for any implementation, especially if you are not a Kalman filter professional with a graduate degree in control theory. The main downside is that the UKF can be a few times slower than the EKF, but this really depends on whether the EKF solves the Jacobian analytically or numerically. If numerically the UKF is almost certainly faster. It has not been proven (and probably it cannot be proven) that the UKF always yields more accurate results than the EKF. In practice it almost always does, often significantly so. It is very easy to understand and implement, and I strongly suggest this filter as your starting point. "
"Finally, we have a new algorithm called the *unscented Kalman filter* (UKF). It does not require you to find analytic solutions to nonlinear equations, and yet almost always performs better than the EKF. It does well with nonlinear problems - problems where the EKF has significant difficulties. Designing the filter is extremely easy. Some will say the jury is still out on the UKF, but to my mind the UKF is superior in almost every way to the EKF. I suggest that the UKF should be the starting point for any implementation, especially if you are not a Kalman filter professional with a graduate degree in control theory. The main downside is that the UKF can be a few times slower than the EKF, but this really depends on whether the EKF solves the Jacobian analytically or numerically. If numerically the UKF is almost certainly faster. It has not been proven (and probably it cannot be proven) that the UKF always yields more accurate results than the EKF. In practice it almost always does, often significantly so. It is very easy to understand and implement, and I strongly suggest this filter as your starting point. "
]
},
{

411
10-Unscented-Kalman-Filter.ipynb
View File

File diff suppressed because one or more lines are too long

183
11-Extended-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

2
README.md
View File

@ -6,7 +6,7 @@ Introductory text for Kalman and Bayesian filters. All code is written in Python
**"Kalman and Bayesian Filters in Python" looks amazing! ... your book is just what I needed** - Allen Downey, Professor and O'Reilly author of several math and programming textbooks, via twitter.
Start reading online now:
Start reading online now by clicking the binder badge below:
[![Binder](http://mybinder.org/badge.svg)](http://mybinder.org/repo/rlabbe/Kalman-and-Bayesian-Filters-in-Python)

4
code/custom.css
View File

@ -184,19 +184,15 @@
margin-top: 0.2em;
}
a:link{
font-weight: bold;
color:#447adb;
}
a:visited{
font-weight: bold;
color: #1d3b84;
}
a:hover{
font-weight: bold;
color: #1d3b84;
}
a:focus{
font-weight: bold;
color:#447adb;
}
a:active{

2
code/kf_internal.py
View File

@ -36,7 +36,6 @@ def plot_dog_track(xs, dog, measurement_var, process_var):
def print_gh(predict, update, z):
print(predict, update, z)
predict_template = '{: 7.3f} {: 8.3f}'
update_template = '{:.3f}\t{: 7.3f} {: 7.3f}'
@ -45,7 +44,6 @@ def print_gh(predict, update, z):
def print_variance(positions):
print('Variance:')
for i in range(0, len(positions), 5):
print('\t{:.4f} {:.4f} {:.4f} {:.4f} {:.4f}'.format(
*[v[1] for v in positions[i:i+5]]))

5
code/ukf_internal.py
View File

@ -258,7 +258,7 @@ def plot_sigma_points():
plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4])
plt.title('alpha=1')
plt.show()
print(sum(Wc0))
def plot_radar(xs, t, plot_x=True, plot_vel=True, plot_alt=True):
xs = np.asarray(xs)
@ -267,16 +267,19 @@ def plot_radar(xs, t, plot_x=True, plot_vel=True, plot_alt=True):
plt.plot(t, xs[:, 0]/1000.)
plt.xlabel('time(sec)')
plt.ylabel('position(km)')
plt.tight_layout()
if plot_vel:
plt.figure()
plt.plot(t, xs[:, 1])
plt.xlabel('time(sec)')
plt.ylabel('velocity')
plt.tight_layout()
if plot_alt:
plt.figure()
plt.plot(t, xs[:,2])
plt.xlabel('time(sec)')
plt.ylabel('altitude')
plt.tight_layout()
plt.show()