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

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