Python filterpy.kalman.KalmanFilter() Examples

def __init__(self,bbox,img=None):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0 

def test_z_checks():
    kf = KalmanFilter(dim_x=3, dim_z=1)
    kf.update(3.)
    kf.update([3])
    kf.update((3))
    kf.update([[3]])
    kf.update(np.array([[3]]))

    try:
        kf.update([[3, 3]])
        assert False, "accepted bad z shape"
    except ValueError:
        pass

    kf = KalmanFilter(dim_x=3, dim_z=2)
    kf.update([3, 4])
    kf.update([[3, 4]])

    kf.update(np.array([[3, 4]]))
    kf.update(np.array([[3, 4]]).T) 

def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4) 
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0 

def test_batch_filter():
    f = KalmanFilter(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.P *= 1000.                  # covariance matrix
    f.R = 5                       # state uncertainty
    f.Q = 0.0001                  # process uncertainty

    zs = [None, 1., 2.]
    m, c, _, _ = f.batch_filter(zs, update_first=False)
    m, c, _, _ = f.batch_filter(zs, update_first=True) 

def test_misshapen():

    """Ensure we get a ValueError if the filter banks are not designed
    properly
    """

    ca = KalmanFilter(3, 1)
    cv = KalmanFilter(2, 1)

    trans = np.array([[0.97, 0.03],
                      [0.03, 0.97]])

    try:
        IMMEstimator([ca, cv], (0.5, 0.5), trans)
        assert "IMM should raise ValueError on filter banks with filters of different sizes"
    except ValueError:
        pass

    try:
        IMMEstimator([], (0.5, 0.5), trans)
        assert "Should raise ValueError on empty bank"
    except ValueError:
        pass 

def const_vel_filter_2d(dt, x_ndim=1, P_diag=(1., 1, 1, 1), R_std=1.,
                        Q_var=0.0001):
    """ helper, constructs 1d, constant velocity filter"""

    kf = KalmanFilter(dim_x=4, dim_z=2)

    kf.x = np.array([[0., 0., 0., 0.]]).T
    kf.P *= np.diag(P_diag)
    kf.F = np.array([[1., dt, 0., 0.],
                     [0., 1., 0., 0.],
                     [0., 0., 1., dt],
                     [0., 0., 0., 1.]])

    kf.H = np.array([[1., 0, 0, 0],
                     [0., 0, 1, 0]])

    kf.R *= np.eye(2) * (R_std**2)
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)
    kf.Q = block_diag(q, q)

    return kf 

def const_vel_filter(dt, x0=0, x_ndim=1, P_diag=(1., 1.), R_std=1.,
                     Q_var=0.0001):
    """ helper, constructs 1d, constant velocity filter"""
    f = KalmanFilter(dim_x=2, dim_z=1)

    if x_ndim == 1:
        f.x = np.array([x0, 0.])
    else:
        f.x = np.array([[x0, 0.]]).T

    f.F = np.array([[1., dt],
                    [0., 1.]])

    f.H = np.array([[1., 0.]])
    f.P = np.diag(P_diag)
    f.R = np.eye(1) * (R_std**2)
    f.Q = Q_discrete_white_noise(2, dt, Q_var)

    return f 

def __init__(self, dim_state, dim_control, dim_measurement, 
                 initial_state_mean, initial_state_covariance, 
                 matrix_F, matrix_B, 
                 process_noise_Q,
                 matrix_H, measurement_noise_R):
        filter = KalmanFilter(dim_x=dim_state, dim_u=dim_control, dim_z=dim_measurement)
        filter.x = initial_state_mean
        filter.P = initial_state_covariance

        filter.Q = process_noise_Q

        filter.F = matrix_F
        filter.B = matrix_B
        filter.H = matrix_H

        filter.R = measurement_noise_R # covariance matrix
        super().__init__(filter) 

def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0 

def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0 

def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0 

