

def test_radar():

    def fx(x, dt):
        A = (np.eye(3) + (dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]])))
        return A.dot(x)

    def hx(x):
        return np.sqrt(((x[0] ** 2) + (x[2] ** 2)))
    dt = 0.05
    sp = JulierSigmaPoints(n=3, kappa=0.0)
    kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
    kf.Q *= 0.01
    kf.R = 10
    kf.x = np.array([0.0, 90.0, 1100.0])
    kf.P *= 100.0
    radar = RadarSim(dt)
    t = np.arange(0, (20 + dt), dt)
    n = len(t)
    xs = np.zeros((n, 3))
    random.seed(200)
    rs = []
    for i in range(len(t)):
        r = radar.get_range()
        kf.predict()
        kf.update(z=[r])
        xs[i, :] = kf.x
        rs.append(r)
    if DO_PLOT:
        print(xs[:, 0].shape)
        plt.figure()
        plt.subplot(311)
        plt.plot(t, xs[:, 0])
        plt.subplot(312)
        plt.plot(t, radar[:, 1])
        plt.subplot(313)
        plt.plot(t, xs[:, 2])

dataset/ETHPy150Open rlabbe/filterpy/filterpy/kalman/tests/test_ukf.py test_radar/VarMisuse@356/388 `xs`->`radar`