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

A lot of changes. I think a lot of changes to the notebooks are just

Browse Source 
due to running them. In the process of adding a bunch of new .py
scripts to support the book. Most important is the DiscreteBayes1D.py
class, which implements a Discrete Bayesian filter in a generalized way.
This commit is contained in:
Roger Labbe 2014-05-02 21:49:35 -05:00
parent 20bd353b8a
commit 8305200ff0
10 changed files with 533 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
DiscreteBayes1D.py Normal file
View File

@ -0,0 +1,149 @@
# -*- coding: utf-8 -*-
"""
Created on Fri May 2 11:48:55 2014
@author: rlabbe
"""
from __future__ import division
from __future__ import print_function
import copy
import numpy as np
import bar_plot
import numpy.random as random
import matplotlib.pyplot as plt
''' should this be a class? seems like both sense and update are very
problem specific
'''
class DiscreteBayes1D(object):
def __init__(self, world_map, belief=None):
self.world_map = copy.deepcopy(world_map)
self.N = len(world_map)
if belief is None:
# create belief, make all equally likely
self.belief = np.empty(self.N)
self.belief.fill (1./self.N)
else:
self.belief = copy.deepcopy(belief)
# This will be used to temporarily store calculations of the new
# belief. 'k' just means 'this iteration'.
self.belief_k = np.empty(self.N)
assert self.belief.shape == self.world_map.shape
def _normalize (self):
s = sum(self.belief)
self.belief = self.belief/s
def sense(self, Z, pHit, pMiss):
for i in range (self.N):
hit = (self.world_map[i] ==Z)
self.belief_k[i] = self.belief[i] * (pHit*hit + pMiss*(1-hit))
# copy results to the belief vector using swap-copy idiom
self.belief, self.belief_k = self.belief_k, self.belief
self._normalize()
def update(self, U, kernel):
N = self.N
kN = len(kernel)
width = int((kN - 1) / 2)
self.belief_k.fill(0)
for i in range(N):
for k in range (kN):
index = (i + (width-k)-U) % N
#print(i,k,index)
self.belief_k[i] += self.belief[index] * kernel[k]
# copy results to the belief vector using swap-copy idiom
self.belief, self.belief_k = self.belief_k, self.belief
def add_noise (Z, count):
n= len(Z)
for i in range(count):
j = random.randint(0,n)
Z[j] = random.randint(0,2)
def animate_three_doors (loops=5):
world = np.array([1,0,1,0,0,0,0,1,0,0,0,1,0,0,0,0,0])
#world = np.array([1,1,1,1,1])
#world = np.array([1,0,1,0,1,0])
f = DiscreteBayes1D(world)
measurements = np.tile(world,loops)
add_noise(measurements, 4)
for m in measurements:
f.sense (m, .8, .2)
f.update(1, (.05, .9, .05))
bar_plot.plot(f.belief)
plt.pause(0.01)
def _test_filter():
def is_near_equal(a,b):
try:
assert sum(abs(a-b)) < 0.001
except:
print(a, b)
assert False
def test_update_1():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (1, [1])
is_near_equal (f.belief, np.array([0,0,0,.8,0]))
f.update (1, [1])
is_near_equal (f.belief, np.array([0,0,0,0,.8]))
f.update (1, [1])
is_near_equal (f.belief, np.array([.8,0,0,0,0]))
f.update (-1, [1])
is_near_equal (f.belief, np.array([0,0,0,0,.8]))
f.update (2, [1])
is_near_equal (f.belief, np.array([0,.8,0,0,0]))
f.update (5, [1])
is_near_equal (f.belief, np.array([0,.8,0,0,0]))
def test_undershoot():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (2, [.2, .8,0.])
is_near_equal (f.belief, np.array([0,0,0,.16,.64]))
def test_overshoot():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (2, [0, .8, .2])
is_near_equal (f.belief, np.array([.16,0,0,0,.64]))
test_update_1()
test_undershoot()
if __name__ == "__main__":
_test_filter()
#animate_three_doors(loops=1)

28
Introduction.ipynb
View File