def __init__(self, bbox):
        """
        Initialises a tracker using initial bounding box.
        """
        # define constant velocity model
        # x, y,s,a, dy, dy, ds
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        self.kf.F = np.array(
            [[1, 0, 0, 0, 1, 0, 0],
             [0, 1, 0, 0, 0, 1, 0],
             [0, 0, 1, 0, 0, 0, 1],
             [0, 0, 0, 1, 0, 0, 0],
             [0, 0, 0, 0, 1, 0, 0],
             [0, 0, 0, 0, 0, 1, 0],
             [0, 0, 0, 0, 0, 0, 1]])
        self.kf.H = np.array(
            [[1, 0, 0, 0, 0, 0, 0],
             [0, 1, 0, 0, 0, 0, 0],
             [0, 0, 1, 0, 0, 0, 0],
             [0, 0, 0, 1, 0, 0, 0]])

        self.kf.R[2:, 2:] *= 10.
        self.kf.P[
        4:,
        4:] *= 1000.  # give high uncertainty to the unobservable initial
        # velocities
        self.kf.P *= 10.
        self.kf.Q[-1, -1] *= 0.01
        self.kf.Q[4:, 4:] *= 0.01
        self.kf.R *= 0.01

        self.kf.x[:4] = convert_bbox_to_z(bbox)
        self.time_since_update = 0
        self.id = KalmanBoxTracker.count
        KalmanBoxTracker.count += 1
        self.history = []
        self.hits = 0
        self.hit_streak = 0
        self.age = 0
        self.lost = False 

def make_ca_filter(dt, noise_factor):
    cafilter = KalmanFilter(dim_x=3, dim_z=1)
    cafilter.x = array([0., 0., 0.])
    cafilter.P *= 3
    cafilter.R *= noise_factor**2
    cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
    cafilter.F = array([[1, dt, 0.5*dt*dt],
                        [0, 1,         dt],
                        [0, 0,          1]], dtype=float)
    cafilter.H = array([[1, 0, 0]], dtype=float)
    return cafilter 

def make_cv_filter(dt, noise_factor):
    cvfilter = KalmanFilter(dim_x = 2, dim_z=1)
    cvfilter.x = array([0., 0.])
    cvfilter.P *= 3
    cvfilter.R *= noise_factor**2
    cvfilter.F = array([[1, dt],
                        [0,  1]], dtype=float)
    cvfilter.H = array([[1, 0]], dtype=float)
    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
    return cvfilter 

def test_rts():
    fk = KalmanFilter(dim_x=2, dim_z=1)

    fk.x = np.array([-1., 1.])    # initial state (location and velocity)

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

    fk.H = np.array([[1.,0.]])      # Measurement function
    fk.P = .01                     # covariance matrix
    fk.R = 5                       # state uncertainty
    fk.Q = 0.001                   # process uncertainty


    zs = [t + random.randn()*4 for t in range(40)]

    mu, cov, _, _ = fk.batch_filter (zs)
    mus = [x[0] for x in mu]

    M, P, _, _ = fk.rts_smoother(mu, cov)

    if DO_PLOT:
        p1, = plt.plot(zs,'cyan', alpha=0.5)
        p2, = plt.plot(M[:,0],c='b')
        p3, = plt.plot(mus,c='r')
        p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["measurement", "RKS", "KF output", "ideal"], loc=4)

        plt.show() 

