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

renamed to be more readable

Browse Source
This commit is contained in:
Roger Labbe
2015-01-27 15:46:34 -08:00
parent 9db6b3f69d
commit cfd4e3485a
27 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

383
experiements/1d_kf_compare.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

415
experiements/1dposvel.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

10801
experiements/2014-03-26-000-Data.csv Normal file
View File

File diff suppressed because it is too large Load Diff

149
experiements/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)

155
experiements/RungeKutta.py Normal file
View File

@@ -0,0 +1,155 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jul 05 09:54:39 2014
@author: rlabbe
"""
from __future__ import division, print_function
import matplotlib.pyplot as plt
from scipy.integrate import ode
import math
import numpy as np
from numpy import random, radians, cos, sin
class BallTrajectory2D(object):
def __init__(self, x0, y0, velocity, theta_deg=0., g=9.8, noise=[0.0,0.0]):
theta = radians(theta_deg)
self.vx0 = velocity * cos(theta)
self.vy0 = velocity * sin(theta)
self.x0 = x0
self.y0 = y0
self.x = x
self.g = g
self.noise = noise
def position(self, t):
""" returns (x,y) tuple of ball position at time t"""
self.x = self.vx0*t + self.x0
self.y = -0.5*self.g*t**2 + self.vy0*t + self.y0
return (self.x +random.randn()*self.noise[0], self.y +random.randn()*self.noise[1])
class BallEuler(object):
def __init__(self, y=100., vel=10., omega=0):
self.x = 0.
self.y = y
omega = radians(omega)
self.vel = vel*np.cos(omega)
self.y_vel = vel*np.sin(omega)
def step (self, dt):
g = -9.8
self.x += self.vel*dt
self.y += self.y_vel*dt
self.y_vel += g*dt
#print self.x, self.y
def rk4(y, x, dx, f):
"""computes 4th order Runge-Kutta for dy/dx.
y is the initial value for y
x is the initial value for x
dx is the difference in x (e.g. the time step)
f is a callable function (y, x) that you supply to compute dy/dx for
the specified values.
"""
k1 = dx * f(y, x)
k2 = dx * f(y + 0.5*k1, x + 0.5*dx)
k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
k4 = dx * f(y + k3, x + dx)
return y + (k1 + 2*k2 + 2*k3 + k4) / 6.
def rk2 (y,x,dx,f):
"""computes the 2nd order Runge-kutta for dy/dx"""
def fx(x,t):
return fx.vel
def fy(y,t):
return fy.vel - 9.8*t
class BallRungeKutta(object):
def __init__(self, x=0, y=100., vel=10., omega = 0.0):
self.x = x
self.y = y
self.t = 0
omega = math.radians(omega)
fx.vel = math.cos(omega) * vel
fy.vel = math.sin(omega) * vel
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
print(fx.vel)
return (self.x, self.y)
def ball_scipy(y0, vel, omega, dt):
vel_y = math.sin(math.radians(omega)) * vel
def f(t,y):
return vel_y-9.8*t
solver = ode(f).set_integrator('dopri5')
solver.set_initial_value(y0)
ys = [y0]
while brk.y >= 0:
t += dt
brk.step (dt)
ys.append(solver.integrate(t))
if __name__ == "__main__":
dt = 1./30
y0 = 15.
vel = 100.
omega = 30.
vel_y = math.sin(math.radians(omega)) * vel
def f(t,y):
return vel_y-9.8*t
be = BallEuler (y=y0, vel=vel,omega=omega)
#be = BallTrajectory2D (x0=0, y0=y0, velocity=vel, theta_deg = omega)
ball_rk = BallRungeKutta (y=y0, vel=vel, omega=omega)
while be.y >= 0:
be.step (dt)
ball_rk.step(dt)
print (ball_rk.y - be.y)
'''
p1 = plt.scatter (be.x, be.y, color='red')
p2 = plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v')
plt.legend([p1,p2], ['euler', 'runge kutta'])
'''

69
experiements/Untitled0.ipynb Normal file
View File

@@ -0,0 +1,69 @@
{
"metadata": {
"name": "",
"signature": "sha256:d69b1215417e5c675abdbd0de4e82039e393b8a3be44f039c407180b720444c8"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"from IPython.display import Image\n",
"Image(url='http://python.org/images/python-logo.gif', format='gif')"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<img src=\"http://python.org/images/python-logo.gif\"/>"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 5,
"text": [
"<IPython.core.display.Image at 0x7fc664185e10>"
]
}
],
"prompt_number": 5
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"Image(url='https://raw.githubusercontent.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/basic_animation.gif', format='gif')\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<img src=\"https://raw.githubusercontent.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/basic_animation.gif\"/>"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 15,
"text": [
"<IPython.core.display.Image at 0x7fc6640f4390>"
]
}
],
"prompt_number": 15
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<img src=\"/home/rlabbe/Downloads/basic_animation\">"
]
}
],
"metadata": {}
}
]
}

179
experiements/Untitled1.ipynb Normal file
View File

@@ -0,0 +1,179 @@
{
"metadata": {
"name": "",
"signature": "sha256:fc6923e502800422d1ca66394e71d567efb82c4dcd09c273561149f8d3b00fe5"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"Complementary Filter"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Convert continuous transfer function into discrete\n",
"\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import scipy.signal as sig\n",
"from math import sqrt\n",
"\n",
"dt = 0.01\n",
"ki = 0.1**2\n",
"kp = sqrt(2)*0.1\n",
"\n",
"num = [kp, ki]\n",
"den = [1, 0.]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
"(array([[ 0.14142136, -0.14042136]]), array([ 1., -1.]), 0.1)"
]
}
],
"prompt_number": 8
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"a = 5\n",
"num = [a, 0]\n",
"den = [a, 1]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 11,
"text": [
"(array([[ 1., -1.]]), array([ 1. , -0.98019867]), 0.1)"
]
}
],
"prompt_number": 11
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import *\n",
"from sympy.abc import *\n",
"a = Symbol('a', positive=True)\n",
"foo = (s/(s+a)).rewrite()\n",
"print(foo)\n",
"inverse_laplace_transform(1/((s+a)), s, t)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"s/(a + s)\n"
]
},
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 39,
"text": [
"exp(-a*t)*Heaviside(t)"
]
}
],
"prompt_number": 39
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}

322
experiements/ball.py Normal file
View File

@@ -0,0 +1,322 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jul 5 16:07:29 2014
@author: rlabbe
"""
import numpy as np
from KalmanFilter import KalmanFilter
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):
def __init__(self, x0, y0, omega_deg, velocity, g=9.8, noise=[1.0,1.0]):
omega = radians(omega_deg)
self.vx0 = velocity * cos(omega)
self.vy0 = velocity * sin(omega)
self.x0 = x0
self.y0 = y0
self.g = g
self.noise = noise
def pos_at_t(self, t):
""" returns (x,y) tuple of ball position at time t"""
x = self.vx0*t + self.x0
y = -0.5*self.g*t**2 + self.vy0*t + self.y0
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)
x0,y0 initial position
launch_angle_deg angle ball is travelling respective to ground plane
velocity_ms speeed of ball in meters/second
noise amount of noise to add to each reported position in (x,y)
"""
omega = radians(launch_angle_deg)
self.v_x = velocity_ms * cos(omega)
self.v_y = velocity_ms * sin(omega)
self.x = x0
self.y = y0
self.noise = noise
def drag_force (self, 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(self, dt, vel_wind=0.):
""" compute the ball position based on the specified time step and
wind velocity. Returns (x,y) position tuple.
"""
# Euler equations for x and y
self.x += self.v_x*dt
self.y += self.v_y*dt
# force due to air drag
v_x_wind = self.v_x - vel_wind
v = sqrt (v_x_wind**2 + self.v_y**2)
F = self.drag_force(v)
# Euler's equations for velocity
self.v_x = self.v_x - F*v_x_wind*dt
self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
return (self.x + random.randn()*self.noise[0],
self.y + random.randn()*self.noise[1])
def test_baseball_path():
ball = BaseballPath (0, 1, 35, 50)
while ball.y > 0:
ball.update (0.1, 0.)
plt.scatter (ball.x, ball.y)
def test_ball_path():
y = 15
x = 0
omega = 0.
noise = [1,1]
v0 = 100.
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
f1.F = np.mat ([[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 0, 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
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
count = 0
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
while f1.x[3,0] > 0:
count += 1
#f1.update (z)
f1.predict()
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_ball_path()
#test_kf()
f1 = test_kf_drag()

38
experiements/balzer.py Normal file
View File

@@ -0,0 +1,38 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 24 15:20:47 2015
@author: rlabbe
"""
import numpy as np
import matplotlib.pyplot as plt
try:
data
except:
cols = ('time','millis','ax','ay','az','rollrate','pitchrate','yawrate',
'roll','pitch','yaw','speed','course','latitude','longitude',
'altitude','pdop','hdop','vdop','epe')
data = np.genfromtxt('2014-03-26-000-Data.csv', delimiter=',',
names=True, usecols=cols).view(np.recarray)
plt.subplot(311)
plt.plot(data.ax)
plt.subplot(312)
plt.plot(data.speed)
plt.subplot(313)
plt.plot(data.course)
plt.tight_layout()
plt.figure()
plt.subplot(311)
plt.plot(data.pitch)
plt.subplot(312)
plt.plot(data.roll)
plt.subplot(313)
plt.plot(data.yaw)
plt.figure()
plt.plot(data.longitude, data.latitude)

