A lot of work getting baseball tracking working. Added long form for P in KF.
This commit is contained in:
Roger Labbe
parent
e1ce21b25c
commit
ee826dbce5
File diff suppressed because one or more lines are too long
@ -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
|
||||
|
||||
|
||||
|
||||
78
baseball.py
78
baseball.py
@ -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
218
bb_test.py
Normal 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
74
secnum.js
Normal 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
79
secnum.py
Normal 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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user