def __init__(self):
        # Initialize parameters for tracker
        self.id = 0
        self.num_hits = 0
        self.num_unmatched = 0
        self.box = []

        # Initialize parameters for the Kalman filter
        self.kf = KalmanFilter(dim_x=8, dim_z=8)
        self.dt = 1.0
        self.x_state = []

        # State transition matrix (assuming constant velocity model)
        self.kf.F = np.array([[1, self.dt, 0, 0, 0, 0, 0, 0],
                              [0, 1, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, self.dt, 0, 0, 0, 0],
                              [0, 0, 0, 1, 0, 0, 0, 0],
                              [0, 0, 0, 0, 1, self.dt, 0, 0],
                              [0, 0, 0, 0, 0, 1, 0, 0],
                              [0, 0, 0, 0, 0, 0, 1, self.dt],
                              [0, 0, 0, 0, 0, 0, 0, 1]])

        # Measurement matrix (assuming we can only measure the coordinates)
        self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, 0, 0, 0, 0, 0],
                              [0, 0, 0, 0, 1, 0, 0, 0],
                              [0, 0, 0, 0, 0, 0, 1, 0]])

        # State covariance matrix
        self.kf.P *= 100.0

        # Process uncertainty
        self.Q_comp_mat = np.array([[self.dt ** 4 / 2., self.dt ** 3 / 2.],
                                    [self.dt ** 3 / 2., self.dt ** 2]])
        self.kf.Q = block_diag(self.Q_comp_mat, self.Q_comp_mat,
                               self.Q_comp_mat, self.Q_comp_mat)

        # State uncertainty
        self.kf.R = np.eye(4)*6.25

    # Method: Used to predict and update the next state for a bounding box 

def make_cv_filter(dt, noise_factor):
    cvfilter = KalmanFilter(dim_x = 2, dim_z=1)
    cvfilter.x = array([0., 0.])
    cvfilter.P *= 3
    cvfilter.R *= noise_factor**2
    cvfilter.F = array([[1, dt],
                        [0,  1]], dtype=float)
    cvfilter.H = array([[1, 0]], dtype=float)
    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
    return cvfilter 

def test_procedural_batch_filter():
    f = KalmanFilter(dim_x=2, dim_z=1)

    f.x = np.array([2., 0])

    f.F = np.array([[1., 1.],
                    [0., 1.]])

    f.H = np.array([[1., 0.]])
    f.P = np.eye(2) * 1000.
    f.R = np.eye(1) * 5
    f.Q = Q_discrete_white_noise(2, 1., 0.0001)

    f.test_matrix_dimensions()

    x = np.array([2., 0])

    F = np.array([[1., 1.],
                  [0., 1.]])

    H = np.array([[1., 0.]])
    P = np.eye(2) * 1000.
    R = np.eye(1) * 5
    Q = Q_discrete_white_noise(2, 1., 0.0001)

    zs = [13., None, 1., 2.] * 10
    m, c, _, _ = f.batch_filter(zs, update_first=False)

    n = len(zs)
    mp, cp, _, _ = batch_filter(x, P, zs, [F]*n, [Q]*n, [H]*n, [R]*n)

    for x1, x2 in zip(m, mp):
        assert np.allclose(x1, x2)

    for p1, p2 in zip(c, cp):
        assert np.allclose(p1, p2) 

def test_default_dims():
    kf = KalmanFilter(dim_x=3, dim_z=1)
    kf.predict()
    kf.update(np.array([[1.]]).T) 

def __init__(self, bbox):
        """
        Initialises a tracker using initial bounding box.
        """
        # define constant velocity model
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        self.kf.F = np.array(
            [[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0],
             [0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]])
        self.kf.H = np.array(
            [[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]])

        self.kf.R[2:, 2:] *= 10.
        self.kf.P[4:, 4:] *= 1000.  # give high uncertainty to the unobservable initial velocities
        self.kf.P *= 10.
        self.kf.Q[-1, -1] *= 0.01
        self.kf.Q[4:, 4:] *= 0.01

        self.kf.x[:4] = convert_bbox_to_z(bbox)
        self.time_since_update = 0
        self.id = KalmanBoxTracker.count
        KalmanBoxTracker.count += 1
        self.history = []
        self.hits = 0
        self.hit_streak = 0
        self.age = 0 