192
experiements/baseball.py Normal file
View File

@@ -0,0 +1,192 @@
# -*- coding: utf-8 -*-
"""
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.
This is based on the book Computational Physics by N. Giordano.
The takeaway point is that the drag coefficient on a stitched baseball is
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
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)
x0,y0 initial position
launch_angle_deg angle ball is travelling respective to ground plane
velocity_ms speeed of ball in meters/second
noise amount of noise to add to each reported position in (x,y)
"""
omega = radians(launch_angle_deg)
self.v_x = velocity_ms * cos(omega)
self.v_y = velocity_ms * sin(omega)
self.x = x0
self.y = y0
self.noise = noise
def drag_force (self, 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(self, dt, vel_wind=0.):
""" compute the ball position based on the specified time step and
wind velocity. Returns (x,y) position tuple.
"""
# Euler equations for x and y
self.x += self.v_x*dt
self.y += self.v_y*dt
# force due to air drag
v_x_wind = self.v_x - vel_wind
v = sqrt (v_x_wind**2 + self.v_y**2)
F = self.drag_force(v)
# Euler's equations for velocity
self.v_x = self.v_x - F*v_x_wind*dt
self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
return (self.x + random.randn()*self.noise[0],
self.y + random.randn()*self.noise[1])
def a_drag (vel, altitude):
""" returns the drag coefficient of a baseball at a given velocity (m/s)
and altitude (m).
"""
v_d = 35
delta = 5
y_0 = 1.0e4
val = 0.0039 + 0.0058 / (1 + math.exp((vel - v_d)/delta))
val *= math.exp(-altitude / y_0)
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)
v_wind = v_wind_mph * 0.447 # mph to m/s
altitude = alt_ft / 3.28 # to m/s
ground_level = altitude
x = [0.0]
y = [altitude]
i = 0
while y[i] >= ground_level:
v = math.sqrt((v_x - v_wind) **2+ v_y**2)
x.append(x[i] + v_x * dt)
y.append(y[i] + v_y * dt)
v_x = v_x - a_drag(v, altitude) * v * (v_x - v_wind) * dt
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.)
plt.plot (x, y)
plt.show()

