Kalman-and-Bayesian-Filters-in-Python/experiments/two_radar.py

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