def test_equations(self):
        data = Tensor([[-50., 50., 1.]])[:, :, None]

        #
        _design = simple_mv_velocity_design(dims=1)
        torch_kf = KalmanFilter(processes=_design.processes.values(), measures=_design.measures)
        batch_design = torch_kf.design.for_batch(1, 1)
        pred = torch_kf(data)

        #
        filter_kf = filterpy_KalmanFilter(dim_x=2, dim_z=1)
        filter_kf.x = batch_design.initial_mean.detach().numpy().T
        filter_kf.P = batch_design.initial_covariance.detach().numpy().squeeze(0)

        filter_kf.F = batch_design.F(0)[0].detach().numpy()
        filter_kf.H = batch_design.H(0)[0].detach().numpy()
        filter_kf.R = batch_design.R(0)[0].detach().numpy()
        filter_kf.Q = batch_design.Q(0)[0].detach().numpy()
        filter_kf.states = []
        for t in range(data.shape[1]):
            filter_kf.states.append(filter_kf.x)
            filter_kf.update(data[:, t, :])
            filter_kf.predict()
        filterpy_states = np.stack(filter_kf.states).squeeze()
        kf_states = pred.means.detach().numpy().squeeze()

        for r, c in product(*[range(x) for x in kf_states.shape]):
            self.assertAlmostEqual(filterpy_states[r, c], kf_states[r, c], places=3) 

def __init__(self, coord3d):
        """
        Initialises a tracker using initial bounding box.
        """
        # define constant velocity model
        # X,Y,Z, dX, dY, dZ
        self.kf = KalmanFilter(dim_x=6, dim_z=3)
        self.kf.F = np.array(
            [[1, 0, 0, 1, 0, 0],
             [0, 1, 0, 0, 1, 0],
             [0, 0, 1, 0, 0, 1],
             [0, 0, 0, 1, 0, 0],
             [0, 0, 0, 0, 1, 0],
             [0, 0, 0, 0, 0, 1]])
        self.kf.H = np.array(
            [[1, 0, 0, 0, 0, 0],
             [0, 1, 0, 0, 0, 0],
             [0, 0, 1, 0, 0, 0]])

        self.kf.R[2:, 2:] *= 10.
        self.kf.P[
        3:,
        3:] *= 1000.  # give high uncertainty to the unobservable initial
        # velocities
        self.kf.P *= 10.
        self.kf.Q[-1, -1] *= 0.01
        self.kf.Q[3:, 3:] *= 0.01
        self.kf.R *= 0.01

        # X, Y, Z, s (area), r
        self.kf.x[:3] = coord3d.reshape(-1, 1)
        self.time_since_update = 0
        self.id = KalmanBox3dTracker.count
        KalmanBox3dTracker.count += 1
        self.history = [coord3d.reshape(-1, 1)]
        self.hits = 0
        self.hit_streak = 0
        self.age = 0
        self.aff_value = 0
        self.occ = False
        self.lost = False 

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) 

def test_against_kf():
    inv = np.linalg.inv

    dt = 1.0
    IM = np.eye(2)
    Q = np.array([[.25, 0.5], [0.5, 1]])

    F = np.array([[1, dt], [0, 1]])
    #QI = inv(Q)
    P = inv(IM)


    from filterpy.kalman import InformationFilter
    from filterpy.common import Q_discrete_white_noise

    #f = IF2(2, 1)
    r_std = .2
    R = np.array([[r_std*r_std]])
    RI = inv(R)

    '''f.F = F.copy()
    f.H = np.array([[1, 0.]])
    f.RI = RI.copy()
    f.Q = Q.copy()
    f.IM = IM.copy()'''


    kf = KalmanFilter(2, 1)
    kf.F = F.copy()
    kf.H = np.array([[1, 0.]])
    kf.R = R.copy()
    kf.Q = Q.copy()

    f0 = InformationFilter(2, 1)
    f0.F = F.copy()
    f0.H = np.array([[1, 0.]])
    f0.R_inv = RI.copy()
    f0.Q = Q.copy()



    #f.IM = np.zeros((2,2))


    for i in range(1, 50):
        z = i + (np.random.rand() * r_std)
        f0.predict()
        #f.predict()
        kf.predict()


        f0.update(z)
        #f.update(z)
        kf.update(z)

        print(f0.x.T, kf.x.T)
        assert np.allclose(f0.x, kf.x)
        #assert np.allclose(f.x, kf.x) 

