本文整理汇总了Python中filterpy.kalman.KalmanFilter.B方法的典型用法代码示例。如果您正苦于以下问题：Python KalmanFilter.B方法的具体用法？Python KalmanFilter.B怎么用？Python KalmanFilter.B使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类filterpy.kalman.KalmanFilter的用法示例。
在下文中一共展示了KalmanFilter.B方法的7个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: ball_filter6
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [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示例2: ball_filter4
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [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示例3: createLegKF
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [as 别名]
def createLegKF(x, y):
# kalman_filter = KalmanFilter(dim_x=4, dim_z=2)
kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
KF_F = np.array([[1.0, dt, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, dt], [0, 0, 0, 1.0]])
KF_q = 0.7 # 0.3
KF_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q), np.zeros((2, 2)))),
np.hstack((np.zeros((2, 2)), Q_discrete_white_noise(2, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_P = np.diag([KF_pd, KF_pv, KF_pd, KF_pv])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
# KF_R = np.diag([KF_rd,KF_rd])
KF_R = np.diag([KF_rd, KF_rd, KF_rv, KF_rv])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
KF_H = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0], [0, 1.0, 0, 0], [0, 0, 0, 1.0]])
kalman_filter.x = np.array([x, 0, y, 0])
kalman_filter.F = KF_F
kalman_filter.H = KF_H
kalman_filter.Q = KF_Q
kalman_filter.B = 0
kalman_filter.R = KF_R
kalman_filter.P = KF_P
return kalman_filter示例4: template
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [as 别名]
def template():
kf = KalmanFilter(dim_x=2, dim_z=1)
# x0
kf.x = np.array([[.0], [.0]])
# P - change over time
kf.P = np.diag([500., 49.])
# F - state transition matrix
dt = .1
kf.F = np.array([[1, dt], [0, 1]])
## now can predict
## дисперсия растет и становится видна корреляция
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='r')
#kf.predict()
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='k', ls='dashed')
#show()
# Control information
kf.B = 0
# !!Attention!! Q! Process noise
kf.Q = Q_discrete_white_noise( dim=2, dt=dt, var=2.35)
# H - measurement function
kf.H = np.array([[1., 0.]])
# R - measure noise matrix
kf.R = np.array([[5.]])示例5: test_univariate
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [as 别名]
def test_univariate():
f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
f.x = np.array([[0]])
f.P *= 50
f.H = np.array([[1.]])
f.F = np.array([[1.]])
f.B = np.array([[1.]])
f.Q = .02
f.R *= .1
for i in range(50):
f.predict()
f.update(i)示例6: createPersonKF
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [as 别名]
def createPersonKF(x, y):
kalman_filter = KalmanFilter(dim_x=6, dim_z=2)
# kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
dt2 = 0.005
# KF_F = np.array([[1., dt, 0 , 0],
# [0 , 1., 0 , 0],
# [0 , 0 , 1., dt],
# [0 , 0 , 0 , 1.]])
KF_Fca = np.array([[1.0, dt, dt2 / 2], [0, 1.0, dt], [0, 0, 1]])
KF_F = np.vstack(
(np.hstack((KF_Fca, np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), KF_Fca))) # np.zeros((3,3)))),
) # , np.zeros((3,3)))) )))#,
# np.hstack((np.zeros((3,3)),np.zeros((3,3)), KF_Fca))))
KF_q = 0.7 # 0.3
# KF_Q = np.vstack((np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q),np.zeros((2,2)))),np.hstack((np.zeros((2,2)),Q_discrete_white_noise(2, dt=0.1, var=KF_q)))))
KF_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(3, dt=0.1, var=KF_q), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), Q_discrete_white_noise(3, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_pa = 30.0
KF_P = np.diag([KF_pd, KF_pv, KF_pa, KF_pd, KF_pv, KF_pa])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
KF_ra = 2 # 0.5
KF_R = np.diag([KF_rd, KF_rd])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv, KF_ra, KF_ra])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0],[0,1.,0,0],[0,0,0,1.]])
KF_H = np.array([[1.0, 0, 0, 0, 0, 0], [0, 0, 0, 1.0, 0, 0]])
# kalman_filter.x = np.array([x,0,y,0])
kalman_filter.x = np.array([x, 0, 0, y, 0, 0])
kalman_filter.F = KF_F
kalman_filter.H = KF_H
kalman_filter.Q = KF_Q
kalman_filter.B = 0
kalman_filter.R = KF_R
kalman_filter.P = KF_P
return kalman_filter示例7: KalmanFilter
# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import B [as 别名]
plt.xlabel('x')
plt.ylabel('y')
f2.show()
# Basic Kalman filter
# https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
# See Chapter 08
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = times[1] - times[0]
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
tracker.Q = block_diag(q, q)
tracker.B = 0
tracker.H = np.array([[1.0, 0, 0, 0],
[0 , 0, 1, 0]])
tracker.R = np.array([[0.1, 0],
[0, 0.1]])
tracker.x = np.array([[x, y, 0, 0]]).T
tracker.P = np.eye(4)*10.
# Convert measurements to expected form
# Note: n x 4 x 1 np array, measurements must be a col vector
measurements = np.array([np.array([pos]).T for pos in positions])
mu, cov, _, _ = tracker.batch_filter(measurements)
f3 = plt.figure()
plt.scatter(xs, ys)