219
experiements/bb_test.py Normal file
View File

@@ -0,0 +1,219 @@
# -*- coding: utf-8 -*-
"""
Created on Wed Jun 4 12:33:38 2014
@author: rlabbe
"""
from __future__ import print_function,division
from filterpy.kalman 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(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 (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;
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()
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)
if __name__ == "__main__":
run_4()

54
experiements/compute_q.ipynb Normal file
View File

@@ -0,0 +1,54 @@
{
"metadata": {
"name": "",
"signature": "sha256:72ca7769e63a40b2f7f89ae8f816dcd0df3658bb19519d8c95b2f6a5e99413ec"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import Symbol, Matrix\n",
"from sympy.interactive import printing\n",
"printing.init_printing()\n",
"\n",
"dt = Symbol('\\Delta t')\n",
"\n",
"Q = Matrix([[.5*dt**2], [.5*dt**2], [dt], [dt]])\n",
"Q*Q.T\n",
"\n",
"\n",
"Q = Matrix([[.5*dt**2], [dt]])\n",
"Q*Q.T"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}0.25 \\Delta t^{4} & 0.5 \\Delta t^{3}\\\\0.5 \\Delta t^{3} & \\Delta t^{2}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"png": "iVBORw0KGgoAAAANSUhEUgAAAKQAAAAzBAMAAADr+L6JAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMA74lUMhCZds3dIma7\nq0Ru0ZIZAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAEHUlEQVRYCe2XTWgTQRTH/0m6m68mFqwgHszi\nJSKURhQRFJJqLd6ailg8JRQtlVYbC34giEHBXks9KEXpnnoQoelFDx7MQREP4oKfqGBuHoq0fkSl\nfqxvZndnd22329KexDnsvJn572/2vZ3sewH8W6O/hCvSLXnep/RPfndkDaWfTK7IVbTqH7G2fZ+P\nVmpdIlKJz+Jg+1Y0ewEr1kK4sCgybukAuUx2G0ceSh/h8xMD94HhfEeNRg3f+BRd9rqRUs/pHFuK\nnGo4wfrOErvy1pWjzkBewmGVBpKG1hpSOocldP5stKC4kZGMNEZiBHSdw97V2YjNdQ8ziyPjTQiW\naZDII9CEiy9VsvG+VWFdFnHVjZwCNrOVSMs51iWH+OsNzZItV+nCkY0aQmyrwAgSX6CRRU0r8N37\n0A038jHtwxxIcBkSks5GPIwNn8niyDUakmwQqdvIZCXIdx/Fjt7BfuNu4/oLmMmQaSKnwd2JKujK\nxX7QPEcWikiaLyNax5s77F0lIKXySL79+RKYZI9hNekrIUs0Stzlr0lDF7nz4cnjUlSNr6N5jpxU\n0GCe9xkFDzBZA6aBCwrAQhR/8pSUVouRslOhUWM+9ptCWUGY3ZulM5JuUcn6G3md5tA4QqGk0NLu\nPERsTjSBpJlHhvvkDvqEwHBcsRyXq2xFnkOoAoTJRQrRX02iZ+KO0/ygil3UXSgCo0LGkWuKCLHX\nA5ynbcsI/jZiT7sXanzeeaGNshmaeAV2iskb5o5EMTAbR0ariPMTEyriZqAMuY5tbJ12n4FkKkVH\nsblHnmI9PWU+xODkTnIWbI41jgw3mTHrAM7J5G3ZOJyBOTrU2w2hfT1pBq4IXDNPUkoNlkM5U8KR\nuEJnSvqO2Oim4yOxIqbU0OVease+4rhUsmGGFc1IzSiUMYH4Buxkut7xoqx1WzoDeeDMM+AGIrqu\nj2Ci5SiiZLGWv3XGUopeSp/PIaoh1vMij3Eu0+uxh2JrAynkq2H8R65GFA3GPxLL4NBz/5gEB47a\nIl/H+zFgq72sLewbYjVfZA/uWVrvfgydqlj1Qjryvf1xFXeZhhBt5InAnLWQ3vmevhFezVEUZK1P\nm/lxo1s88710VnwP5pHtogBX7UXrKRfJ956vx1EUBIvzkYvk+86MLXdZjqLgtmPBekqvfE+RnKw4\n9E7TLgqSSjInVkykd74fw5QdeXEbN+yiYP+23bbIRIrkPC/f7xl67QaJkaMoSOm6mDbSGSCQtLJg\nvrfvEBaridxFgbFkOe6X7wXINuYXBS4k/PK9TRKWRpa7KHAj/fK9AAljgaLAjfTL94IkDKO8dBUF\nbqRfvhckYSxQFLiRfvlekISxQFHgRgrlyg3rB7lykiD8R4pQrNhoW8Kf52Vtwv48L+Ev/nKY9Bf/\nD+iXYj8NWuVEAAAAAElFTkSuQmCC\n",
"prompt_number": 8,
"text": [
"\u23a1 4 3\u23a4\n",
"\u23a20.25\u22c5\\Delta t 0.5\u22c5\\Delta t \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a30.5\u22c5\\Delta t \\Delta t \u23a6"
]
}
],
"prompt_number": 8
}
],
"metadata": {}
}
]
}