def single_measurement_test():
    dt = 0.1
    sigma = 2.

    kf2 = KalmanFilter(dim_x=2, dim_z=1)

    kf2.F = array ([[1., dt], [0., 1.]])
    kf2.H = array ([[1., 0.]])
    kf2.x = array ([[0.], [1.]])
    kf2.Q = array ([[dt**3/3, dt**2/2],
                    [dt**2/2, dt]]) * 0.02
    kf2.P *= 100
    kf2.R[0,0] = sigma**2

    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*sigma
        z = array([[m0]])

        kf2.predict()
        kf2.update(z)

        xs.append(kf2.x.T[0])
        zs.append(z.T[0])
        nom.append(i)

    xs = asarray(xs)
    zs = asarray(zs)
    nom = asarray(nom)


    res = nom-xs[:,0]
    std_dev = np.std(res)
    print('std: {:.3f}'.format (std_dev))

    global DO_PLOT
    if DO_PLOT:

        plt.subplot(211)
        plt.plot(xs[:,0])
        #plt.plot(zs[:,0])


        plt.subplot(212)
        plt.plot(res)
        plt.show()

    return std_dev 

def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):
    dt = 0.1

    kf2 = KalmanFilter(dim_x=2, dim_z=2)

    kf2.F = array ([[1., dt], [0., 1.]])
    kf2.H = array ([[1., 0.], [1., 0.]])
    kf2.x = array ([[0.], [0.]])
    kf2.Q = array ([[dt**3/3, dt**2/2],
                    [dt**2/2, dt]]) * 0.02
    kf2.P *= 100
    kf2.R[0,0] = wheel_sigma**2
    kf2.R[1,1] = gps_sigma**2


    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*wheel_sigma
        m1 = i + randn()*gps_sigma
        if gps_sigma >1e40:
            m1 = -1e40

        z = array([[m0], [m1]])

        kf2.predict()
        kf2.update(z)

        xs.append(kf2.x.T[0])
        zs.append(z.T[0])
        nom.append(i)

    xs = asarray(xs)
    zs = asarray(zs)
    nom = asarray(nom)


    res = nom-xs[:,0]
    std_dev = np.std(res)
    print('fusion std: {:.3f}'.format (np.std(res)))

    if DO_PLOT:

        plt.subplot(211)
        plt.plot(xs[:,0])
        #plt.plot(zs[:,0])
        #plt.plot(zs[:,1])

        plt.subplot(212)
        plt.axhline(0)
        plt.plot(res)
        plt.show()

    print(kf2.Q)
    print(kf2.K)
    return std_dev 

def kf_circle():
    from filterpy.kalman import KalmanFilter
    from math import radians
    import math
    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]])

    def hx_inv(x, y):
        angle = math.atan2(y, x)
        radius = math.sqrt(x*x + y*y)
        return np.array([radius, angle])

    std_noise = .1

    kf = KalmanFilter(dim_x=3, dim_z=2)
    kf.x = np.array([50., 0., 0.])

    F = np.array([[1., 0, 0.],
                  [0., 1., 1.],
                  [0., 0., 1.]])

    kf.F = F
    kf.P *= 100
    kf.H = np.array([[1, 0, 0],
                     [0, 1, 0]])

    kf.R = np.eye(2)*(std_noise**2)
    #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)

    zs = []
    kfxs = []
    for t in range(2000):
        a = t / 30 + 90
        x = cos(radians(a)) * 50. + randn() * std_noise
        y = sin(radians(a)) * 50. + randn() * std_noise

        z = hx_inv(x, y)
        zs.append(z)

        kf.predict()
        kf.update(z)

        # save data
        kfxs.append(kf.x)

    zs = np.asarray(zs)
    kfxs = np.asarray(kfxs)

    if DO_PLOT:
        plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
        plt.plot(kfxs[:, 0], kfxs[:, 1], c='g', label='KF')
        plt.legend(loc='best')
        plt.axis('equal') 

