Python KalmanFilter.u方法代码示例

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def ball_filter6(dt,R=1., Q = 0.1):
    f1 = KalmanFilter(dim=6)
    g = 10

    f1.F = np.mat ([[1., dt, dt**2,  0,       0,  0],
                    [0,  1., dt,     0,       0,  0],
                    [0,  0,  1.,     0,       0,  0],
                    [0,  0,  0.,    1., dt, -0.5*dt*dt*g],
                    [0,  0,  0,      0, 1.,      -g*dt],
                    [0,  0,  0,      0, 0.,      1.]])

    f1.H = np.mat([[1,0,0,0,0,0],
                   [0,0,0,0,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,0]])


    f1.R = np.mat(np.eye(6)) * R

    f1.Q = np.zeros((6,6))
    f1.Q[2,2] = Q
    f1.Q[5,5] = Q
    f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
    f1.P = np.eye(6) * 50.
    f1.B = 0.
    f1.u = 0

    return f1

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def ball_filter4(dt,R=1., Q = 0.1):
    f1 = KalmanFilter(dim=4)
    g = 10

    f1.F = np.mat ([[1., dt,  0, 0,],
                    [0,  1.,  0, 0],
                    [0,  0,  1., dt],
                    [0,  0,  0.,  1.]])

    f1.H = np.mat([[1,0,0,0],
                   [0,0,0,0],
                   [0,0,1,0],
                   [0,0,0,0]])



    f1.B = np.mat([[0,0,0,0],
                   [0,0,0,0],
                   [0,0,1.,0],
                   [0,0,0,1.]])

    f1.u = np.mat([[0],
                   [0],
                   [-0.5*g*dt**2],
                   [-g*dt]])

    f1.R = np.mat(np.eye(4)) * R

    f1.Q = np.zeros((4,4))
    f1.Q[1,1] = Q
    f1.Q[3,3] = Q
    f1.x = np.mat([0, 0 , 0, 0]).T
    f1.P = np.eye(4) * 50.
    return f1

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def rot_box_kalman_filter(initial_state, Q_std, R_std):
    """
    Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
    position, centroid velocity, dimensions, and rotation angle.

    Parameters
    ----------
    initial_state : sequence of floats
        [x, vx, y, vy, w, h, phi]
    Q_std : float
        Standard deviation to use for process noise covariance matrix
    R_std : float
        Standard deviation to use for measurement noise covariance matrix

    Returns
    -------
    kf : filterpy.kalman.KalmanFilter instance
    """
    kf = KalmanFilter(dim_x=7, dim_z=5)
    dt = 1.0   # time step

    # state mean and covariance
    kf.x = np.array([initial_state]).T
    kf.P = np.eye(kf.dim_x) * 500.

    # no control inputs
    kf.u = 0.

    # state transition matrix
    kf.F = np.eye(kf.dim_x)
    kf.F[0, 1] = kf.F[2, 3] = dt

    # measurement matrix - maps from state space to observation space, so
    # shape is dim_z x dim_x.
    kf.H = np.zeros([kf.dim_z, kf.dim_x])

    # z = Hx. H has nonzero coefficients for the following components of kf.x:
    #   x            y            w            h           phi
    kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0

    # measurement noise covariance
    kf.R = np.eye(kf.dim_z) * R_std**2

    # process noise covariance for x-vx or y-vy pairs
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)

    # diagonal process noise sub-matrix for width, height, and phi
    qq = Q_std**2*np.eye(3)

    # process noise covariance matrix for full state
    kf.Q = block_diag(q, q, qq)

    return kf

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def point_2d_kalman_filter(initial_state, Q_std, R_std):
    """
    Parameters
    ----------
    initial_state : sequence of floats
        [x0, vx0, y0, vy0]
    Q_std : float
        Standard deviation to use for process noise covariance matrix
    R_std : float
        Standard deviation to use for measurement noise covariance matrix

    Returns
    -------
    kf : filterpy.kalman.KalmanFilter instance
    """
    kf = KalmanFilter(dim_x=4, dim_z=2)
    dt = 1.0   # time step

    # state mean (x, vx, y, vy) and covariance
    kf.x = np.array([initial_state]).T
    kf.P = np.eye(kf.dim_x) * 500.

    # no control inputs
    kf.u = 0.

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

    # measurement matrix - maps from state space to observation space
    kf.H = np.array([[1, 0, 0, 0],
                     [0, 0, 1, 0]])

    # measurement noise covariance
    kf.R = np.eye(kf.dim_z) * R_std**2

    # process noise covariance
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
    kf.Q = block_diag(q, q)

    return kf

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def init_filter(dt):

    states = 18 # number of states, first and second order derivatives
    obs = 6 # observations

    filter = KalmanFilter(dim_x=states,dim_z=obs)

    filter.x = np.zeros([1,states]).T

    transition_mat = np.eye(9)

    for i in range(0,9):

        id_vel = i + 3
        id_acc = i + 6

        if id_vel < 9:
            transition_mat[i,id_vel] = dt
        if id_acc < 9:
            transition_mat[i,id_acc] = 0.5 * dt * dt

    zero_mat = np.zeros([9,9])

    tmp1 = np.hstack([transition_mat,zero_mat])
    tmp2 = np.hstack([zero_mat,transition_mat])

    filter.F = np.vstack([tmp1,tmp2])

    filter.H = np.zeros([obs,states])
    filter.H[0,0] = 1
    filter.H[1,1] = 1
    filter.H[2,2] = 1
    filter.H[3,9] = 1
    filter.H[4,10] = 1
    filter.H[5,11] = 1

    filter.Q = np.eye(states) * 1e-4; # process noise
    filter.R = np.eye(obs) * 0.01 # measurement noise
    filter.P = np.eye(states) * 1e-4 # covariance post
    filter.u = 0.

    return filter

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
def init_kalman():

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

    dt = 0.01

    filter.x = np.array([[0,0,0,0]]).T
    filter.F = np.array([[1,0,dt,0],
                        [0,1,0,dt],
                        [0,0,1,0],
                        [0,0,0,1]])
    #q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
    #filter.Q = block_diag(q, q)
    filter.Q = np.eye(4) * 1e-4; # process noise
    print(filter.Q)
    filter.H = np.array([[1, 0, 0,        0],
                      [0,        1, 0, 0]])
    filter.R = np.array([[0.1, 0],[0, 0.1]]) # measurement noise
    filter.P = np.eye(4) * 1e-4 # covariance post
    filter.u = 0.

    return filter

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import u [as 别名]
        
        self.cnt += 1
        
        return [self.pos[0] + random.randn() * self.noise_scale,
                self.pos[1] + random.randn() * self.noise_scale]
    
    def getPos(self):
        return [self.pos[0], self.pos[1]]

tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.
tracker.F = np.array([[1, dt, 0, 0],
                      [0,  1, 0, 0],
                      [0,  0, 1, dt],
                      [0,  0, 0, 1]])
tracker.u = 0.
tracker.H = np.array([[1/0.3084, 0, 0, 0],
                      [0, 0, 1/0.3084, 0]])
tracker.R = np.array([[5,0],
                      [0,5]])
tracker.Q = np.eye(4) * 0.1
tracker.P = np.eye(4) * 500

count = 80
tx, ty = [],[]
xs, ys = [],[]
pxs, pys = [],[]
sensor = PosSensor1([0,0], (2,2), 1)

for i in range(count):
    tp = sensor.getPos()