

def test_rts():

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

    def hx(x):
        return np.sqrt(((x[0] ** 2) + (x[2] ** 2)))
    dt = 0.05
    sp = JulierSigmaPoints(n=3, kappa=1.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)
    kf.x = np.array([0.0, 90.0, 1100.0])
    kf.P = (np.eye(3) * 100)
    (M, P) = kf.batch_filter(rs)
    assert np.array_equal(M, xs), 'Batch filter generated different output'
    Qs = ([kf.Q] * len(t))
    (M2, P2, K) = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
    if DO_PLOT:
        print(xs[:, 0].shape)
        plt.figure()
        plt.subplot(311)
        plt.plot(t, xs[:, 0])
        plt.plot(t, M2[:, 0], c='g')
        plt.subplot(312)
        plt.plot(t, xs[:, 1])
        plt.plot(t, M2[:, 1], c='g')
        plt.subplot(313)
        plt.plot(t, xs[:, 2])
        plt.plot(t, M2[:, 2], c='g')

dataset/ETHPy150Open rlabbe/filterpy/filterpy/kalman/tests/test_ukf.py test_rts/original