

def two_radar():
    import numpy as np
    import matplotlib.pyplot as plt
    from numpy import array
    from numpy.linalg import norm
    from numpy.random import randn
    from math import atan2, radians
    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\n            radians.\n            '
            diff = np.subtract(self.pos, ac_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)

    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
    dt = 1.0

    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
    (vx, vy) = (0.1, 0.1)
    f = UnscentedKalmanFilter(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
    aircraft = ACSim((100, 100), ((vx * dt), (vy * dt)), 2e-08)
    range_std = 0.001
    bearing_std = (1 / 1000)
    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, vx, 100, vy])
    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=R2)
    f.Q[0:2, 0:2] = q
    f.Q[2:4, 2:4] = q
    f.P = np.diag([0.1, 0.01, 0.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, pM, pP) = 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(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.tight_layout()
    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.tight_layout()
    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.tight_layout()
    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.tight_layout()
    plt.show()

dataset/ETHPy150Open rlabbe/filterpy/filterpy/kalman/tests/test_ukf.py two_radar/VarMisuse@627/782 `dt`->`R2`