Пример #1
0
def test_1d_0P():
    global inf
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = (np.array([[1., 1.],
                     [0., 1.]]))    # state transition matrix

    f.H = np.array([[1., 0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1.,0.]])    # Measurement function
    inf.R_inv *= 1./5                 # state uncertainty
    inf.Q = np.eye(2)*0.0001
    inf.P_inv = 0.000000000000000000001
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (50):
        # create measurement = t plus white noise
        z = t + random.randn()* np.sqrt(5)
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        try:
            print(t, inf.P)
        except:
            pass

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

    #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #2
0
def test_1d():
    f = KalmanFilter(dim_x=2, dim_z=1)
    inf = InformationFilter(dim_x=2, dim_z=1)

    # ensure __repr__ doesn't assert
    str(inf)


    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    inf.x = f.x.copy()
    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    inf.F = f.F.copy()
    f.H = np.array([[1.,0.]])      # Measurement function
    inf.H = np.array([[1.,0.]])    # Measurement function
    f.R *= 5                       # state uncertainty
    inf.R_inv *= 1./5               # state uncertainty
    f.Q *= 0.0001                  # process uncertainty
    inf.Q *= 0.0001


    m = []
    r = []
    r2 = []
    zs = []
    s = Saver(inf)
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        inf.update(z)
        inf.predict()

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)
        print(inf.y)
        print(inf.SI)
        s.save()

        assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12


    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #3
0
def test_1d():
    global inf
    f = KalmanFilter(dim_x=2, dim_z=1)
    inf = InformationFilter(dim_x=2, dim_z=1)

    # ensure __repr__ doesn't assert
    str(inf)


    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    inf.x = f.x.copy()
    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    inf.F = f.F.copy()
    f.H = np.array([[1.,0.]])      # Measurement function
    inf.H = np.array([[1.,0.]])    # Measurement function
    f.R *= 5                       # state uncertainty
    inf.R_inv *= 1./5               # state uncertainty
    f.Q *= 0.0001                  # process uncertainty
    inf.Q *= 0.0001


    m = []
    r = []
    r2 = []
    zs = []
    s = Saver(inf)
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.update(z)
        f.predict()

        inf.update(z)
        inf.predict()

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)
        print(inf.y)
        s.save()

        assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #4
0
def test_1d_0P():
    global inf
    f = KalmanFilter(dim_x=2, dim_z=1)
    inf = InformationFilter(dim_x=2, dim_z=1)

    f.x = np.array([[2.], [0.]])  # initial state (location and velocity)

    f.F = (np.array([[1., 1.], [0., 1.]]))  # state transition matrix

    f.H = np.array([[1., 0.]])  # Measurement function
    f.R = np.array([[5.]])  # state uncertainty
    f.Q = np.eye(2) * 0.0001  # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1., 0.]])  # Measurement function
    inf.R_inv *= 1. / 5  # state uncertainty
    inf.Q = np.eye(2) * 0.0001
    inf.P_inv = 0.000000000000000000001
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []

    zs = []
    for t in range(50):
        # create measurement = t plus white noise
        z = t + random.randn() * np.sqrt(5)
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        try:
            print(t, inf.P)
        except:
            pass

        # save data
        r.append(f.x[0, 0])
        r2.append(inf.x[0, 0])
        m.append(z)

    #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #5
0
def test_1d_0P():
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    f.H = np.array([[1.,0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1.,0.]])    # Measurement function
    inf.R_inv *= 1./5                 # state uncertainty
    inf.Q *= 0.0001
    inf.P_inv = 0
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

        #assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #6
0
def test_1d_0P():
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    f.H = np.array([[1.,0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

    inf.x = f.x.copy()
    inf.F = f.F.copy()
    inf.H = np.array([[1.,0.]])    # Measurement function
    inf.R_inv *= 1./5                 # state uncertainty
    inf.Q *= 0.0001
    inf.P_inv = 0
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

        #assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)
Пример #7
0
def test_1d_0P():
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    inf.x = f.x.copy()
    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    inf.F = f.F.copy()
    f.H = np.array([[1.,0.]])    # Measurement function
    inf.H = np.array([[1.,0.]])    # Measurement function
    f.R = 5.                 # state uncertainty
    inf.R_inv = 1./5                 # state uncertainty
    f.Q = 0.0001                 # process uncertainty
    inf.Q = 0.0001
    f.P *= 20
    inf.P_inv = 0
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)