Python kalman.KalmanFilter方法代码示例

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
def test_default_dims():
    kf = KalmanFilter(dim_x=3, dim_z=1)
    kf.predict()
    kf.update(np.array([[1.]]).T) 

# 需要导入模块: from filterpy import kalman [as 别名]
# 或者: from filterpy.kalman import KalmanFilter [as 别名]
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