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

Extensive addtions to UKF chapter. 

I think I finally arrived at a good ordering of material.
Started with implementing a linear problem just so we can
see how it differs from the linear KF, then added problems
step by step. Got rid of most of the poor performing filters.
This commit is contained in:
Roger Labbe 2015-02-22 11:33:51 -08:00
parent 5e479234e1
commit 37743bf604
6 changed files with 1805 additions and 1313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,5 +1,7 @@
# test document not for distribution
short.pdf
10_Unscented_Kalman_Filter_.ipynb
tmp.ipynb
# Byte-compiled / optimized / DLL files
__pycache__/

2651
10_Unscented_Kalman_Filter.ipynb
View File

File diff suppressed because one or more lines are too long

14
book_format.py
View File

@ -5,6 +5,8 @@ import matplotlib.pyplot as plt
import json
import numpy as np
import sys
from contextlib import contextmanager
sys.path.insert(0,'./code') # allow us to import book_format
@ -40,7 +42,17 @@ def reset_axis():
def set_figsize(x, y):
pylab.rcParams['figure.figsize'] = x, y
@contextmanager
def figsize(x,y):
"""Temporarily set the figure size using 'with figsize(a,b):'"""
set_figsize(x,y)
yield
reset_axis()
def _decode_list(data):
rv = []
for item in data:

40
code/ekf_internal.py
View File

@ -19,13 +19,13 @@ def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
f1.F = np.array ([[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, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 1]]) # ddy = -g.
f1.H = np.array([
[1, 0, 0, 0, 0],
[0, 0, 1, 0, 0]])
f1.R *= r
f1.Q *= q
@ -34,20 +34,20 @@ def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
vy = sin(omega) * v0
f1.x = np.array([[x,vx,y,vy,-9.8]]).T
return f1
class BaseballPath(object):
def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)):
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)
@ -77,7 +77,7 @@ class BaseballPath(object):
# 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)
@ -85,7 +85,7 @@ class BaseballPath(object):
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],
return (self.x + random.randn()*self.noise[0],
self.y + random.randn()*self.noise[1])
@ -96,7 +96,7 @@ def plot_ball():
theta = 35. # launch angle
v0 = 50.
dt = 1/10. # time step
ball = BaseballPath(x0=x, y0=y, launch_angle_deg=theta, velocity_ms=v0, noise=[.3,.3])
f1 = ball_kf(x,y,theta,v0,dt,r=1.)
f2 = ball_kf(x,y,theta,v0,dt,r=10.)
@ -105,29 +105,29 @@ def plot_ball():
ys = []
xs2 = []
ys2 = []
while f1.x[2,0] > 0:
t += dt
x,y = ball.update(dt)
z = np.mat([[x,y]]).T
f1.update(z)
f2.update(z)
xs.append(f1.x[0,0])
ys.append(f1.x[2,0])
xs2.append(f2.x[0,0])
ys2.append(f2.x[2,0])
f1.predict()
ys2.append(f2.x[2,0])
f1.predict()
f2.predict()
p1 = plt.scatter(x, y, color='green', marker='o', s=75, alpha=0.5)
p2, = plt.plot (xs, ys,lw=2)
p3, = plt.plot (xs2, ys2,lw=4, c='r')
plt.legend([p1,p2, p3], ['Measurements', 'Kalman filter(R=0.5)', 'Kalman filter(R=10)'])
plt.show()
def show_radar_chart():
plt.xlim([0.9,2.5])
plt.ylim([0.5,2.5])
@ -146,6 +146,8 @@ def show_radar_chart():
arrowprops=dict(arrowstyle='->', ec='b',shrinkA=0, shrinkB=4))
ax.annotate('$\Theta$ (', xy=(1.2, 1.1), color='b')
ax.annotate('Aircraft', xy=(2.04,2.), color='b')
ax.annotate('altitude', xy=(2.04,1.5), color='k')
ax.annotate('X', xy=(1.5, .9))

89
code/ukf_internal.py
View File

