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

Added Designing nonlinear filter chapter.

Browse Source 
Worked on trying to incorporate air drag, but didn't get far. Need to
study more.
This commit is contained in:
Roger Labbe 2014-07-06 21:15:16 -07:00
parent 0697383a4e
commit 60f1b5a05e
6 changed files with 994 additions and 1039 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1072
Designing_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

697
Designing_Nonlinear_Kalman_Filters.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

64
exp/KalmanFilter.py
View File

@ -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)

4
exp/RungeKutta.py
View File

@ -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):

185
exp/ball.py
View File

@ -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()

11
toc.ipynb
View File

@ -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",