42
experiements/distributions.py Normal file
View File

@@ -0,0 +1,42 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 17 20:17:14 2015
@author: rlabbe
"""
from scipy.stats import t, norm
import matplotlib.pyplot as plt
import numpy as np
import math
import random
df =4
mu = 10
std = 2
x = np.linspace(-5, 20, 100)
plt.plot(x, t.pdf(x, df=df, loc=mu, scale=std), 'r-', lw=5, label='t pdf')
x2 = np.linspace(mu-10, mu+10, 100)
plt.plot(x, norm.pdf(x, mu, std), 'b-', lw=5, label='gaussian pdf')
plt.legend()
plt.figure()
def student_t(df, mu, std): # nu equals number of degrees of freedom
x = random.gauss(0, std)
y = 2.0*random.gammavariate(0.5*df, 2.0)
return x / (math.sqrt(y/df)) + mu
N = 100000
ys = [student_t(2.7, 100, 2) for i in range(N)]
plt.hist(ys, 10000, histtype='step')
ys = [random.gauss(100,2) for i in range(N)]
plt.hist(ys, 10000, histtype='step',color='r')
plt.show()

108
experiements/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()

36
experiements/dog_track_1d.py Normal file
View File

@@ -0,0 +1,36 @@
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 30 10:35:19 2014
@author: rlabbe
"""
import numpy.random as random
import math
class dog_sensor(object):
def __init__(self, x0 = 0, motion=1, noise=0.0):
self.x = x0
self.motion = motion
self.noise = math.sqrt(noise)
def sense(self):
self.x = self.x + self.motion
self.x += random.randn() * self.noise
return self.x
def measure_dog ():
if not hasattr(measure_dog, "x"):
measure_dog.x = 0
measure_dog.motion = 1
if __name__ == '__main__':
dog = dog_sensor(noise = 1)
for i in range(10):
print (dog.sense())

140
experiements/doppler.py Normal file
View File

