A lot of work getting baseball tracking working. Added long form for P in KF.

This commit is contained in:
Roger Labbe 2014-06-05 09:21:15 -07:00
parent e1ce21b25c
commit ee826dbce5
6 changed files with 1185 additions and 130 deletions

File diff suppressed because one or more lines are too long

View File

@ -12,8 +12,21 @@ import numpy.random as random
class KalmanFilter:
def __init__(self, dim):
""" Create a Kalman filter of dimension 'dim'"""
def __init__(self, dim, 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
@ -24,6 +37,9 @@ class KalmanFilter:
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))
if use_short_form:
self.update = self.update_short_form
def update(self, Z):
@ -39,11 +55,41 @@ 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

View File

@ -1,14 +1,7 @@
# -*- coding: utf-8 -*-
"""
Spyder Editor
This temporary script file is located here:
/home/rlabbe/.spyder2/.temp.py
"""
"""
Computes the trajectory of a stitched baseball with air drag.
Takes altitude into account (higher altitude means further travel) and the
stitching on the baseball influencing drag.
@ -20,9 +13,11 @@ LOWER the higher its velocity, which is somewhat counterintuitive.
"""
from __future__ import division
import math
import matplotlib.pyplot as plt
from numpy.random import randn
import numpy as np
def a_drag (vel, altitude):
""" returns the drag coefficient of a baseball at a given velocity (m/s)
@ -38,13 +33,45 @@ def a_drag (vel, altitude):
return val
def compute_trajectory_vacuum (v_0_mph,
theta,
dt=0.01,
noise_scale=0.0,
x0=0., y0 = 0.):
theta = math.radians(theta)
x = x0
y = y0
v0_y = v_0_mph * math.sin(theta)
v0_x = v_0_mph * math.cos(theta)
xs = []
ys = []
t = dt
while y >= 0:
x = v0_x*t + x0
y = -0.5*9.8*t**2 + v0_y*t + y0
xs.append (x + randn() * noise_scale)
ys.append (y + randn() * noise_scale)
t += dt
return (xs, ys)
def compute_trajectory(v_0_mph, theta, v_wind_mph=0, alt_ft = 0.0, dt = 0.01):
g = 9.8
### comput
theta = math.radians(theta)
# initial velocity in direction of travel
v_0 = v_0_mph * 0.447 # mph to m/s
# velocity components in x and y
v_x = v_0 * math.cos(theta)
v_y = v_0 * math.sin(theta)
@ -58,7 +85,6 @@ def compute_trajectory(v_0_mph, theta, v_wind_mph=0, alt_ft = 0.0, dt = 0.01):
i = 0
while y[i] >= ground_level:
g = 9.8
v = math.sqrt((v_x - v_wind) **2+ v_y**2)
@ -69,7 +95,37 @@ def compute_trajectory(v_0_mph, theta, v_wind_mph=0, alt_ft = 0.0, dt = 0.01):
v_y = v_y - a_drag(v, altitude) * v * v_y * dt - g * dt
i += 1
# meters to yards
np.multiply (x, 1.09361)
np.multiply (y, 1.09361)
return (x,y)
def predict (x0, y0, x1, y1, dt, time):
g = 10.724*dt*dt
v_x = x1-x0
v_y = y1-y0
v = math.sqrt(v_x**2 + v_y**2)
x = x1
y = y1
while time > 0:
v_x = v_x - a_drag(v, y) * v * v_x
v_y = v_y - a_drag(v, y) * v * v_y - g
x = x + v_x
y = y + v_y
time -= dt
return (x,y)
if __name__ == "__main__":
x,y = compute_trajectory(v_0_mph = 110., theta=35., v_wind_mph = 0., alt_ft=5000.)

218
bb_test.py Normal file
View File

@ -0,0 +1,218 @@
# -*- coding: utf-8 -*-
"""
Created on Wed Jun 4 12:33:38 2014
@author: rlabbe
"""
from __future__ import print_function,division
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import baseball
from numpy.random import randn
def ball_filter6(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=6)
g = 10
f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0],
[0, 1., dt, 0, 0, 0],
[0, 0, 1., 0, 0, 0],
[0, 0, 0., 1., dt, -0.5*dt*dt*g],
[0, 0, 0, 0, 1., -g*dt],
[0, 0, 0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,1,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0]])
f1.R = np.mat(np.eye(6)) * R
f1.Q = np.zeros((6,6))
f1.Q[2,2] = Q
f1.Q[5,5] = Q
f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
f1.P = np.eye(6) * 50.
f1.B = 0.
f1.u = 0
return f1
def ball_filter4(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat ([[1., dt, 0, 0,],
[0, 1., 0, 0],
[0, 0, 1., dt],
[0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0],
[0,0,0,0],
[0,0,1,0],
[0,0,0,0]])
f1.B = np.mat([[0,0,0,0],
[0,0,0,0],
[0,0,1.,0],
[0,0,0,1.]])
f1.u = np.mat([[0],
[0],
[-0.5*g*dt**2],
[-g*dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4,4))
f1.Q[1,1] = Q
f1.Q[3,3] = Q
f1.x = np.mat([0, 0 , 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def plot_ball_filter6 (f1, zs, skip_start=-1, skip_end=-1):
xs, ys = [],[]
pxs, pys = [],[]
for i,z in enumerate(zs):
m = np.mat([z[0], 0, 0, z[1], 0, 0]).T
f1.predict ()
if i == skip_start:
x2 = xs[-2]
x1 = xs[-1]
y2 = ys[-2]
y1 = ys[-1]
if i >= skip_start and i <= skip_end:
#x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1))
x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30)
m[0] = x
m[3] = y
#print ('skip', i, f1.x[2],f1.x[5])
f1.update_long_form (m)
'''
if i >= skip_start and i <= skip_end:
#f1.x[2] = -0.1
#f1.x[5] = -17.
pass
else:
f1.update (m)
'''
xs.append (f1.x[0,0])
ys.append (f1.x[3,0])
pxs.append (z[0])
pys.append(z[1])
if i > 0 and z[1] < 0:
break;
'''
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.axis('equal')
plt.legend([p1,p2], ['filter', 'measurement'], 2)
plt.xlim([0,xs[-1]])
plt.show()
'''
def plot_ball_filter4 (f1, zs, skip_start=-1, skip_end=-1):
xs, ys = [],[]
pxs, pys = [],[]
for i,z in enumerate(zs):
m = np.mat([z[0], 0, z[1], 0]).T
f1.predict ()
if i == skip_start:
x2 = xs[-2]
x1 = xs[-1]
y2 = ys[-2]
y1 = ys[-1]
if i >= skip_start and i <= skip_end:
#x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1))
x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30)
m[0] = x
m[2] = y
f1.update_long_form (m)
'''
if i >= skip_start and i <= skip_end:
#f1.x[2] = -0.1
#f1.x[5] = -17.
pass
else:
f1.update (m)
'''
xs.append (f1.x[0,0])
ys.append (f1.x[2,0])
pxs.append (z[0])
pys.append(z[1])
if i > 0 and z[1] < 0:
break;
'''
plt.plot (xs, ys, 'r--')
plt.plot (pxs, pys)
plt.axis('equal')
plt.legend([p1,p2], ['filter', 'measurement'], 2)
plt.xlim([0,xs[-1]])
plt.show()
'''
start_skip = 20
end_skip = 60
def run_6():
dt = 1/30
noise = 0.0
f1 = ball_filter6(dt, R=.16, Q=0.1)
#plt.cla()
x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt)
znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)]
plot_ball_filter6 (f1, znoise, start_skip, end_skip)
def run_4():
dt = 1/30
noise = 0.0
f1 = ball_filter4(dt, R=.16, Q=0.1)
plt.cla()
x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt)
znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)]
plot_ball_filter4 (f1, znoise, start_skip, end_skip)
run_6()

74
secnum.js Normal file
View File

@ -0,0 +1,74 @@
console.log("Section numbering...");
function number_sections(threshold) {
var h1_number = 0;
var h2_number = 0;
if (threshold === undefined) {
threshold = 2; // does nothing so far
}
var cells = IPython.notebook.get_cells();
for (var i=0; i < cells.length; i++) {
var cell = cells[i];
if (cell.cell_type !== 'heading') continue;
var level = cell.level;
if (level > threshold) continue;
if (level === 1) {
h1_number ++;
var h1_element = cell.element.find('h1');
var h1_html = h1_element.html();
console.log("h1_html: " + h1_html);
var patt = /^[0-9]+\.\s(.*)/; // section number at start of string
var title = h1_html.match(patt); // just the title
if (title != null) {
h1_element.html(h1_number + ". " + title[1]);
}
else {
h1_element.html(h1_number + ". " + h1_html);
}
h2_number = 0;
}
if (level === 2) {
h2_number ++;
var h2_element = cell.element.find('h2');
var h2_html = h2_element.html();
console.log("h2_html: " + h2_html);
var patt = /^[0-9]+\.[0-9]+\.\s/;
var result = h2_html.match(patt);
if (result != null) {
h2_html = h2_html.replace(result, "");
}
h2_element.html(h1_number + "." + h2_number + ". " + h2_html);
}
}
}
number_sections();
// $([IPython.evnts]).on('create.Cell', number_sections);
$([IPython.events]).on('selected_cell_type_changed.Notebook', number_sections);

79
secnum.py Normal file
View File

@ -0,0 +1,79 @@
"""
IPython Notebook magic command for automatically numbering sections of a notebook "interactively"
Use:
%install_ext https://raw.github.com/dpsanders/ipython_extensions/master/section_numbering/secnum.py
once to install, and then
%load_ext secnum
%secnum
to get automatic section numbering, which is updated when any cell changes type.
(This could, of course, be more efficient.)
This is based, completely and shamelessly, on MinRK's `nbtoc` magic
<https://github.com/minrk/ipython_extensions>
Thanks also to:
- Clayton Davis, for a great explanation of HTML, CSS and JavaScript
- Brian Granger, for a crucial hint about extracting a JQuery object from a notebook cell object
- Fernando Perez, for an inspiring talk at SciPy 2013 (and, of course, for creating IPython)
"""
import io
import os
import urllib2
from IPython.display import display_html, display_javascript
here = os.path.abspath(os.path.dirname(__file__))
secnum_js = ""
def download(fname, redownload=False):
"""download a file
if redownload=False, the file will not be downloaded if it already exists.
"""
dest = os.path.join(here, fname)
if os.path.exists(dest) and not redownload:
return
url = 'https://raw.github.com/dpsanders/ipython_extensions/master/section_numbering/' + fname
print("Downloading %s to %s" % (url, dest))
filein = urllib2.urlopen(url)
fileout = open(dest, "wb")
chunk = filein.read(1024)
while chunk:
fileout.write(chunk)
chunk = filein.read(1024)
filein.close()
fileout.close()
def load_file(fname, redownload=False):
"""load global variable from a file"""
download(fname, redownload)
with io.open(os.path.join(here, fname)) as f:
globals()[fname.replace('.', '_')] = f.read()
load_file('secnum.js')
def secnum(line):
"""add a table of contents to an IPython Notebook"""
display_javascript(secnum_js, raw=True)
def update_secnum(line):
"""download the latest version of the nbtoc extension from GitHub"""
download('secnum.py', True)
download('secnum.js', True)
get_ipython().extension_manager.reload_extension("secnum")
def load_ipython_extension(ip):
ip.magics_manager.register_function(secnum)
ip.magics_manager.register_function(update_secnum)