

def test_circle():
    from filterpy.kalman import KalmanFilter
    from math import radians

    def hx(x):
        radius = x[0]
        angle = x[1]
        x = (cos(radians(angle)) * radius)
        y = (sin(radians(angle)) * radius)
        return np.array([x, y])

    def fx(x, dt):
        return np.array([x[0], (x[1] + x[2]), x[2]])
    std_noise = 0.1
    sp = JulierSigmaPoints(n=3, kappa=0.0)
    f = UKF(dim_x=3, dim_z=2, dt=0.01, hx=hx, fx=fx, points=sp)
    f.x = np.array([50.0, 90.0, 0])
    f.P *= 100
    f.R = (np.eye(2) * (std_noise ** 2))
    f.Q = (np.eye(3) * 0.001)
    f.Q[(0, 0)] = 0
    f.Q[(2, 2)] = 0
    kf = KalmanFilter(dim_x=6, dim_z=2)
    kf.x = np.array([50.0, 0.0, 0, 0, 0.0, 0.0])
    F = np.array([[1.0, 1.0, 0.5, 0.0, 0.0, 0.0], [0.0, 1.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0, 1.0, 0.5], [0.0, 0.0, 0.0, 0.0, 1.0, 1.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
    kf.F = F
    kf.P *= 100
    kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
    kf.R = f.R
    kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1.0, 1e-05)
    kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1.0, 1e-05)
    measurements = []
    results = []
    zs = []
    kfxs = []
    for t in range(0, 12000):
        a = ((t / 30) + 90)
        x = ((cos(radians(a)) * 50.0) + (randn() * std_noise))
        y = ((sin(radians(a)) * 50.0) + (randn() * std_noise))
        z = np.array([x, y])
        zs.append(z)
        f.predict()
        f.update(z)
        kf.predict()
        kf.update(y)
        results.append(hx(f.x))
        kfxs.append(kf.x)
    results = np.asarray(results)
    zs = np.asarray(zs)
    kfxs = np.asarray(kfxs)
    print(results)
    if DO_PLOT:
        plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
        plt.plot(results[:, 0], results[:, 1], c='k', label='UKF')
        plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF')
        plt.legend(loc='best')
        plt.axis('equal')

dataset/ETHPy150Open rlabbe/filterpy/filterpy/kalman/tests/test_ukf.py test_circle/VarMisuse@623/680 `z`->`y`