@@ -0,0 +1,140 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 17 15:04:08 2015
@author: rlabbe
"""
from numpy.random import randn
import numpy as np
from numpy import array
import matplotlib.pyplot as plt
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import KalmanFilter
from math import sqrt
import math
import random
pos_var = 1.
dop_var = 2.
dt = 1/20
def rand_student_t(df, mu=0, std=1):
"""return random number distributed by student's t distribution with
`df` degrees of freedom with the specified mean and standard deviation.
"""
x = random.gauss(0, std)
y = 2.0*random.gammavariate(0.5*df, 2.0)
return x / (math.sqrt(y/df)) + mu
np.random.seed(124)
class ConstantVelocityObject(object):
def __init__(self, x0, vel, noise_scale):
self.x = np.array([x0, vel])
self.noise_scale = noise_scale
self.vel = vel
def update(self, dt):
pnoise = abs(randn()*self.noise_scale)
if self.x[1] > self.vel:
pnoise = -pnoise
self.x[1] += pnoise
self.x[0] += self.x[1]*dt
return self.x.copy()
def sense_pos(self):
return self.x[0] + [randn()*self.noise_scale[0]]
def vel(self):
return self.x[1] + [randn()*self.noise_scale[1]]
def sense(x, noise_scale=(1., 0.01)):
return x + [randn()*noise_scale[0], randn()*noise_scale[1]]
def sense_t(x, noise_scale=(1., 0.01)):
return x + [rand_student_t(2)*noise_scale[0],
rand_student_t(2)*noise_scale[1]]
R2 = np.zeros((2, 2))
R1 = np.zeros((1, 1))
R2[0, 0] = pos_var
R2[1, 1] = dop_var
R1[0,0] = dop_var
H2 = np.array([[1., 0.], [0., 1.]])
H1 = np.array([[0., 1.]])
kf = KalmanFilter(2, 1)
kf.F = array([[1, dt],
[0, 1]])
kf.H = array([[1, 0],
[0, 1]])
kf.Q = Q_discrete_white_noise(2, dt, .02)
kf.x = array([0., 0.])
kf.R = R1
kf.H = H1
kf.P *= 10
random.seed(1234)
track = []
zs = []
ests = []
sensor_noise = (sqrt(pos_var), sqrt(dop_var))
obj = ConstantVelocityObject(0., 1., noise_scale=.18)
for i in range(30000):
x = obj.update(dt/10)
z = sense_t(x, sensor_noise)
if i % 10 != 0:
continue
if i % 60 == 87687658760:
kf.predict()
kf.update(z, H=H2, R=R2)
else:
kf.predict()
kf.update(z[1], H=H1, R=R1)
ests.append(kf.x)
track.append(x)
zs.append(z)
track = np.asarray(track)
zs = np.asarray(zs)
ests = np.asarray(ests)
plt.figure()
plt.subplot(211)
plt.plot(track[:,0], label='track')
plt.plot(zs[:,0], label='measurements')
plt.plot(ests[:,0], label='filter')
plt.legend(loc='best')
plt.subplot(212)
plt.plot(track[:,1], label='track')
plt.plot(zs[:,1], label='measurements')
plt.plot(ests[:,1], label='filter')
plt.show()

78
experiements/gauss.py Normal file
View File

@@ -0,0 +1,78 @@
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 22 11:38:49 2014
@author: rlabbe
"""
from __future__ import division, print_function
import math
import matplotlib.pyplot as plt
import numpy.random as random
class gaussian(object):
def __init__(self, mean, variance):
try:
self.mean = float(mean)
self.variance = float(variance)
except:
self.mean = mean
self.variance = variance
def __add__ (a, b):
return gaussian (a.mean + b.mean, a.variance + b.variance)
def __mul__ (a, b):
m = (a.variance*b.mean + b.variance*a.mean) / (a.variance + b.variance)
v = 1. / (1./a.variance + 1./b.variance)
return gaussian (m, v)
def __call__(self, x):
""" Impl
"""
return math.exp (-0.5 * (x-self.mean)**2 / self.variance) / \
math.sqrt(2.*math.pi*self.variance)
def __str__(self):
return "(%f, %f)" %(self.mean, self.sigma)
def stddev(self):
return math.sqrt (self.variance)
def as_tuple(self):
return (self.mean, self.variance)
def __tuple__(self):
return (self.mean, self.variance)
def __getitem__ (self,index):
""" maybe silly, allows you to access obect as a tuple:
a = gaussian(3,4)
print (tuple(a))
"""
if index == 0: return self.mean
elif index == 1: return self.variance
else: raise StopIteration
class KF1D(object):
def __init__ (self, pos, sigma):
self.estimate = gaussian(pos,sigma)
def update(self, Z,var):
self.estimate = self.estimate * gaussian (Z,var)
def predict(self, U, var):
self.estimate = self.estimate + gaussian (U,var)
def mul2 (a, b):
m = (a['variance']*b['mean'] + b['variance']*a['mean']) / (a['variance'] + b['variance'])
v = 1. / (1./a['variance'] + 1./b['variance'])
return gaussian (m, v)
#varying_error_kf( noise_factor=100)