@ -15,14 +15,34 @@
"\n",
"The Kalman filter was introduced to the world via papers published in 1958 and 1960 by Rudolph E Kalman. This work built on work by Nobert Wiener. Kalman's early papers were extremely abstract, but researchers quickly realized that the papers described a very practical technique to filter noisy data. From then until now it has been an ongoing topic of research, and there are many books and papers devoted not only to the basics, but many specializations and extensions to the technique. If you are reading this, you have likely come across some of them.\n",
"\n",
"If you are like me, you probably find them nearly impenetrable. I find that almost all start with very abstract math, assume familiarity with notation and naming conventions that I haven't seen before, and focus heavily on proof rather than exposition and teaching. This is perhaps understandable, but it is a regrettable situation. \n",
"If you are like me, you probably find them nearly impenetrable. I find that almost all start with very abstract math, assume familiarity with notation and naming conventions that I haven't seen before, and focus heavily on proof rather than exposition and teaching. This is perhaps understandable, but it is a regrettable situation and not necessary. \n",
"\n",
"After struggling through this material for some time, things finally began making sense. A majority of my 'aha' moments were due to implementing and experimenting with various simple filters. What to make of an equation like $K_k=P_{k}^{-}H^{T}_{k}[H_k P^{-}_{k} H^{T}_{k} + R_k]^{-1}$ is initially puzzing. This is especially true when it pops out as the result of two to three pages of linear algrebra, and each variable is given only a very abstract, mathematically rigorous definition. One book I have doesn't bother to define $R_k$ despite using it throuout its 4 pages of derivation of K. If instead I tell you that K is just a scaling factor for choosing how much of a measurement and how much of a prediction to use in the filter, what K is becomes obvious (although perhaps the computation is still a bit mysterious). After implementing a few Kalman filters for toy problems and varying the values of various matrices and constants I developed both an intuitive and fairly deep understanding of how Kalman filters work. This knowledge is indispensible; it is trivial to code the handful of linear equations for a Kalman filter, and they never change. I do mean a handful - you can implement the simplest Kalman filter in 10 lines of code. \n",
" "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"** this needs a lot of editting. bored with it for now.**"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"While you do need to know some basic probability and some very basic linear algebra to understand and implement Kalman filters, by and large you really do not need to understand the complicated, multi-page derivations. The end result of all the math is a small handful of equations which you can use to perform very sophisticated filtering, smoothing, and tracking. Implementing the basic equations is never difficult. Kalman filter design is much more an art than a science - implementers spend a few minutes writing the basic equations, and then a lot of time tuning the filter for their specific problem. \n",
"\n",
"I compare this to a student learning the equation for the volume of a sphere: $V(r) = \\frac{4}{3}\\pi r^3$ A student can use this equation without even understanding the functional notation $V(r)$, and certainly they do not need to be able to derive the equation via calculus. Eventually, in some domains, knowledge of the calculus will become useful, but an enormous amount of work can be done by only knowing the equation and how to apply it. Also, it is often useful to understand facts about a sphere, such as it the shape that encloses the greatest volume with the smallest surface area. You do not have to know how to prove that to make use of that information to explain the reason for why bubbles are round, or to economically fence an area for livestock. \n",
"\n",
"I argue the same is largely true with Kalman filters. In this book I will not prove the Kalman filter equations are correct, nor will I derive them. I will instead strive to build in you an physical, intuitive understanding of what the equations mean, and how they interact. Of course, if you are trying to navigate a spaceship to Mars you will require a more sophisticated understanding than I provide, and you will not view this as a useful resource. But the rest of people, who perhaps wants to track heads in video, or control a little hobby robot, or smooth some data, I think this approach should provide a lot of insight into how to create Kalman filters, and provide enough background that the standard texts are now approachable.\n",
"\n",
"I argue the same is largely true with Kalman filters. In this book I will not prove the Kalman filter equations are correct, nor will I derive them. I will instead strive to build in you an physical, intuitive understanding of what the equations mean, and how they interact. Of course, if you are trying to navigate a spaceship to Mars you will require a more sophisticated understanding than I provide, and you will not view this as a useful resource. But the rest of people, who perhaps wants to track heads in video, or control a little hobby robot, or smooth some data, I think this approach should provide a lot of insight into how to create Kalman filters, and provide enough background that the standard texts are now approachable."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"#### Prerequisites\n",
"\n",
"While Kalman filters are used in many domains, they were initially created to solve problems with missle tracking and navigation. Most of my examples will draw from physical examples such as these. However, I do this not just from a sense of history, but because I believe that helps the student form strong pysical intuitions about what the filter is doing. For example, we will learn how the Kalman filter creates and uses something called \"hidden variables\". This is normally presented in a highly abstract manner. However, it is actually quite simple. Suppose I know your position at several points in time. From that I can calculate your velocity even though I don't have any sensor that directly measures your velocity. In a Kalman filter, if I have a position sensor, the filter will generate the hidden, or unobserved variable velocity. A lot of seemingly arcane terminology suddenly becomes concrete and clear. So you should have taken a basic physics course and understand equations like $d = \\frac{a}{2} t^2 + v_0 t + d_0$\n",
@ -37,7 +57,7 @@
"\n",
"Since this is a book on doing Kalman filtering with Python I will expect that you know Python; I do not attempt to teach it. With that said, it is a very easy language to learn, and it reads much like pseudocode. If you are more familiar with another language I do not think you will have any major difficulties reading the source code. I purposefully restrict the code to the more basic features of Python to accomodate people with varying skill levels. If you use this code in your own work, feel free to use more advanced Python facilities if it strikes your fancy.\n",
"\n",
"So that is a fair amount of prerequisites, but I think you will already have most of them if you are seriously trying to solve a problem where Kalman filters are useful. \n"
"So that is a fair amount of prerequisites, but I think you will already have most of them if you are seriously trying to solve a problem where Kalman filters are useful. "
]
},
{

226
Kalman Filters.ipynb
View File

File diff suppressed because one or more lines are too long

129
KalmanFilter1D.py Normal file
View File

@ -0,0 +1,129 @@
# -*- coding: utf-8 -*-
"""
Created on Thu May 1 19:48:54 2014
@author: rlabbe
"""
from __future__ import division
import gauss as g;
import math
import matplotlib.pyplot as plt
import noise
import numpy.random as random
class KalmanFilter1D(object):
def __init__ (self, est_0=g.gaussian(0,1000)):
self.estimate = est_0
def update_estimate(self, Z):
self.estimate = self.estimate * Z
def project_ahead(self, U):
self.estimate = self.estimate + U
def fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
mu = 0
sig = 1000
measurements = [x+5 for x in range(100)]
f = KalmanFilter1D (g.gaussian(mu,sig))
ys = []
errs = []
xs = []
for i in range(len(measurements)):
r = noise.white_noise (noise_factor)
m = measurements[i] + r
f.update_estimate (g.gaussian(m, measurement_error))
xs.append(m)
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
f.project_ahead (g.gaussian(20, motion_error))
plt.clf()
p1, = plt.plot (measurements, 'r')
p2, = plt.plot (xs,'g')
p3, = plt.plot (ys, 'b')
plt.legend ([p1,p2,p3],['actual', 'measurement', 'filter'], 2)
#plt.errorbar (x=range(len(ys)), color='b', y=ys, yerr=errs)
plt.show()
def varying_error_kf(noise_factor=1.0):
motion_sig = 2.
mu = 0
sig = 1000
f = KF1D (mu,sig)
ys = []
us = []
errs = []
xs = []
for i in range(len(measurements)):
r = random.randn() * noise_factor
m = measurements[i] + r
print (r)
f.update (m, abs(r*10))
xs.append(m)
#print ("measure:" + str(f.estimate))
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
f.predict (1.0, motion_sig)
#print ("predict:" + str(f.estimate))
plt.clf()
plt.plot (measurements, 'r')
plt.plot (xs,'g')
plt.errorbar (x=range(len(ys)), color='b', y=ys, yerr=errs)
plt.show()
def _test_foo ():
sensor_error = 1
movement = .1
movement_error = .1
pos = g.gaussian(1,500)
zs = []
ps = []
filter_ = KalmanFilter1D(pos)
m_1 = filter_.estimate.mu
for i in range(300):
filter_.update_estimate(g.gaussian(movement, movement_error))
Z = math.sin(i/12.) + math.sqrt(abs(noise.white_noise(.02)))
movement = filter_.estimate.mu - m_1
m_1 = filter_.estimate.mu
print movement, filter_.estimate.sigma
zs.append(Z)
filter_.project_ahead (g.gaussian(Z, sensor_error))
ps.append(filter_.estimate[0])
p1, = plt.plot(zs,c='r', linestyle='dashed')
p2, = plt.plot(ps, c='b')
plt.legend([p1,p2], ['measurement', 'filter'], 2)
plt.show()
if __name__ == "__main__":
if 1:
# use same seed to get repeatable results
random.seed(10)
fixed_error_kf(measurement_error=100.,
motion_error=.1,
noise_factor=50)
#_test_foo()

72
Multidimensional Kalman Filters.ipynb
View File

File diff suppressed because one or more lines are too long

16
bar_plot.py Normal file
View File

@ -0,0 +1,16 @@
# -*- coding: utf-8 -*-
"""
Created on Fri May 2 12:21:40 2014
@author: rlabbe
"""
import matplotlib.pyplot as plt
import numpy as np
def plot(pos):
plt.cla()
ax = plt.gca()
x = np.arange(len(pos))
ax.bar(x, pos)
plt.ylim([0,1])
plt.xticks(x+0.5, x)

61
gauss.py
View File

@ -50,68 +50,9 @@ class KF1D(object):
self.estimate = self.estimate + gaussian (U,var)
measurements = [x+5 for x in range(100)]
def fixed_error_kf(measurement_error, noise_factor = 1.0):
motion_sig = 2.
mu = 0
sig = 1000
f = KF1D (mu,sig)
ys = []
errs = []
xs = []
for i in range(len(measurements)):
r = random.randn() * noise_factor
m = measurements[i] + r
f.update (m, measurement_error)
xs.append(m)
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
f.predict (1.0, motion_sig)
plt.clf()
plt.plot (measurements, 'r')
plt.plot (xs,'g')
plt.errorbar (x=range(len(ys)), color='b', y=ys, yerr=errs)
plt.show()
def varying_error_kf(noise_factor=1.0):
motion_sig = 2.
mu = 0
sig = 1000
f = KF1D (mu,sig)
ys = []
us = []
errs = []
xs = []
for i in range(len(measurements)):
r = random.randn() * noise_factor
m = measurements[i] + r
print (r)
f.update (m, abs(r*10))
xs.append(m)
#print ("measure:" + str(f.estimate))
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
f.predict (1.0, motion_sig)
#print ("predict:" + str(f.estimate))
plt.clf()
plt.plot (measurements, 'r')
plt.plot (xs,'g')
plt.errorbar (x=range(len(ys)), color='b', y=ys, yerr=errs)
plt.show()
varying_error_kf( noise_factor=100)
#varying_error_kf( noise_factor=100)

2
gaussian.py
View File

@ -96,8 +96,6 @@ def plot_sigma_ellipse(ellipse,title=None):
def plot_sigma_ellipses(ellipses,title=None,axis_equal=True,x_lim=None,y_lim=None):
""" plots the ellipse produced from sigma_ellipse."""
isct = plt.Circle((2,2),1,color='b',fill=False)
plt.figure().gca().add_patch(isct)
if x_lim is not None:
axis_equal = False

27
mkf_internal.py Normal file
View File

@ -0,0 +1,27 @@
# -*- coding: utf-8 -*-
"""
Created on Thu May 1 16:56:49 2014
@author: rlabbe
"""
def show_residual_chart():
xlim([0.9,2.5]);ylim([0.5,2.5])
scatter ([1,2,2],[1,2,1.3])
scatter ([2],[1.8],marker='o')
ax = plt.axes()
ax.annotate('', xy=(2,2), xytext=(1,1),
arrowprops=dict(arrowstyle='->', ec='b',shrinkA=3, shrinkB=4))
ax.annotate('prediction', xy=(1.7,2), color='b')
ax.annotate('measurement', xy=(2.05, 1.28))
ax.annotate('prior measurement', xy=(1, 0.9))
ax.annotate('residual', xy=(2.04,1.6), color='r')
ax.annotate('new estimate', xy=(2,1.8),xytext=(2.15,1.9),
arrowprops=dict(arrowstyle='->', shrinkA=3, shrinkB=4))
ax.annotate('', xy=(2,2), xytext=(2,1.3),
arrowprops=dict(arrowstyle="<->",
ec="r",
shrinkA=5, shrinkB=5))
title("Kalman Filter Prediction Update Step")
show()

13
noise.py Normal file
View File

@ -0,0 +1,13 @@
# -*- coding: utf-8 -*-
"""
Created on Fri May 2 10:28:27 2014
@author: rlabbe
"""
import numpy.random
def white_noise (sigma2=1.):
return sigma2 * numpy.random.randn()
if __name__ == "__main__":
assert white_noise(.0) == 0.