def test_procedure_form():

    dt = 1.
    std_z = 10.1

    x = np.array([[0.], [0.]])
    F = np.array([[1., dt], [0., 1.]])
    H = np.array([[1., 0.]])
    P = np.eye(2)
    R = np.eye(1)*std_z**2
    Q = Q_discrete_white_noise(2, dt, 5.1)

    kf = KalmanFilter(2, 1)
    kf.x = x.copy()
    kf.F = F.copy()
    kf.H = H.copy()
    kf.P = P.copy()
    kf.R = R.copy()
    kf.Q = Q.copy()

    measurements = []
    xest = []
    pos = 0.
    for t in range(2000):
        z = pos + random.randn() * std_z
        pos += 100

        # perform kalman filtering
        x, P = predict(x, P, F, Q)
        kf.predict()
        assert norm(x - kf.x) < 1.e-12
        x, P, _, _, _, _ = update(x, P, z, R, H, True)
        kf.update(z)
        assert norm(x - kf.x) < 1.e-12

        # save data
        xest.append(x.copy())
        measurements.append(z)

    xest = np.asarray(xest)
    measurements = np.asarray(measurements)
    # plot data
    if DO_PLOT:
        plt.plot(xest[:, 0])
        plt.plot(xest[:, 1])
        plt.plot(measurements) 

def test_noisy_11d():
    f = KalmanFilter(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.P *= 1000.                  # covariance matrix
    f.R = 5                       # state uncertainty
    f.Q = 0.0001                  # process uncertainty

    measurements = []
    results = []

    zs = []
    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()

        # save data
        results.append(f.x[0])
        measurements.append(z)

        # test mahalanobis
        a = np.zeros(f.y.shape)
        maha = scipy_mahalanobis(a, f.y, f.SI)
        assert f.mahalanobis == approx(maha)

    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2., 0]]).T
    f.P = np.eye(2) * 100.
    m, c, _, _ = f.batch_filter(zs, update_first=False)

    # plot data
    if DO_PLOT:
        p1, = plt.plot(measurements, 'r', alpha=0.5)
        p2, = plt.plot(results, 'b')
        p4, = plt.plot(m[:, 0], 'm')
        p3, = plt.plot([0, 100], [0, 100], 'g')  # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["noisy measurement", "KF output", "ideal", "batch"], loc=4)

        plt.show() 

def test_noisy_1d():
    f = KalmanFilter(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.P *= 1000.                  # covariance matrix
    f.R = 5                       # state uncertainty
    f.Q = 0.0001                  # process uncertainty

    measurements = []
    results = []

    zs = []
    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()

        # save data
        results.append(f.x[0, 0])
        measurements.append(z)

        # test mahalanobis
        a = np.zeros(f.y.shape)
        maha = scipy_mahalanobis(a, f.y, f.SI)
        assert f.mahalanobis == approx(maha)


    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2., 0]]).T
    f.P = np.eye(2) * 100.
    s = Saver(f)
    m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
    s.to_array()
    assert len(s.x) == len(zs)
    assert len(s.x) == len(s)

    # plot data
    if DO_PLOT:
        p1, = plt.plot(measurements, 'r', alpha=0.5)
        p2, = plt.plot(results, 'b')
        p4, = plt.plot(m[:, 0], 'm')
        p3, = plt.plot([0, 100], [0, 100], 'g')  # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["noisy measurement", "KF output", "ideal", "batch"], loc=4)
        plt.show()