"""
Created on Sun Feb 15 14:29:23 2015
@author: rlabbe
"""
"""
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)
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
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)
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])
p = R2.z_to_x(z[2], z[3])
plt.show()
two_radar_constalt()