136
experiements/gh.py Normal file
View File

@@ -0,0 +1,136 @@
# -*- coding: utf-8 -*-
"""
Created on Thu May 15 16:07:26 2014
@author: RL
"""
from __future__ import division
import matplotlib.pyplot as plt
import numpy.random as random
def g_h_filter (data, x, dx, g, h, dt=1.):
results = []
for z in data:
x_est = x + (dx*dt)
residual = z - x_est
dx = dx + h * (residual / float(dt))
x = x_est + g * residual
print('gx',dx,)
results.append(x)
return results
'''
computation of x
x_est = weight + gain
residual = z - weight - gain
x = weight + gain + g (z - weight - gain)
w + gain + gz -wg -ggain
w -wg + gain - ggain + gz
w(1-g) + gain(1-g) +gz
(w+g)(1-g) +gz
'''
'''
gain computation
gain = gain + h/t* (z - weight - gain)
= gain + hz/t -hweight/t - hgain/t
= gain(1-h/t) + h/t(z-weight)
'''
'''
gain+ h*(z-w -gain*t)/t
gain + hz/t -hw/t -hgain
gain*(1-h) + h/t(z-w)
'''
def weight2():
w = 0
gain = 200
t = 10.
weight_scale = 1./6
gain_scale = 1./10
weights=[2060]
for i in range (len(weights)):
z = weights[i]
w_pre = w + gain*t
new_w = w_pre * (1-weight_scale) + z * (weight_scale)
print('new_w',new_w)
gain = gain *(1-gain_scale) + (z - w) * gain_scale/t
print (z)
print(w)
#gain = new_gain * (gain_scale) + gain * (1-gain_scale)
w = new_w
print ('w',w,)
print ('gain=',gain)
def weight3():
w = 160.
gain = 1.
t = 1.
weight_scale = 6/10.
gain_scale = 2./3
weights=[158]
for i in range (len(weights)):
z = weights[i]
w_pre = w + gain*t
new_w = w_pre * (1-weight_scale) + z * (weight_scale)
print('new_w',new_w)
gain = gain *(1-gain_scale) + (z - w) * gain_scale/t
print (z)
print(w)
#gain = new_gain * (gain_scale) + gain * (1-gain_scale)
w = new_w
print ('w',w,)
print ('gain=',gain)
weight3()
'''
#zs = [i + random.randn()*50 for i in range(200)]
zs = [158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0]
#zs = [2060]
data= g_h_filter(zs, 160, 1, .6, 0, 1.)
'''
data = g_h_filter([2060], x=0, dx=200, g=1./6, h = 1./10, dt=10)
print data
'''
print data
print data2
plt.plot(data)
plt.plot(zs, 'g')
plt.show()
'''

57
experiements/histogram.py Normal file
View File