@ -9,18 +9,77 @@ from matplotlib.patches import Ellipse,Arrow
import stats
import numpy as np
import math
from filterpy.kalman import UnscentedKalmanFilter as UKF
from stats import plot_covariance_ellipse
def _sigma_points(mean, sigma, kappa):
sigma1 = mean + math.sqrt((1+kappa)*sigma)
sigma2 = mean - math.sqrt((1+kappa)*sigma)
return mean, sigma1, sigma2
def arrow(x1,y1,x2,y2):
return Arrow(x1,y1, x2-x1, y2-y1, lw=2, width=0.1, ec='k', color='k')
def arrow(x1,y1,x2,y2, width=0.2):
return Arrow(x1,y1, x2-x1, y2-y1, lw=1, width=width, ec='k', color='k')
def show_two_sensor_bearing():
circle1=plt.Circle((-4,0),5,color='#004080',fill=False,linewidth=20, alpha=.7)
circle2=plt.Circle((4,0),5,color='#E24A33', fill=False, linewidth=5, alpha=.7)
fig = plt.gcf()
ax = fig.gca()
plt.axis('equal')
plt.xlim((-10,10))
plt.ylim((-10,10))
plt.plot ([-4,0], [0,3], c='#004080')
plt.plot ([4,0], [0,3], c='#E24A33')
plt.text(-4, -.5, "A", fontsize=16, horizontalalignment='center')
plt.text(4, -.5, "B", fontsize=16, horizontalalignment='center')
ax.add_artist(circle1)
ax.add_artist(circle2)
plt.show()
def show_sigma_transform():
fig = plt.figure()
ax=fig.gca()
x = np.array([0, 5])
P = np.array([[4, -2.2], [-2.2, 3]])
plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5)
S = UKF.sigma_points(x=x, P=P, kappa=0)
plt.scatter(S[:,0], S[:,1], c='k', s=80)
x = np.array([15, 5])
P = np.array([[3, 1.2],[1.2, 6]])
plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5)
ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6))
ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6))
ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6))
ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6))
ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6))
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
#plt.axis('equal')
plt.show()
def show_2d_transform():
ax=plt.gca()
ax.add_artist(Ellipse(xy=(2,5), width=2, height=3,angle=70,linewidth=1,ec='k'))
ax.add_artist(Ellipse(xy=(7,5), width=2.2, alpha=0.3, height=3.8,angle=150,linewidth=1,ec='k'))
ax.add_artist(arrow(2, 5, 6, 4.8))
ax.add_artist(arrow(1.5, 5.5, 7, 3.8))
ax.add_artist(arrow(2.3, 4.1, 8, 6))
@ -68,32 +127,29 @@ def show_sigma_selections():
def show_sigmas_for_2_kappas():
# generate the Gaussian data
xs = np.arange(-4, 4, 0.1)
mean = 0
sigma = 1.5
ys = [stats.gaussian(x, mean, sigma*sigma) for x in xs]
def sigma_points(mean, sigma, kappa):
sigma1 = mean + math.sqrt((1+kappa)*sigma)
sigma2 = mean - math.sqrt((1+kappa)*sigma)
return mean, sigma1, sigma2
#generate our samples
kappa = 2
x0,x1,x2 = sigma_points(mean, sigma, kappa)
x0,x1,x2 = _sigma_points(mean, sigma, kappa)
samples = [x0,x1,x2]
for x in samples:
p1 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='k')
kappa = -.5
x0,x1,x2 = sigma_points(mean, sigma, kappa)
x0,x1,x2 = _sigma_points(mean, sigma, kappa)
samples = [x0,x1,x2]
for x in samples:
p2 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='b')
plt.legend([p1,p2], ['$kappa$=2', '$kappa$=-0.5'])
plt.plot(xs, ys)
plt.show()
@ -101,5 +157,6 @@ def show_sigmas_for_2_kappas():
if __name__ == '__main__':
show_sigma_selections()
show_sigma_transform()
#show_sigma_selections()

322
experiments/two_radar.py
View File

