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.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

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