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

Started altering to use filterpy project.

Browse Source 
Not fully tested, but the multidimensional chapter is working.
This commit is contained in:
Roger Labbe 2014-07-14 21:46:59 -07:00
parent 212a1e1a85
commit 79301c9224
11 changed files with 289 additions and 721 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
Designing_Nonlinear_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

131
KalmanFilter.py
View File

@ -1,131 +0,0 @@
# -*- coding: utf-8 -*-
"""
Created on Fri Oct 18 18:02:07 2013
@author: rlabbe
"""
import numpy as np
import scipy.linalg as linalg
import matplotlib.pyplot as plt
import numpy.random as random
class KalmanFilter:
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_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(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):
"""
Add a new measurement to the kalman filter.
"""
# 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
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.B * self.u
self.P = self.F * self.P * self.F.T + self.Q
if __name__ == "__main__":
f = KalmanFilter (dim_x=2, dim_z=2)
f.x = np.matrix([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.matrix([[1.,1.],
[0.,1.]]) # state transition matrix
f.H = np.matrix([[1.,0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q *= 0.0001 # process uncertainty
measurements = []
results = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
# perform kalman filtering
f.measure (z)
f.predict()
# save data
results.append (f.x[0,0])
measurements.append(z)
# plot data
p1, = plt.plot(measurements,'r')
p2, = plt.plot (results,'b')
p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
plt.legend([p1,p2, p3], ["noisy measurement", "KF output", "ideal"], 4)
plt.show()

136
KalmanFilter1D.py
View File

@ -1,136 +0,0 @@
# -*- coding: utf-8 -*-
"""
Created on Thu May 1 19:48:54 2014
@author: rlabbe
"""
import math
import matplotlib.pyplot as plt
import noise
import numba
import numpy.random as random
class KalmanFilter1D(object):
def __init__ (self, x0, var):
self.mean = x0
self.variance = var
def estimate(self, z, z_variance):
self.mean = (self.variance*z + z_variance*self.mean) / (self.variance + z_variance)
self.variance = 1. / (1./self.variance + 1./z_variance)
def project(self, u, u_variance):
self.mean += u
self.variance += u_variance
def _fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
mean = 0
sig = 1000
measurements = [x for x in range(100)]
f = KalmanFilter1D (mean,sig)
ys = []
errs = []
xs = []
for i in range(len(measurements)):
r = noise.white_noise (noise_factor)
z = measurements[i] + r
f.estimate (z, measurement_error)
xs.append(z)
ys.append(f.mean)
errs.append (f.variance)
f.project (1, 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.
mean = 0
sig = 1000
measurements = [x for x in range(100)]
f = KalmanFilter1D (mean,sig)
ys = []
errs = []
xs = []
for i in range(len(measurements)):
r = random.randn() * noise_factor
m = measurements[i] + r
f.estimate (m, abs(r*10))
xs.append(m)
ys.append(f.mean)
errs.append (f.variance)
f.project (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 _test_sin ():
sensor_error = 1
movement = .1
movement_error = .1
pos = (1,500)
zs = []
ps = []
filter_ = KalmanFilter1D(pos[0],pos[1])
m_1 = filter_.mean
for i in range(300):
filter_.project(movement, movement_error)
Z = math.sin(i/12.) + math.sqrt(abs(noise.white_noise(.02)))
movement = filter_.mean - m_1
m_1 = filter_.mean
zs.append(Z)
filter_.estimate (Z, sensor_error)
ps.append(filter_.mean)
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 0:
# use same seed to get repeatable results
random.seed(10)
if 1:
plt.figure()
_varying_error_kf ()
if 1:
plt.figure()
_fixed_error_kf(measurement_error=1.,
motion_error=.1,
noise_factor=50)
if 1:
plt.figure()
_test_sin()

247
Multidimensional_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

99
UKF.py
View File

@ -1,99 +0,0 @@
# -*- coding: utf-8 -*-
"""
Created on Wed May 28 17:40:19 2014
@author: rlabbe
"""
from numpy import matrix, zeros, asmatrix, size
from numpy.linalg import cholesky
def sigma_points (mean, covariance, kappa):
""" Computes the sigma points and weights for an unscented Kalman filter.
xm are the means, and P is the covariance. kappa is an arbitrary constant
constant. Returns tuple of the sigma points and weights.
This is the original algorithm as published by Julier and Uhlmann.
Later algorithms introduce more parameters - alpha, beta,
Works with both scalar and array inputs:
sigma_points (5, 9, 2) # mean 5, covariance 9
sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
"""
mean = asmatrix(mean)
covariance = asmatrix(covariance)
n = size(mu)
# initialize to zero
Xi = asmatrix (zeros((n,2*n+1)))
W = asmatrix (zeros(2*n+1))
# all weights are 1/ 2(n+kappa)) except the first one.
W[0,1:] = 1. / (2*(n+kappa))
W[0,0] = float(kappa) / (n + kappa)
# use cholesky to find matrix square root of (n+kappa)*cov
# U'*U = (n+kappa)*P
U = asmatrix (cholesky((n+kappa)*covariance))
# mean is in location 0.
Xi[:,0] = mean
for col in range (n):
Xi[:, col+1] = mean + U[:, col]
for k in range (n):
Xi[:, n+col+1] = mean - U[:, col]
return (Xi, W)
def unscented_transform (Xi, W, NoiseCov=None):
""" computes the unscented transform of a set of sigma points 'X'
and weights 'W'.
W should be in the form:
[w0, w1, w2,...wn].T
where w0 is the mean,
Xi should be in the form:
[X_00,
returns the mean and covariance in a tuple '(mean, cov)'
"""
W = asmatrix(W)
Xi = asmatrix(Xi)
n, kmax = Xi.shape
# initialize results to 0
mean = matrix (zeros((n,1)))
cov = matrix (zeros((n,n)))
for k in range (kmax):
mean += W[0,k] * Xi[:,k]
for k in range (kmax):
cov += W[0,k]*(Xi[:,k]-mu) * (Xi[:,k]-mu).T
return (mean, cov)
if __name__ == "__main__":
xi = matrix ([[1,2,3,4,5,6,7],[1,2,3,4,5,6,7],[1,2,3,4,5,6,7]])
mu = matrix ([1,2,3,4,5,6,7])
m,c = unscented_transform(xi, mu)
print m
print c

0
__init__.py Normal file
View File

54
discrete_bayes.ipynb
View File

File diff suppressed because one or more lines are too long

131
exp/KalmanFilter.py
View File

@ -1,131 +0,0 @@
# -*- coding: utf-8 -*-
"""
Created on Fri Oct 18 18:02:07 2013
@author: rlabbe
"""
import numpy as np
import scipy.linalg as linalg
import matplotlib.pyplot as plt
import numpy.random as random
class KalmanFilter:
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_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(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):
"""
Add a new measurement to the kalman filter.
"""
# 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
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.B * self.u
self.P = self.F * self.P * self.F.T + self.Q
if __name__ == "__main__":
f = KalmanFilter (dim_x=2, dim_z=2)
f.x = np.matrix([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.matrix([[1.,1.],
[0.,1.]]) # state transition matrix
f.H = np.matrix([[1.,0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q *= 0.0001 # process uncertainty
measurements = []
results = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
# perform kalman filtering
f.measure (z)
f.predict()
# save data
results.append (f.x[0,0])
measurements.append(z)
# plot data
p1, = plt.plot(measurements,'r')
p2, = plt.plot (results,'b')
p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
plt.legend([p1,p2, p3], ["noisy measurement", "KF output", "ideal"], 4)
plt.show()

108
exp/dme.py Normal file
View File

@ -0,0 +1,108 @@
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
from KalmanFilter import *
from math import cos, sin, sqrt, atan2
def H_of (pos, pos_A, pos_B):
""" Given the position of our object at 'pos' in 2D, and two transmitters
A and B at positions 'pos_A' and 'pos_B', return the partial derivative
of H
"""
theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])
theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])
if False:
return np.mat([[0, -cos(theta_a), 0, -sin(theta_a)],
[0, -cos(theta_b), 0, -sin(theta_b)]])
else:
return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
[-cos(theta_b), 0, -sin(theta_b), 0]])
class DMESensor(object):
def __init__(self, pos_a, pos_b, noise_factor=1.0):
self.A = pos_a
self.B = pos_b
self.noise_factor = noise_factor
def range_of (self, pos):
""" returns tuple containing noisy range data to A and B
given a position 'pos'
"""
ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0,0]
for i in range(count):
# move (1,1) each step, so just use i
pos = [i,i]
# compute the difference in range between the nominal track and measured
# ranges
ra,rb = d.range_of(pos)
rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
print ra, rb
print rx,ry
z = np.mat([[ra-rx],[rb-ry]])
print z.T
# compute linearized H for this time step
f1.H = H_of (pos, pos_a, pos_b)
# store stuff so we can plot it later
xs.append (f1.x[0,0]+i)
ys.append (f1.x[2,0]+i)
pxs.append (pos[0])
pys.append(pos[1])
# perform the Kalman filter steps
f1.predict ()
f1.update(z)
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.legend([p1,p2], ['filter', 'ideal'], 2)
plt.show()

9
exp/temp.py Normal file
View File

@ -0,0 +1,9 @@
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
import KalmanFilter

18
g-h_filter.ipynb
View File

@ -1414,7 +1414,12 @@
"\n",
"I assumed that my weight gain was constant at 1 lb/day, and so when I tried to fit a polynomial of $n=1$, which is a line, the result very closely matched the actual weight gain. But, of course, no one consistantly only gains or loses weight. We flucuate. Using 'polyfit()' for a longer series of data would yield poor results. In contrast, the g-h filter reacts to changes in the rate - the $h$ term controls how quickly the filter reacts to these changes. If we gain weight, hold steady for awhile, then lose weight, the filter will track that change automatically. 'polyfit()' would not be able to do that unless the gain and loss could be well represented by a polynomial.\n",
"\n",
"Another advantage of this form of filter, even if the data fits a *n*-degree polynomial, is that it is *recursive*. That is, we can compute the estimate for this time period knowing nothing more than the estimate and rate from the last time period. In contrast, if you dig into the implementation for `polyfit()` you will see that it needs all of the data before it can produce an answer. Therefore algorithms like `polyfit()` are not well suited for real-time data filtering. In the 60's when the Kalman filter was developed computers were very slow and had extremely limited memory. They were utterly unable to store, for example, thousands of readings from an aircraft's inertial navigation system, nor could they process all of that data in the short period of time needed to provide accurate and up-to-date navigation information. The Kalman filter only needs to store the last estimate and a few related parameters, and requires only a relatively small number of computations to generate the next estimate. Today we have so much memory and processing power that this advantage is somewhat less important, but at the time the Kalman filter was a major breakthrough not just because of the mathematical properties, but because it could (barely) run on the hardware of the day. \n",
"Another advantage of this form of filter, even if the data fits a *n*-degree polynomial, is that it is *recursive*. That is, we can compute the estimate for this time period knowing nothing more than the estimate and rate from the last time period. In contrast, if you dig into the implementation for `polyfit()` you will see that it needs all of the data before it can produce an answer. Therefore algorithms like `polyfit()` are not well suited for real-time data filtering. In the 60's when the Kalman filter was developed computers were very slow and had extremely limited memory. They were utterly unable to store, for example, thousands of readings from an aircraft's inertial navigation system, nor could they process all of that data in the short period of time needed to provide accurate and up-to-date navigation information. \n",
"\n",
"\n",
"Up until the mid 20th century various forms of Least Squares Estimation was used for this type of filtering. For example, for NASA's Apollo program had a ground network for tracking the Command and Service Model (CSM) and the Lunar Module (LM). They took measurements over many minutes, batched the data together, and slowly computed an answer. In 1960 Stanley Schmidt at NASA Ames recognized the utility of Rudolf Kalman's seminal paper and invited him to Ames. Schmidt applied Kalman's work to the onboard navigation sytems on the CSM and LM, and called it the \"Kalman filter\".[1] Soon after, the world moved to this faster, recursive filter.\n",
"\n",
"The Kalman filter only needs to store the last estimate and a few related parameters, and requires only a relatively small number of computations to generate the next estimate. Today we have so much memory and processing power that this advantage is somewhat less important, but at the time the Kalman filter was a major breakthrough not just because of the mathematical properties, but because it could (barely) run on the hardware of the day. \n",
"\n",
"This subject is much deeper than this short discussion suggests. We will consider these topics many more times throughout the book."
]
@ -1443,12 +1448,19 @@
"There is another feature of these filters we have barely touched upon - Bayesian statistics. You will note that the term 'Bayesian' is in the title of this book; this is not a coincidence! For the time being we will leave $g$ and $h$ behind, largely unexplored, and develop a very powerful form of probabilistic reasoning about filtering. Yet suddenly this same g-h filter algorithm will appear, this time with a formal mathematical edifice that allows us to create filters from multiple sensors, to accurately estimate the amount of error in our solution, and to control robots."
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"References"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"missing: properties - recursive - markov - random variable\n",
"should we fit bayes into here, or just leave to the next chapter"
"[1] http://nescacademy.nasa.gov/review/downloadfile.php?file=FundamentalsofKalmanFiltering_Presentation.pdf&id=199&distr=Public"
]
}
],