Beispiel #1
0
def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2], [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together

    sp0 = JulierSigmaPoints(n=2, kappa=kappa)
    sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000)

    w0, _ = sp0.weights()
    w1, _ = sp1.weights()

    Xi0 = sp0.sigma_points(x, P)
    Xi1 = sp1.sigma_points(x, P)

    assert max(Xi1[:, 0]) > max(Xi0[:, 0])
    assert max(Xi1[:, 1]) > max(Xi0[:, 1])

    if DO_PLOT:
        plt.figure()
        for i in range(Xi0.shape[0]):
            plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0],
                        (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1],
                        color='blue')

        for i in range(Xi1.shape[0]):
            plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0],
                        (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1],
                        color='green')

        stats.plot_covariance_ellipse([1, 2], P)
Beispiel #2
0
def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2], [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together

    sp0 = JulierSigmaPoints(n=2, kappa=kappa)
    sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000)
    sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3)
    sp3 = SimplexSigmaPoints(n=2)

    # test __repr__ doesn't crash
    str(sp0)
    str(sp1)
    str(sp2)
    str(sp3)

    w0, _ = sp0.weights()
    w1, _ = sp1.weights()
    w2, _ = sp2.weights()
    w3, _ = sp3.weights()

    Xi0 = sp0.sigma_points(x, P)
    Xi1 = sp1.sigma_points(x, P)
    Xi2 = sp2.sigma_points(x, P)
    Xi3 = sp3.sigma_points(x, P)

    assert max(Xi1[:, 0]) > max(Xi0[:, 0])
    assert max(Xi1[:, 1]) > max(Xi0[:, 1])

    if DO_PLOT:
        plt.figure()
        for i in range(Xi0.shape[0]):
            plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0],
                        (Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1],
                        color='blue',
                        label='Julier low $\kappa$')

        for i in range(Xi1.shape[0]):
            plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0],
                        (Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1],
                        color='green',
                        label='Julier high $\kappa$')
        # for i in range(Xi2.shape[0]):
        #     plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0],
        #                 (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1],
        #                 color='red')
        for i in range(Xi3.shape[0]):
            plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0],
                        (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1],
                        color='black',
                        label='Simplex')

        stats.plot_covariance_ellipse([1, 2], P)
Beispiel #3
0
def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2],
                  [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together

    sp0 = JulierSigmaPoints(n=2, kappa=kappa)
    sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)
    sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3)
    sp3 = SimplexSigmaPoints(n=2)

    w0, _ = sp0.weights()
    w1, _ = sp1.weights()
    w2, _ = sp2.weights()
    w3, _ = sp3.weights()

    Xi0 = sp0.sigma_points(x, P)
    Xi1 = sp1.sigma_points(x, P)
    Xi2 = sp2.sigma_points(x, P)
    Xi3 = sp3.sigma_points(x, P)

    assert max(Xi1[:,0]) > max(Xi0[:,0])
    assert max(Xi1[:,1]) > max(Xi0[:,1])

    if DO_PLOT:
        plt.figure()
        for i in range(Xi0.shape[0]):
            plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0],
                        (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1],
                         color='blue', label='Julier low $\kappa$')

        for i in range(Xi1.shape[0]):
            plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0],
                        (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1],
                         color='green', label='Julier high $\kappa$')
        # for i in range(Xi2.shape[0]):
        #     plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0],
        #                 (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1],
        #                 color='red')
        for i in range(Xi3.shape[0]):
            plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0],
                        (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1],
                        color='black', label='Simplex')

        stats.plot_covariance_ellipse([1, 2], P)
Beispiel #4
0
def test_sigma_points_1D():
    """ tests passing 1D data into sigma_points"""

    kappa = 0.
    sp = JulierSigmaPoints(1, kappa)

    #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)

    Wm, Wc = sp.weights()
    assert np.allclose(Wm, Wc, 1e-12)
    assert len(Wm) == 3

    mean = 5
    cov = 9

    Xi = sp.sigma_points(mean, cov)
    xm, ucov = unscented_transform(Xi, Wm, Wc, 0)

    # sum of weights*sigma points should be the original mean
    m = 0.0
    for x, w in zip(Xi, Wm):
        m += x * w

    assert abs(m - mean) < 1.e-12
    assert abs(xm[0] - mean) < 1.e-12
    assert abs(ucov[0, 0] - cov) < 1.e-12

    assert Xi.shape == (3, 1)
Beispiel #5
0
def test_sigma_points_1D():
    """ tests passing 1D data into sigma_points"""

    kappa = 0.
    sp = JulierSigmaPoints(1, kappa)

    #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)

    Wm, Wc = sp.weights()
    assert np.allclose(Wm, Wc, 1e-12)
    assert len(Wm) == 3

    mean = 5
    cov = 9

    Xi = sp.sigma_points (mean, cov)
    xm, ucov = unscented_transform(Xi,Wm, Wc, 0)

    # sum of weights*sigma points should be the original mean
    m = 0.0
    for x, w in zip(Xi, Wm):
        m += x*w

    assert abs(m-mean) < 1.e-12
    assert abs(xm[0] - mean) < 1.e-12
    assert abs(ucov[0,0]-cov) < 1.e-12

    assert Xi.shape == (3,1)
Beispiel #6
0
def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2],
                  [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together

    sp0 = JulierSigmaPoints(n=2, kappa=kappa)
    sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)

    w0, _ = sp0.weights()
    w1, _ = sp1.weights()

    Xi0 = sp0.sigma_points (x, P)
    Xi1 = sp1.sigma_points (x, P)

    assert max(Xi1[:,0]) > max(Xi0[:,0])
    assert max(Xi1[:,1]) > max(Xi0[:,1])
Beispiel #7
0
def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2],
                  [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together

    sp0 = JulierSigmaPoints(n=2, kappa=kappa)
    sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)

    w0, _ = sp0.weights()
    w1, _ = sp1.weights()

    Xi0 = sp0.sigma_points (x, P)
    Xi1 = sp1.sigma_points (x, P)

    assert max(Xi1[:,0]) > max(Xi0[:,0])
    assert max(Xi1[:,1]) > max(Xi0[:,1])

    if DO_PLOT:
        plt.figure()
        for i in range(Xi0.shape[0]):
            plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0],
                        (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1],
                         color='blue')

        for i in range(Xi1.shape[0]):
            plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0],
                        (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1],
                         color='green')

        stats.plot_covariance_ellipse([1, 2], P)