37743bf604
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.
283 lines
6.1 KiB
Python
283 lines
6.1 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Sun Feb 15 14:29:23 2015
|
|
|
|
@author: rlabbe
|
|
"""
|
|
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Sun Feb 8 09:34:36 2015
|
|
|
|
@author: rlabbe
|
|
"""
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
|
|
from numpy import array, asarray
|
|
from numpy.linalg import norm
|
|
from numpy.random import randn
|
|
import math
|
|
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
|
|
radians.
|
|
"""
|
|
|
|
diff = np.subtract(ac_pos, self.pos)
|
|
rng = norm(diff)
|
|
brg = atan2(diff[1], diff[0])
|
|
return rng, brg
|
|
|
|
|
|
def noisy_reading(self, ac_pos):
|
|
rng, brg = self.reading_of(ac_pos)
|
|
rng += randn() * self.range_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)
|
|
self.pos += vel
|
|
|
|
return self.pos
|
|
|
|
|
|
def two_radar_constvel():
|
|
dt = 5
|
|
|
|
|
|
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
|
|
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()
|
|
|
|
|
|
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.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()
|
|
|
|
|
|
two_radar_constalt() |