@ -19,145 +19,265 @@ from numpy import array, asarray
from numpy.linalg import norm
from numpy.random import randn
import math
from math import atan2, radians, degrees
from math import sin, cos, atan2, radians, degrees
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.common import Q_discrete_white_noise
class RadarStation(object):
def __init__(self, pos, range_std, bearing_std):
self.pos = asarray(pos)
self.range_std = range_std
self.bearing_std = bearing_std
def reading_of(self, ac_pos):
""" Returns range and bearing to aircraft as tuple. bearing is in
""" Returns range and bearing to aircraft as tuple. bearing is in
radians.
"""
diff = np.subtract(self.pos, ac_pos)
diff = np.subtract(ac_pos, self.pos)
rng = norm(diff)
brg = atan2(diff[1], diff[0])
brg = atan2(diff[1], diff[0])
return rng, brg
def noisy_reading(self, ac_pos):
rng, brg = self.reading_of(ac_pos)
rng, brg = self.reading_of(ac_pos)
rng += randn() * self.range_std
brg += randn() * self.bearing_std
brg += randn() * self.bearing_std
return rng, brg
def z_to_x(self, slant_range, angle):
""" given a reading, convert to world coordinates"""
x = cos(angle)*slant_range
z = sin(angle)*slant_range
return self.pos + (x,z)
class ACSim(object):
def __init__(self, pos, vel, vel_std):
self.pos = asarray(pos, dtype=float)
self.vel = asarray(vel, dtype=float)
self.vel_std = vel_std
def update(self):
vel = self.vel + (randn() * self.vel_std)
vel = self.vel + (randn() * self.vel_std)
self.pos += vel
print(pos)
return self.pos
dt = 1.
def hx(x):
r1, b1 = hx.R1.reading_of((x[0], x[2]))
r2, b2 = hx.R2.reading_of((x[0], x[2]))
def two_radar_constvel():
dt = 5
return array([r1, b1, r2, b2])
pass
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1]*dt
x_est[2] += x[3]*dt
return x_est
f = UKF(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=1)
aircraft = ACSim ((100,100), (0.1,0.1), 0.0)
R1 = RadarStation ((0,0), range_std=1.0, bearing_std=radians(0.5))
R2 = RadarStation ((200,0), range_std=1.0, bearing_std=radians(0.5))
hx.R1 = R1
hx.R2 = R2
f.x = array([100, 1, 100, 1])
f.R = np.diag([1.0, 0.5, 1.0, 0.5])
f.Q *= 0.0020
xs = []
track = []
for i in range(int(20/dt)):
pos = aircraft.update()
def hx(x):
r1, b1 = hx.R1.reading_of((x[0], x[2]))
r2, b2 = hx.R2.reading_of((x[0], x[2]))
r1, b1 = R1.noisy_reading(pos)
r2, b2, = R2.noisy_reading(pos)
return array([r1, b1, r2, b2])
pass
z = np.array([r1, b1, r2, b2])
track.append(pos.copy())
f.predict()
f.update(z)
xs.append(f.x)
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1]*dt
x_est[2] += x[3]*dt
return x_est
f = UKF(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
aircraft = ACSim ((100,100), (0.1*dt,0.02*dt), 0.002)
range_std = 0.2
bearing_std = radians(0.5)
R1 = RadarStation ((0,0), range_std, bearing_std)
R2 = RadarStation ((200,0), range_std, bearing_std)
hx.R1 = R1
hx.R2 = R2
f.x = array([100, 0.1, 100, 0.02])
f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2])
q = Q_discrete_white_noise(2, var=0.002, dt=dt)
#q = np.array([[0,0],[0,0.0002]])
f.Q[0:2, 0:2] = q
f.Q[2:4, 2:4] = q
f.P = np.diag([.1, 0.01, .1, 0.01])
track = []
zs = []
for i in range(int(300/dt)):
pos = aircraft.update()
r1, b1 = R1.noisy_reading(pos)
r2, b2 = R2.noisy_reading(pos)
z = np.array([r1, b1, r2, b2])
zs.append(z)
track.append(pos.copy())
zs = asarray(zs)
xs, Ps, Pxz = f.batch_filter(zs)
ms, _, _ = f.rts_smoother2(xs, Ps, Pxz)
track = asarray(track)
time = np.arange(0,len(xs)*dt, dt)
plt.figure()
plt.subplot(411)
plt.plot(time, track[:,0])
plt.plot(time, xs[:,0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('x position (m)')
plt.subplot(412)
plt.plot(time, track[:,1])
plt.plot(time, xs[:,2])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('y position (m)')
plt.subplot(413)
plt.plot(time, xs[:,1])
plt.plot(time, ms[:,1])
plt.legend(loc=4)
plt.ylim([0, 0.2])
plt.xlabel('time (sec)')
plt.ylabel('x velocity (m/s)')
plt.subplot(414)
plt.plot(time, xs[:,3])
plt.plot(time, ms[:,3])
plt.ylabel('y velocity (m/s)')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.show()
xs = asarray(xs)
track = asarray(track)
time = np.arange(0,len(xs)*dt, dt)
def two_radar_constalt():
dt = .05
def hx(x):
r1, b1 = hx.R1.reading_of((x[0], x[2]))
r2, b2 = hx.R2.reading_of((x[0], x[2]))
return array([r1, b1, r2, b2])
pass
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1]*dt
return x_est
vx = 100/1000 # meters/sec
vz = 0.
f = UKF(dim_x=3, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
aircraft = ACSim ((0, 1), (vx*dt, vz*dt), 0.00)
range_std = 1/1000.
bearing_std =1/1000000.
R1 = RadarStation (( 0, 0), range_std, bearing_std)
R2 = RadarStation ((60, 0), range_std, bearing_std)
hx.R1 = R1
hx.R2 = R2
f.x = array([aircraft.pos[0], vx, aircraft.pos[1]])
f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2])
q = Q_discrete_white_noise(2, var=0.0002, dt=dt)
#q = np.array([[0,0],[0,0.0002]])
f.Q[0:2, 0:2] = q
f.Q[2,2] = 0.0002
f.P = np.diag([.1, 0.01, .1])*0.1
track = []
zs = []
for i in range(int(500/dt)):
pos = aircraft.update()
r1, b1 = R1.noisy_reading(pos)
r2, b2 = R2.noisy_reading(pos)
z = np.array([r1, b1, r2, b2])
zs.append(z)
track.append(pos.copy())
zs = asarray(zs)
xs, Ps = f.batch_filter(zs)
ms, _, _ = f.rts_smoother(xs, Ps)
track = asarray(track)
time = np.arange(0,len(xs)*dt, dt)
plt.figure()
plt.subplot(311)
plt.plot(time, track[:,0])
plt.plot(time, xs[:,0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('x position (m)')
plt.figure()
plt.subplot(411)
plt.plot(time, track[:,0])
plt.plot(time, xs[:,0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('x position (m)')
plt.subplot(312)
plt.plot(time, xs[:,1]*1000, label="UKF")
plt.plot(time, ms[:,1]*1000, label='RTS')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('velocity (m/s)')
plt.subplot(313)
plt.plot(time, xs[:,2]*1000, label="UKF")
plt.plot(time, ms[:,2]*1000, label='RTS')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('altitude (m)')
plt.ylim([900,1100])
for z in zs[:10]:
p = R1.z_to_x(z[0], z[1])
#plt.scatter(p[0], p[1], marker='+', c='k')
p = R2.z_to_x(z[2], z[3])
#plt.scatter(p[0], p[1], marker='+', c='b')
plt.show()
plt.subplot(412)
plt.plot(time, track[:,1])
plt.plot(time, xs[:,3])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('y position (m)')
plt.subplot(413)
plt.plot(time, xs[:,1])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('x velocity (m/s)')
plt.subplot(414)
plt.plot(time, xs[:,3])
plt.ylabel('y velocity (m/s)')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.show()
two_radar_constalt()