@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 22 10:43:38 2014
@author: rlabbe
"""
p = [.2, .2, .2, .2, .2]
world = ['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pOvershoot = 0.1
pUndershoot = 0.1
pExact = 0.8
def normalize (p):
s = sum(p)
for i in range (len(p)):
p[i] = p[i] / s
def sense(p, Z):
q= []
for i in range (len(p)):
hit = (world[i] ==Z)
q.append(p[i] * (pHit*hit + pMiss*(1-hit)))
normalize(q)
return q
def move(p, U):
q = []
for i in range(len(p)):
pexact = p[(i-U) % len(p)] * pExact
pover = p[(i-U-1) % len(p)] * pOvershoot
punder = p[(i-U+1) % len(p)] * pUndershoot
q.append (pexact + pover + punder)
normalize(q)
return q
if __name__ == "__main__":
p = sense(p, 'red')
print p
pause()
for z in measurements:
p = sense (p, z)
p = move (p, 1)
print p

47
experiements/image_tracker.py Normal file
View File

@@ -0,0 +1,47 @@
# -*- coding: utf-8 -*-
"""
Created on Sat May 24 14:42:55 2014
@author: rlabbe
"""
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
f = KalmanFilter (dim=4)
dt = 1
f.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
f.H = np.mat ([[1, 0, 0, 0],
[0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50,0],
[0, 50]])
f.x = np.mat([0,0,0,0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
f.predict ()
f.update (z)
xs.append (f.x[0,0])
ys.append (f.x[2,0])
plt.plot (xs, ys)
plt.show()

57
experiements/mkf_ellipse_test.py Normal file
View File

@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 11 20:47:52 2014
@author: rlabbe
"""
from DogSensor import DogSensor
from filterpy.kalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import stats
def dog_tracking_filter(R,Q=0,cov=1.):
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.matrix([[0], [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.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):
dog = DogSensor(velocity=1, noise=noise)
f = dog_tracking_filter(R=R, Q=Q, cov=10.)
ps = []
zs = []
cov = []
for t in range (count):
z = dog.sense()
f.update (z)
#print (t,z)
ps.append (f.x[0,0])
cov.append(f.P)
zs.append(z)
f.predict()
p0, = plt.plot([0,count],[0,count],'g')
p1, = plt.plot(range(1,count+1),zs,c='r', linestyle='dashed')
p2, = plt.plot(range(1,count+1),ps, c='b')
plt.axis('equal')
plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2)
plt.title(title)
for i,p in enumerate(cov):
print(i,p)
e = stats.sigma_ellipse (p, i+1, ps[i])
stats.plot_sigma_ellipse(e, axis_equal=False)
plt.xlim((-1,count))
plt.show()
if __name__ == "__main__":
plot_track (noise=30, R=5, Q=2, count=20)

13
experiements/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.

104
experiements/nonlinear_plots.py Normal file
View File

@@ -0,0 +1,104 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 18 11:09:23 2014
@author: rlabbe
"""
from __future__ import division
import numpy as np
import matplotlib.pyplot as plt
from numpy.random import normal
def plot_transfer_func(data, f, lims,num_bins=1000):
ys = f(data)
#plot output
plt.subplot(2,2,1)
plt.hist(ys, num_bins, orientation='horizontal',histtype='step')
plt.ylim(lims)
plt.gca().xaxis.set_ticklabels([])
# plot transfer function
plt.subplot(2,2,2)
x = np.arange(lims[0], lims[1],0.1)
y = f(x)
plt.plot (x,y)
isct = f(0)
plt.plot([0,0,lims[0]],[lims[0],isct,isct],c='r')
plt.xlim(lims)
# plot input
plt.subplot(2,2,4)
plt.hist(data, num_bins, histtype='step')
plt.xlim(lims)
plt.gca().yaxis.set_ticklabels([])
plt.show()
normals = normal(loc=0.0, scale=1, size=5000000)
#rint h(normals).sort()
def f(x):
return 2*x + 1
def g(x):
return (cos(4*(x/2+0.7)))*sin(0.3*x)-0.9*x
return (cos(4*(x/3+0.7)))*sin(0.3*x)-0.9*x
#return -x+1.2*np.sin(0.7*x)+3
return sin(5-.2*x)
def h(x): return cos(.4*x)*x
plot_transfer_func (normals, g, lims=(-4,4),num_bins=500)
del(normals)
#plt.plot(g(np.arange(-10,10,0.1)))
'''
ys = f(normals)
r = np.linspace (min(normals), max(normals), num_bins)
h= np.histogram(ys, num_bins,density=True)
print h
print len(h[0]), len(h[1][0:-1])
#plot output
plt.subplot(2,2,1)
h = np.histogram(ys, num_bins,normed=True)
p, = plt.plot(h[0],h[1][1:])
plt.ylim((-10,10))
plt.xlim((max(h[0]),0))
# plot transfer function
plt.subplot(2,2,2)
x = np.arange(-10,10)
y = 1.2*x + 1
plt.plot (x,y)
plt.plot([0,0],[-10,f(0)],c='r')
plt.ylim((-10,10))
# plot input
plt.subplot(2,2,4)
h = np.histogram(normals, num_bins,density=True)
plt.plot(h[1][1:],h[0])
plt.xlim((-10,10))
plt.show()
'''

118
experiements/range_finder.py Normal file
View File

@@ -0,0 +1,118 @@
# -*- coding: utf-8 -*-
"""
Created on Sat May 24 19:14:06 2014
@author: rlabbe
"""
from __future__ import division, print_function
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
import math
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 = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = math.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)
def dist(a,b):
return math.sqrt ((a[0]-b[0])**2 + (a[1]-b[1])**2)
def H_of (pos, pos_A, pos_B):
from math import sin, cos, atan2
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])
return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
[-cos(theta_b), 0, -sin(theta_b), 0]])
# equivalently we can do this...
#dist_a = dist(pos, pos_A)
#dist_b = dist(pos, pos_B)
#return np.mat([[(pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a,0],
# [(pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b,0]])
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim=4)
dt = 1.0 # time step
'''
f1.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
'''
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 = np.eye(2) * 1.
f1.Q = np.eye(4) * .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 = [],[]
d = DMESensor (pos_a, pos_b, noise_factor=1.)
pos = [0,0]
for i in range(count):
pos = (i,i)
ra,rb = d.range_of(pos)
rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
print ('range =', ra,rb)
z = np.mat([[ra-rx],[rb-ry]])
print('z =', z)
f1.H = H_of (pos, pos_a, pos_b)
print('H =', f1.H)
##f1.update (z)
print (f1.x)
xs.append (f1.x[0,0]+i)
ys.append (f1.x[2,0]+i)
pxs.append (pos[0])
pys.append(pos[1])
f1.predict ()
print (f1.H * f1.x)
print (z)
print (f1.x)
f1.update(z)
print(f1.x)
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.legend([p1,p2], ['filter', 'ideal'], 2)
plt.show()

100
experiements/satellite.ipynb Normal file
View File

@@ -0,0 +1,100 @@
{
"metadata": {
"name": "",
"signature": "sha256:9fb5a469b0b0f27f1e721735fae76d74f0ad242621b94c3f528e9392b7deab82"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"import math\n",
"\n",
"def range_to(x1, x2):\n",
" dx = x1[0] - x2[0]\n",
" dy = x1[1] - x2[1]\n",
" print('dx',dx,dy)\n",
" return math.sqrt(dx*dx + dy*dy)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 40
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"s1 = (0., 2.) # satellite 1\n",
"s2 = (1., 2.) # satellite 2\n",
"p = (-0.5, 1.) # platform postion\n",
"\n",
"\n",
"s1 = (0., 200.) # satellite 1\n",
"s2 = (100., 220.) # satellite 2\n",
"p = (10, 1.) # platform postion\n",
"\n",
"r1 = range_to(s1, p)\n",
"r2 = range_to(s2, p)\n",
"\n",
"x1, y1 = s1[0], s1[1]\n",
"x2, y2 = s2[0], s2[1]\n",
"\n",
"A = -(y2 - y1) / (x2 - x1)\n",
"B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))\n",
"\n",
"a = 1 + A**2\n",
"b = -2*A*x1 + 2*A*B - 2*y1\n",
"c = x1**2 - 2*x1*B + y1**2 - r1**2\n",
"\n",
"y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)\n",
"x = A*y + B\n",
"\n",
"print('p', x,y)\n",
"print('err', x-p[0], y-p[1])"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"dx -10.0 199.0\n",
"dx 90.0 219.0\n",
"p 10.051726583768032 0.7413670811596572\n",
"err 0.05172658376803163 -0.2586329188403428\n"
]
}
],
"prompt_number": 55
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"A"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 56,
"text": [
"-0.2"
]
}
],
"prompt_number": 56
}
],
"metadata": {}
}
]
}

9
experiements/temp.py Normal file
View File

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

88
experiements/test_stats.py Normal file
View File

@@ -0,0 +1,88 @@
# -*- coding: utf-8 -*-
"""
py.test module to test stats.py module.
Created on Wed Aug 27 06:45:06 2014
@author: rlabbe
"""
from __future__ import division
from math import pi, exp
import numpy as np
from stats import gaussian, multivariate_gaussian, _to_cov
from numpy.linalg import inv
from numpy import linalg
def near_equal(x,y):
return abs(x-y) < 1.e-15
def test_gaussian():
import scipy.stats
mean = 3.
var = 1.5
std = var**0.5
for i in np.arange(-5,5,0.1):
p0 = scipy.stats.norm(mean, std).pdf(i)
p1 = gaussian(i, mean, var)
assert near_equal(p0, p1)
def norm_pdf_multivariate(x, mu, sigma):
""" extremely literal transcription of the multivariate equation.
Slow, but easy to verify by eye compared to my version."""
n = len(x)
sigma = _to_cov(sigma,n)
det = linalg.det(sigma)
norm_const = 1.0 / (pow((2*pi), n/2) * pow(det, .5))
x_mu = x - mu
result = exp(-0.5 * (x_mu.dot(inv(sigma)).dot(x_mu.T)))
return norm_const * result
def test_multivariate():
from scipy.stats import multivariate_normal as mvn
from numpy.random import rand
mean = 3
var = 1.5
assert near_equal(mvn(mean,var).pdf(0.5),
multivariate_gaussian(0.5, mean, var))
mean = np.array([2.,17.])
var = np.array([[10., 1.2], [1.2, 4.]])
x = np.array([1,16])
assert near_equal(mvn(mean,var).pdf(x),
multivariate_gaussian(x, mean, var))
for i in range(100):
x = np.array([rand(), rand()])
assert near_equal(mvn(mean,var).pdf(x),
multivariate_gaussian(x, mean, var))
assert near_equal(mvn(mean,var).pdf(x),
norm_pdf_multivariate(x, mean, var))
mean = np.array([1,2,3,4])
var = np.eye(4)*rand()
x = np.array([2,3,4,5])
assert near_equal(mvn(mean,var).pdf(x),
norm_pdf_multivariate(x, mean, var))