class KFMotionModel(object):
def __init__(self,bb):
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.
self.kf.P *= 10.
self.kf.Q[-1,-1] *= 0.01
self.kf.Q[4:,4:] *= 0.01
self.kf.x[:4,0] = np.array(bb2z(bb))
self.history = []
self.predicted = False
def update(self,bb):
self.history = []
bb = np.array(bb2z(bb))
bb = np.expand_dims(bb, axis=1)
self.kf.update(bb)
self.predicted = False
def predict(self):
if not self.predicted:
if((self.kf.x[6]+self.kf.x[2])<=0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.history.append(z2bb(self.kf.x))
self.predicted=True
return self.history[-1]
def get_state(self):
return z2bb(self.kf.x)
def create_kalman_filter(self, det):
"""(x, y, s(area), r(aspect ratio), x', y', s')
"""
model = KalmanFilter(dim_x=7, dim_z=4)
model.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],
], 'float32')
model.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],
], 'float32')
model.R[2:,2:] *= 10.
model.P[4:,4:] *= 1000. # high uncertainty of initial volocity
model.P *= 10.
model.Q[-1,-1] *= 0.01
model.Q[4:,4:] *= 0.01
model.x[:4] = np.array(xywh_to_xysr(*det), 'float32').reshape(4, 1)
return model
def _KF_init(self): # para: Center of box used for prediction
KF = KalmanFilter(4,2)
# KF.x = location + [0,0,0,0]
# KF.F = np.array([
# [1,0,0,0,1,0,0,0],
# [0,1,0,0,0,1,0,0],
# [0,0,1,0,0,0,1,0],
# [0,0,0,1,0,0,0,1],
# [0,0,0,0,1,0,0,0],
# [0,0,0,0,0,1,0,0],
# [0,0,0,0,0,0,1,0],
# [0,0,0,0,0,0,0,1]])
# KF.H = np.array([
# [1,0,0,0,0,0,0,0],
# [0,1,0,0,0,0,0,0],
# [0,0,1,0,0,0,0,0],
# [0,0,0,1,0,0,0,0]])
KF.x = self.KF_center + [0,0] # can be improved for accuracy e.g. from which edge
KF.F = np.array([
[1,0,1,0],
[0,1,0,1],
[0,0,1,0],
[0,0,0,1]])
KF.H = np.array([
[1,0,0,0],
[0,1,0,0]])
KF.P *= 100
KF.R *= 100
# KF.Q *= 2
# KF.predict()
return KF
def __init__(self, dt, ID, position_x, position_y):
self.p_x = KalmanFilter(dim_x=3, dim_z=1)
self.p_y = KalmanFilter(dim_x=3, dim_z=1)
self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
self.p_x.H = np.array([[1, 0., 0.]])
self.p_y.H = np.array([[1, 0., 0.]])
self.R_x_std = 0.01 # update to the correct value
self.Q_x_std = 7 # update to the correct value
self.R_y_std = 0.01 # update to the correct value
self.Q_y_std = 7 # update to the correct value
self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2)
self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2)
self.p_x.R *= self.R_x_std ** 2
self.dt = dt
self.ID = ID
self.p_x.x = np.array([[position_x], [0.], [0.]])
self.p_y.x = np.array([[position_y], [0.], [0.]])
self.p_x.P *= 100. # can very likely be set to 100.
self.p_y.P *= 100. # can very likely be set to 100.
self.time_since_last_update = 0.0
self.p_y.R *= self.R_y_std ** 2
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 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 update(self,bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if((self.kf.x[6]+self.kf.x[2])<=0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if(self.time_since_update>0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
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 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 ZeroOrderKF(R, Q, P=20):
""" Create zero order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=1, dim_z=1)
kf.x = np.array([0.])
kf.R *= R
kf.Q *= Q
kf.P *= P
kf.F = np.eye(1)
kf.H = np.eye(1)
return kf
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 (0,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)
def FirstOrderKF(R, Q, dt):
""" Create first order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.zeros(2)
kf.P *= np.array([[100, 0], [0, 1]])
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, Q)
kf.F = np.array([[1., dt],
[0., 1]])
kf.H = np.array([[1., 0]])
return kf
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)
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)
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 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)
#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)
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.]])
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
def __init__(self,bbox):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=8, dim_z=4)
self.kf.F = np.array([[1,0,0,0,1,0,0.5,0],[0,1,0,0,0,1,0,0.5],[0,0,1,0,0,0,0,0],[0,0,0,1,0,0,0,0],[0,0,0,0,1,0,1,0],[0,0,0,0,0,1,0,1],[0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,1]])
self.kf.H = np.array([[1,0,0,0,0,0,0,0],[0,1,0,0,0,0,0,0],[0,0,1,0,0,0,0,0],[0,0,0,1,0,0,0,0]])
self.kf.R[:,:] *= 25.0 # set measurement uncertainty for positions
self.kf.Q[:2,:2] = 0.0 # process uncertainty for positions is zero - only moves due to velocity, leave process for width height as 1 to account for turning
self.kf.Q[2:4,2:4] *= 0.1 # process uncertainty for width/height for turning
self.kf.Q[4:6,4:6] = 0.0 # process uncertainty for velocities is zeros - only accelerates due to accelerations
self.kf.Q[6:,6:] *= 0.01 # process uncertainty for acceleration
self.kf.P[4:,4:] *= 15.0 # maximum speed
z=convert_bbox_to_kfx(bbox)
self.kf.x[:4] = z
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.hits = 1
self.hit_streak = 1
self.age = 1
self.score = bbox[4]
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.
# Give high uncertainty to the unobservable initial velocities.
self.kf.P[4:, 4:] *= 1000.
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
class CarTracker:
def __init__(self, dt, ID, position_x, position_y):
self.p_x = KalmanFilter(dim_x=3, dim_z=1)
self.p_y = KalmanFilter(dim_x=3, dim_z=1)
self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
self.p_x.H = np.array([[1, 0., 0.]])
self.p_y.H = np.array([[1, 0., 0.]])
self.R_x_std = 0.01 # update to the correct value
self.Q_x_std = 7 # update to the correct value
self.R_y_std = 0.01 # update to the correct value
self.Q_y_std = 7 # update to the correct value
self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2)
self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2)
self.p_x.R *= self.R_x_std ** 2
self.dt = dt
self.ID = ID
self.p_x.x = np.array([[position_x], [0.], [0.]])
self.p_y.x = np.array([[position_y], [0.], [0.]])
self.p_x.P *= 100. # can very likely be set to 100.
self.p_y.P *= 100. # can very likely be set to 100.
self.time_since_last_update = 0.0
self.p_y.R *= self.R_y_std ** 2
def update_pose(self,position_x, position_y):
self.time_since_last_update = 0.0 # reset time since last update
self.p_x.update([[position_x]])
self.p_y.update([[position_y]])
def predict_pose(self):
self.time_since_last_update += self.dt
self.p_x.predict()
self.p_y.predict()
def get_last_update_time(self):
return self.time_since_last_update
def get_position(self):
return [self.p_x.x[0], self.p_y.x[0]]
def get_current_error(self):
return [(self.p_x.P[0])[0], (self.p_y.P[0])[0]]
def get_total_speed(self):
# calculate magnitude of speed
return np.sqrt(self.p_x.x[1]**2+self.p_y.x[1]**2)
def get_ID(self):
return self.ID
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 = KalmanFilter(dim_x=8, dim_z=4)
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0, 0],
[0, 1, 0, 0, 0, 1, 0, 0],
[0, 0, 1, 0, 0, 0, 1, 0],
[0, 0, 0, 1, 0, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1]])
self.kf.H = np.array(
[[1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0]])
# self.kf.R[2:, 2:] *= 10.
self.kf.R[:, :] *= FLAGS.measurement_noise
# Give high uncertainty to the unobservable initial velocities.
self.kf.P[4:, 4:] *= 10000.
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
# @mhsung
self.bbox_idx = -1
self.class_idx = -1
# bbox: [x0, y0, x1, y2, bbox_idx, class_index, ...]
# Store bounding box index and class index if given.
if len(bbox) >= 5:
self.bbox_idx = bbox[4]
if len(bbox) >= 6:
self.class_idx = bbox[5]
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)
def tracker1():
#
#
# Design 2D filter
#
#
# 1. Choose state vars - x
# 2. Design state trans. Function - F
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.
# time step 1 second
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
# 3. Design Process Noise Mat - Q
v = 0.05
q = Q_discrete_white_noise(dim=2, dt=dt, var=v)
tracker.Q = block_diag(q, q)
# 4. B
# 5. Design measurement function
tracker.H = np.array([[1/0.3048, 0, 0, 0],
[0, 0, 1/0.3048, 0]])
# 6. Design Meas. Noise Mat - R
tracker.R = np.array([[5, 0],[0, 5]])
# Init conditions
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
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
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 test_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2.0, 0]) # initial state (location and velocity)
f.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
f.H = np.array([[1.0, 0.0]]) # Measurement function
f.P *= 1000.0 # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
zs = [None, 1.0, 2.0]
m, c, _, _ = f.batch_filter(zs, update_first=False)
m, c, _, _ = f.batch_filter(zs, update_first=True)
def __init__(self,bb):
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.
self.kf.P *= 10.
self.kf.Q[-1,-1] *= 0.01
self.kf.Q[4:,4:] *= 0.01
self.kf.x[:4,0] = np.array(bb2z(bb))
self.history = []
self.predicted = False
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)
# test both list of matrices, and single matrix forms
mp, cp, _, _ = batch_filter(x, P, zs, F, Q, [H]*n, R)
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 dog_tracking_filter(R,Q=0,cov=1.):
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.matrix([[0], [0]]) # initial state (location and velocity)
f.F = np.matrix([[1,1],[0,1]]) # state transition matrix
f.H = np.matrix([[1,0]]) # Measurement function
f.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
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
def __init__(self, bbox3D, info):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=10, dim_z=7)
self.kf.F = np.array([
[1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix
[0, 1, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
])
self.kf.H = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function,
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0]
])
# with angular velocity
# self.kf = KalmanFilter(dim_x=11, dim_z=7)
# self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix
# [0,1,0,0,0,0,0,0,1,0,0],
# [0,0,1,0,0,0,0,0,0,1,0],
# [0,0,0,1,0,0,0,0,0,0,1],
# [0,0,0,0,1,0,0,0,0,0,0],
# [0,0,0,0,0,1,0,0,0,0,0],
# [0,0,0,0,0,0,1,0,0,0,0],
# [0,0,0,0,0,0,0,1,0,0,0],
# [0,0,0,0,0,0,0,0,1,0,0],
# [0,0,0,0,0,0,0,0,0,1,0],
# [0,0,0,0,0,0,0,0,0,0,1]])
# self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function,
# [0,1,0,0,0,0,0,0,0,0,0],
# [0,0,1,0,0,0,0,0,0,0,0],
# [0,0,0,1,0,0,0,0,0,0,0],
# [0,0,0,0,1,0,0,0,0,0,0],
# [0,0,0,0,0,1,0,0,0,0,0],
# [0,0,0,0,0,0,1,0,0,0,0]])
# self.kf.R[0:,0:] *= 10. # measurement uncertainty
self.kf.P[
7:,
7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
self.kf.P *= 10.
# self.kf.Q[-1,-1] *= 0.01 # process uncertainty
self.kf.Q[7:, 7:] *= 0.01
self.kf.x[:7] = bbox3D.reshape((7, 1))
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 1 # number of total hits including the first detection
self.hit_streak = 1 # number of continuing hit considering the first detection
self.first_continuing_hit = 1
self.still_first = True
self.age = 0
self.info = info # other info
class Localization:
""" localization module for Boat-Udrone system """
rospy.init_node('LocalizationUdrone', anonymous=True)
last_t = rospy.Time.now()
def __init__(self):
rospy.Subscriber('/aruco_single/pose', PoseStamped, callback=self.aruco_pose_subscriber)
self.observation = None
self.listener = tf.TransformListener()
# Kalman filter definition
self.kf = KalmanFilter(dim_x=3, dim_z=1)
self.dt = 0.01
self.kf.x = np.array([[0.5],
[0.5],
[0.5]])
self.kf.P = np.array([[2, 0, 0],
[0, 1, 0],
[0, 0, 1]])
self.kf.H = np.array([[1., 0, 0]])
self.kf.R = np.eye(1) * 1.5
self.kf.Q = np.eye(3) * 0.002
self.kf.F = np.array([[1., self.dt, -self.dt],
[0., 1., 0.],
[0, 0, 1.]])
# Variables to maintain kalman update only on new aruco observation
self.aruco_previous_timestamp = rospy.Time.now()
self.aruco_pose_timestamp = rospy.Time.now()
self.is_observation = False
# Plots to stop_count iterations
self.plot = True
self.stop_count = 100 # 5000, means 50 sec if dt is 0.01
self.count = 0
if self.plot:
self.save_time = np.empty((0, 1)) # n x timestamps
self.save_observation = np.empty((0, 7)) # n x 7, where 7 values are x, y, z and quaternion
self.save_ground_truth = np.empty((0, 7)) # n x 7, where 7 values are x, y, z and quaternion
self.save_estimate = np.empty((0, 3)) # n x 3, where 2 values are the state estimates x and xdot
self.save_observation_time = np.empty((0, 1)) # n x timestamps
self.run_filter()
self.save_data_to_file()
self.plot_fusion()
def save_data_to_file(self):
np.save('logs/kf_with_boat_ground_truth.npy', self.save_ground_truth)
np.save('logs/kf_with_boat_observations.npy', self.save_observation)
np.save('logs/kf_with_boat_observation_time.npy', self.save_observation_time)
np.save('logs/kf_with_boat_estimates.npy', self.save_estimate)
def aruco_pose_subscriber(self, msg):
""" This subscriber publishes the timestamp of the aruco detection.
This will make sure the rate of Kalman update is synchronized to image detections
"""
self.aruco_pose_timestamp = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs)
def run_filter(self):
rate = rospy.Rate(100.0)
# print(kf.x)
while not rospy.is_shutdown():
try:
# (trans, rot) = listener.lookupTransform('to', 'from', rospy.Time(0))
# timestamp is used to make sure update happens only after an observation
if (self.aruco_pose_timestamp - self.aruco_previous_timestamp) > rospy.Duration(0):
self.is_observation = True
(trans, rot) = self.listener.lookupTransform('/udrone', '/world', self.aruco_pose_timestamp)
self.aruco_previous_timestamp = self.aruco_pose_timestamp
print("Aruco Observation: ", trans)
if self.plot:
(gtrans, grot) = self.listener.lookupTransform('/ground_truth_udrone',
'/ground_truth_heron',
rospy.Time(0))
print("Ground Truth: ", gtrans)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as Ex:
print(Ex)
rate.sleep()
continue
self.kf.predict()
print("Predicted pos x: ", self.kf.x[0][0])
# Do kalman update call only when the camera detects an aruco marker
if self.is_observation:
self.kf.update(np.array([trans[0]]))
self.is_observation = False
if self.plot:
self.save_observation_time = np.append(self.save_observation_time,
np.array([[self.count]]),
axis=0)
self.save_observation = np.append(self.save_observation,
np.array([trans + rot]),
axis=0)
print("Updated pos x : ", self.kf.x[0][0])
if self.plot:
self.count += 1
self.save_time = np.append(self.save_time, np.array([[self.count]]), axis=0)
self.save_estimate = np.append(self.save_estimate, np.array([self.kf.x.T.flatten()]), axis=0)
self.save_ground_truth = np.append(self.save_ground_truth,
np.array([gtrans + grot]),
axis=0)
print(self.count)
if self.count > self.stop_count:
break
rate.sleep()
def plot_fusion(self, dim=2, axis=0):
if dim == 2:
# 2D plots
ax = plt.subplot(111)
if self.save_observation_time.shape[0] > 0:
ax.scatter(self.save_observation_time, self.save_observation[:, axis], marker='o', c="green",
label='Sensor Observation')
# 0 is x-axis, 1 is x-axis vel
# 2 is y-axis, 3 is y-axis vel
# 4 is z-axis, 5 is z-axis vel
ax.plot(self.save_time, self.save_estimate[:, axis*2], ls='-', label='State Estimate')
ax.plot(self.save_time, self.save_ground_truth[:, axis], ls='-.', label='Ground Truth')
plt.title("Sensor observation v/s State estimate v/s Ground truth")
ax.set_xlabel("Time")
ax.set_ylabel("Position")
ax.legend()
plt.legend()
plt.show()
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0
def __init__(self, bbox3D, info):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=10, dim_z=7)
self.kf.F = np.array([
[1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix
[0, 1, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
])
self.kf.H = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function,
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0]
])
# with angular velocity
# self.kf = KalmanFilter(dim_x=11, dim_z=7)
# self.kf.F = np.array([[1,0,0,0,0,0,0,1,0,0,0], # state transition matrix
# [0,1,0,0,0,0,0,0,1,0,0],
# [0,0,1,0,0,0,0,0,0,1,0],
# [0,0,0,1,0,0,0,0,0,0,1],
# [0,0,0,0,1,0,0,0,0,0,0],
# [0,0,0,0,0,1,0,0,0,0,0],
# [0,0,0,0,0,0,1,0,0,0,0],
# [0,0,0,0,0,0,0,1,0,0,0],
# [0,0,0,0,0,0,0,0,1,0,0],
# [0,0,0,0,0,0,0,0,0,1,0],
# [0,0,0,0,0,0,0,0,0,0,1]])
# self.kf.H = np.array([[1,0,0,0,0,0,0,0,0,0,0], # measurement function,
# [0,1,0,0,0,0,0,0,0,0,0],
# [0,0,1,0,0,0,0,0,0,0,0],
# [0,0,0,1,0,0,0,0,0,0,0],
# [0,0,0,0,1,0,0,0,0,0,0],
# [0,0,0,0,0,1,0,0,0,0,0],
# [0,0,0,0,0,0,1,0,0,0,0]])
# self.kf.R[0:,0:] *= 10. # measurement uncertainty
self.kf.P[
7:,
7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
self.kf.P *= 10.
# self.kf.Q[-1,-1] *= 0.01 # process uncertainty
self.kf.Q[7:, 7:] *= 0.01
self.kf.x[:7] = bbox3D.reshape((7, 1))
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 1 # number of total hits including the first detection
self.hit_streak = 1 # number of continuing hit considering the first detection
self.first_continuing_hit = 1
self.still_first = True
self.age = 0
self.info = info # other info
def update(self, bbox3D, info):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1 # number of continuing hit
if self.still_first:
self.first_continuing_hit += 1 # number of continuing hit in the fist time
######################### orientation correction
if self.kf.x[3] >= np.pi:
self.kf.x[3] -= np.pi * 2 # make the theta still in the range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
new_theta = bbox3D[3]
if new_theta >= np.pi:
new_theta -= np.pi * 2 # make the theta still in the range
if new_theta < -np.pi: new_theta += np.pi * 2
bbox3D[3] = new_theta
predicted_theta = self.kf.x[3]
if abs(new_theta - predicted_theta) > np.pi / 2.0 and abs(
new_theta - predicted_theta
) < np.pi * 3 / 2.0: # if the angle of two theta is not acute angle
self.kf.x[3] += np.pi
if self.kf.x[3] > np.pi:
self.kf.x[3] -= np.pi * 2 # make the theta still in the range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
# now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90
if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0:
if new_theta > 0: self.kf.x[3] += np.pi * 2
else: self.kf.x[3] -= np.pi * 2
#########################
self.kf.update(bbox3D)
if self.kf.x[3] >= np.pi:
self.kf.x[3] -= np.pi * 2 # make the theta still in the range
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
self.info = info
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
self.kf.predict()
if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2
if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.still_first = False
self.time_since_update += 1
self.history.append(self.kf.x)
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return self.kf.x[:7].reshape((7, ))
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0
def __init__(self, bbox):
"""
Initialises a tracker using initial bounding box.
"""
# define constant velocity model
self.kf = KalmanFilter(dim_x=8, dim_z=4)
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0,
0], [0, 1, 0, 0, 0, 1, 0, 0],
[0, 0, 1, 0, 0, 0, 1,
0], [0, 0, 0, 1, 0, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0,
0], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1]])
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 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. #关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态；
self.kf.Q[
-1,
-1] *= 0.01 #Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值；
self.kf.Q[
4:,
4:] *= 0.01 #Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果Q为零，那么我们只相信预测值；
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 update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
#print(self.kf.x)
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
# print("111",self.kf.x)
# print("222",convert_x_to_bbox(self.kf.x))
#if self.kf.x[]
a = convert_x_to_bbox(self.kf.x)
if a[0][0] < 0:
a[0][0] = 0
if a[0][1] < 0:
a[0][1] = 0
return a
def setup_kalman(self):
# Optical Flow velocity in the x direction
self.flow_x = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z)
self._setup_kalman(self.flow_x, cfg.flow)
# OPtical Flow velocity in the y
self.flow_y = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z)
self._setup_kalman(self.flow_y, cfg.flow)
# Velocity of left wheels from encoder
self.vel_left = KalmanFilter(dim_x=cfg.vel_left.dim_x, dim_z=cfg.vel_left.dim_z)
self._setup_kalman(self.vel_left, cfg.vel_left)
# Velocity of the right wheels from encoder
self.vel_right = KalmanFilter(dim_x=cfg.vel_right.dim_x, dim_z=cfg.vel_right.dim_z)
self._setup_kalman(self.vel_right, cfg.vel_right)
# Rotational velocity from gyroscope
self.rotation = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z)
self._setup_kalman(self.rotation, cfg.rotation)
# Overall motion with all sensors included
self.odometry = KalmanFilter(dim_x=cfg.odometry.dim_x, dim_z=cfg.odometry.dim_z)
self._setup_kalman(self.odometry, cfg.odometry)
# Rotational Velocity
self.rot_vel = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z)
self._setup_kalman(self.rot_vel, cfg.rotation)
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 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
self.class_id = bbox[-1]
self.score = bbox[4]
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
self.class_id = bbox[-1]
self.score = bbox[4]
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0 # How many object been tracked
def __init__(self, bbox, feature, trk_id=None):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=7, dim_z=4)
# State transition matrix
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]])
# Measurement function
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]])
# Measuerment uncertainty (dim_z, dim_z), default I
self.kf.R[2:, 2:] *= 10.
# Covariance matrix (dim_x, dim_x), default I
self.kf.P[
4:,
4:] *= 1000. #give high uncertainty to the unobservable initial velocities
self.kf.P *= 10.
# Process noise (dim_x, dim_x), default I
self.kf.Q[-1, -1] *= 0.01
self.kf.Q[4:, 4:] *= 0.01
# Assign the initial value of the state
self.kf.x[:4] = convert_bbox_to_z(bbox)
self.time_since_update = 0
if trk_id is None:
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
else:
self.id = trk_id
self.history = [] # bbox history between updates
self.hits = 0 # total number of updates
self.hit_streak = 0 # consective number of updates (probationary perirod)
self.age = 0
self.features = []
if feature is not None:
self.features.append(feature)
self.predected_box = []
def update(self, bbox, feature):
"""
Updates the state vector with observed bbox.
(Unnecessarily update in every frame)
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
self.features.append(feature)
if len(self.features) > 100:
self.features.pop(0)
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
(Predict in every frame)
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
self.predected_box.append(self.history[-1][0])
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
def get_velocity(self):
"""
Returns the current velocity estimate.
"""
u_dot = self.kf.x[4]
v_dot = self.kf.x[5]
s_dot = self.kf.x[6]
return np.array([u_dot, v_dot, s_dot]).reshape((1, 3))
def get_predicted_bbox(self):
"""
Returns the current bounding box predicted.
"""
# print(self.history)
if not self.predected_box:
return self.get_state()
predicted_box = self.predected_box
self.predected_box = []
return predicted_box
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()
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)
class KalmanBoxTracker:
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
def __init__(self, bbox, tid):
"""
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._pre_z = KalmanBoxTracker._convert_bbox_to_z(bbox)
self.kf.x[:4] = self._pre_z
self.tid = tid
self.predict_times_since_update = 0
self.update_times_since_predict = 0
def _convert_bbox_to_z(bbox):
"""
Takes a bounding box in the form [x1,y1,x2,y2] and returns z in the form
[x,y,s,r] where x,y is the centre of the box and s is the scale/area and r is
the aspect ratio
"""
w = bbox[2] - bbox[0]
h = bbox[3] - bbox[1]
x = bbox[0] + w / 2.
y = bbox[1] + h / 2.
s = w * h #scale is just area
r = w / h
return np.array([x, y, s, r]).reshape((4, 1))
def _convert_x_to_bbox(x):
"""
Takes a bounding box in the centre form [x,y,s,r] and returns it in the form
[x1,y1,x2,y2] where x1,y1 is the top left and x2,y2 is the bottom right
"""
w = np.sqrt(x[2] * x[3])
h = x[2] / w
return np.array(
[x[0] - w / 2., x[1] - h / 2., x[0] + w / 2.,
x[1] + h / 2.]).reshape((1, 4))
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.predict_times_since_update = 0
self.update_times_since_predict += 1
self._pre_z = KalmanBoxTracker._convert_bbox_to_z(bbox)
self.kf.update(self._pre_z)
return
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if (self.kf.x[6] + self.kf.x[2]) <= 0:
self.kf.x[6] *= 0.0
self.kf.predict()
self.update_times_since_predict = 0
self.predict_times_since_update += 1
return KalmanBoxTracker._convert_x_to_bbox(self.kf.x)
def stay(self):
self.kf.update(self._pre_z)
return
class OneEyeTracker():
"""One Eye Tracker using a Kalman Filter
Parameters
----------
x_init : np.array
Initial states of the the eye:
[x, vx, ax, y, vy, ay, size]
"""
def __init__(self, x_init):
# SETTINGS
# Prediction Errors
axdev = 10 # pixels/s^2
aydev = 10 # pixels/s^2
sdev = 10 # pixels
# Measurement Errors
xdev_measure = 5
ydev_measure = 5
sdev_measure = 2
# Initial Errors of States
xdev_init = 240
ydev_init = 320
sdev_init = 10
# FPS
fps = 30
# KALMAN
# Initialize the filter's matrices
self.my_filter = KalmanFilter(dim_x=7, dim_z=3)
self.my_filter.F = np.array(
[[1, 1 / float(fps),
np.power(1 / float(fps), 2) / 2, 0, 0, 0, 0],
[0, 1, 1 / float(fps), 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1 / float(fps),
np.power(1 / float(fps), 2) / 2, 0],
[0, 0, 0, 0, 1, 1 / float(fps), 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]]) # state transition matrix
self.my_filter.Q = np.array(
[[
np.power(axdev, 2) * np.power(1 / float(fps), 4) / 36,
np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, 0, 0, 0,
0
],
[
np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 4,
np.power(axdev, 2) * 1 / float(fps) * 1 / 2, 0, 0, 0, 0
],
[
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6,
np.power(axdev, 2) * 1 / float(fps) * 1 / 2,
np.power(axdev, 2), 0, 0, 0, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 4) / 36,
np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 4,
np.power(aydev, 2) * 1 / float(fps) * 1 / 2, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6,
np.power(aydev, 2) * 1 / float(fps) * 1 / 2,
np.power(aydev, 2), 0
], [0, 0, 0, 0, 0, 0, np.power(sdev, 2)]]) # process uncertainty
self.my_filter.H = np.array([[1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0,
1]]) # Measurement function
self.my_filter.R = np.array([[np.power(xdev_measure, 2), 0, 0],
[0, np.power(ydev_measure, 2), 0],
[0, 0, np.power(sdev_measure,
2)]]) # state uncertainty
self.my_filter.P = np.array(
[[np.power(xdev_init, 2), 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0], [0, 0, 0,
np.power(ydev_init, 2), 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, pow(sdev_init,
2)]]) # coovariance initizialitation
self.my_filter.x = x_init # initial state (location(x,y), velocity(x,y), acceleration(x,y), and size)
def predict(self):
"""Predict the future states of the Kalman Filter
Returns
-------
x : np.array
States of the of the eye after prediction
[x, vx, ax, y, vy, ay, size]
"""
self.my_filter.predict()
return self.my_filter.x
def update(self, measurement):
"""Update the states of the Kalman Filter with a measurement
Parameters
-------
measurement : np.array
Measurement of the states of the eye
[x, y, size]
Returns
-------
x : np.array
States of the of the eye after measurement
[x, vx, ax, y, vy, ay, size]
"""
self.my_filter.update(measurement)
return self.my_filter.x
def test_linear_rts():
""" for a linear model the Kalman filter and UKF should produce nearly
identical results.
Test code mostly due to user gboehl as reported in GitHub issue #97, though
I converted it from an AR(1) process to constant velocity kinematic
model.
"""
dt = 1.0
F = np.array([[1., dt], [.0, 1]])
H = np.array([[1., .0]])
def t_func(x, dt):
F = np.array([[1., dt], [.0, 1]])
return np.dot(F, x)
def o_func(x):
return np.dot(H, x)
sig_t = .1 # peocess
sig_o = .0001 # measurement
N = 50
X_true, X_obs = [], []
for i in range(N):
X_true.append([i + 1, 1.])
X_obs.append(i + 1 + np.random.normal(scale=sig_o))
X_true = np.array(X_true)
X_obs = np.array(X_obs)
oc = np.ones((1, 1)) * sig_o**2
tc = np.zeros((2, 2))
tc[1, 1] = sig_t**2
tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2)
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1)
ukf = UnscentedKalmanFilter(dim_x=2,
dim_z=1,
dt=dt,
hx=o_func,
fx=t_func,
points=points)
ukf.x = np.array([0., 1.])
ukf.R = np.copy(oc)
ukf.Q = np.copy(tc)
s = Saver(ukf)
s.save()
s.to_array()
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([[0., 1]]).T
kf.R = np.copy(oc)
kf.Q = np.copy(tc)
kf.H = np.copy(H)
kf.F = np.copy(F)
mu_ukf, cov_ukf = ukf.batch_filter(X_obs)
x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf)
mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs)
x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf)
# check results of filtering are correct
kfx = mu_kf[:, 0, 0]
ukfx = mu_ukf[:, 0]
kfxx = mu_kf[:, 1, 0]
ukfxx = mu_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
# error in position should be smaller then error in velocity, hence
# atol is different for the two tests.
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
# now ensure the RTS smoothers gave nearly identical results
kfx = x_kf[:, 0, 0]
ukfx = x_ukf[:, 0]
kfxx = x_kf[:, 1, 0]
ukfxx = x_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
return ukf
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
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]])
std_noise = .1
sp = JulierSigmaPoints(n=3, kappa=0.)
f = UnscentedKalmanFilter(dim_x=3,
dim_z=2,
dt=.01,
hx=hx,
fx=fx,
points=sp)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2) * (std_noise**2)
f.Q = np.eye(3) * .001
f.Q[0, 0] = 0
f.Q[2, 2] = 0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5],
[0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]])
kf.F = F
kf.P *= 100
kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
results = []
zs = []
kfxs = []
for t in range(12000):
a = t / 30 + 90
x = cos(radians(a)) * 50. + randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x, y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append(hx(f.x))
kfxs.append(kf.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
plt.plot(results[:, 0], results[:, 1], c='k', label='UKF')
plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0
def __init__(self, bbox):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=8, dim_z=4)
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0.5, 0],
[0, 1, 0, 0, 0, 1, 0, 0.5],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 1, 0],
[0, 0, 0, 0, 0, 1, 0, 1],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1]])
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0]])
self.kf.R[:, :] *= 25.0 # set measurement uncertainty for positions
self.kf.Q[:2, :
2] = 0.0 # process uncertainty for positions is zero - only moves due to velocity, leave process for width height as 1 to account for turning
self.kf.Q[
2:4, 2:
4] *= 0.1 # process uncertainty for width/height for turning
self.kf.Q[
4:6, 4:
6] = 0.0 # process uncertainty for velocities is zeros - only accelerates due to accelerations
self.kf.Q[6:,
6:] *= 0.01 # process uncertainty for acceleration
self.kf.P[4:, 4:] *= 5.0 # maximum speed
z = mothe.yolo.convert_bbox_to_kfx(bbox)
self.kf.x[:4] = z
self.time_since_update = 0
self.id = mothe.yolo.KalmanBoxTracker.count
mothe.yolo.KalmanBoxTracker.count += 1
self.hits = 1
self.hit_streak = 1
self.age = 1
self.score = bbox[4]
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.hits += 1
self.hit_streak += 1
self.score = (self.score * (self.hits - 1.0) /
float(self.hits)) + (bbox[4] / float(self.hits))
z = mothe.yolo.convert_bbox_to_kfx(bbox)
self.kf.update(z)
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_kfx_to_bbox(self.kf.x)
def get_distance(self, y):
"""
Returns the mahalanobis distance to the given point.
"""
b1 = convert_kfx_to_bbox(self.kf.x[:4])[0]
return (bbox_iou(b1, y))
class KalmanBoxTracker(object):
"""
This class represents the internal state of individual tracked objects observed as bbox.
"""
count = 1 # benchmark starts with 1
def __init__(self, bbox):
"""
Initialises a tracker using initial bounding box.
"""
self.original_id = bbox[5] # <--- add to keep track of original IDs
#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
z = convert_bbox_to_z(bbox)
self.kf.x[:4] = z
self.original_center = z # <--- keep track of where this object first appeared
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.speed = 0
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.original_id = bbox[5] # <--- update to keep track of original IDs
z = convert_bbox_to_z(bbox)
self.kf.update(z)
self.speed = np.linalg.norm(z[:2] -
self.original_center[:2]) / self.age
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
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 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_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 __init__(self, x_init):
# SETTINGS
# Prediction Errors
axdev = 10 # pixels/s^2
aydev = 10 # pixels/s^2
sdev = 10 # pixels
# Measurement Errors
xdev_measure = 5
ydev_measure = 5
sdev_measure = 2
# Initial Errors of States
xdev_init = 240
ydev_init = 320
sdev_init = 10
# FPS
fps = 30
# KALMAN
# Initialize the filter's matrices
self.my_filter = KalmanFilter(dim_x=7, dim_z=3)
self.my_filter.F = np.array(
[[1, 1 / float(fps),
np.power(1 / float(fps), 2) / 2, 0, 0, 0, 0],
[0, 1, 1 / float(fps), 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1 / float(fps),
np.power(1 / float(fps), 2) / 2, 0],
[0, 0, 0, 0, 1, 1 / float(fps), 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]]) # state transition matrix
self.my_filter.Q = np.array(
[[
np.power(axdev, 2) * np.power(1 / float(fps), 4) / 36,
np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6, 0, 0, 0,
0
],
[
np.power(axdev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 4,
np.power(axdev, 2) * 1 / float(fps) * 1 / 2, 0, 0, 0, 0
],
[
np.power(axdev, 2) * np.power(1 / float(fps), 2) / 6,
np.power(axdev, 2) * 1 / float(fps) * 1 / 2,
np.power(axdev, 2), 0, 0, 0, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 4) / 36,
np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 3) / 12,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 4,
np.power(aydev, 2) * 1 / float(fps) * 1 / 2, 0
],
[
0, 0, 0,
np.power(aydev, 2) * np.power(1 / float(fps), 2) / 6,
np.power(aydev, 2) * 1 / float(fps) * 1 / 2,
np.power(aydev, 2), 0
], [0, 0, 0, 0, 0, 0, np.power(sdev, 2)]]) # process uncertainty
self.my_filter.H = np.array([[1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0,
1]]) # Measurement function
self.my_filter.R = np.array([[np.power(xdev_measure, 2), 0, 0],
[0, np.power(ydev_measure, 2), 0],
[0, 0, np.power(sdev_measure,
2)]]) # state uncertainty
self.my_filter.P = np.array(
[[np.power(xdev_init, 2), 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0], [0, 0, 0,
np.power(ydev_init, 2), 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, pow(sdev_init,
2)]]) # coovariance initizialitation
self.my_filter.x = x_init # initial state (location(x,y), velocity(x,y), acceleration(x,y), and size)
import numpy as np
import matplotlib.pyplot as plt
from kalman import KLFilter
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
kl2 = KalmanFilter(dim_x=2, dim_z=2, dim_u=1)
N = 200
def train_noise(var):
"""
This is the noise that hits the train...
"""
return np.random.normal(0, scale=np.sqrt(var), size=N)
def get_true_force():
"""
The true force driving the train:
"""
return np.cos(np.linspace(0, np.pi * 2, N)) * F
def train_measurement_noise(var):
"""
This is the noise due to measurement error
"""
return np.random.normal(0, scale=np.sqrt(var), size=N)
class Localization:
""" localization module for Boat-Udrone system """
rospy.init_node('LocalizationUdrone', anonymous=True)
last_t = rospy.Time.now()
def __init__(self):
rospy.Subscriber('/aruco_single/pose',
PoseStamped,
callback=self.aruco_pose_subscriber)
self.pub_states = rospy.Publisher('udroneboat', UdroneStates)
self.observation = None
self.listener = tf.TransformListener()
# Kalman filter definition
self.kf = KalmanFilter(dim_x=6, dim_z=3)
self.dt = 0.01
self.kf.x = np.array([[0.5], [0.5], [0.5], [0.5], [5], [0.5]])
self.kf.P = np.array([[2, 0, 0, 0, 0, 0], [0, 2, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0], [0, 0, 0, 2, 0, 0],
[0, 0, 0, 0, 2, 0], [0, 0, 0, 0, 0, 2]])
self.kf.H = np.array([[1., 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0]])
self.kf.R = np.array([[2, 0, 0], [0, 2, 0], [0, 0, 2]])
self.kf.Q = np.array([[0.002, 0, 0, 0, 0, 0], [0, 0.02, 0, 0, 0, 0],
[0, 0, 0.002, 0, 0, 0], [0, 0, 0, 0.02, 0, 0],
[0, 0, 0, 0, 0.002, 0], [0, 0, 0, 0, 0, 0.02]])
self.kf.F = np.array([[1., self.dt, 0, 0, 0, 0], [0, 1., 0, 0, 0, 0],
[0, 0, 1., self.dt, 0, 0], [0, 0, 0, 1., 0, 0],
[0, 0, 0, 0, 1., self.dt], [0, 0, 0, 0, 0, 1.]])
# Variables to maintain kalman update only on new aruco observation
self.aruco_previous_timestamp = rospy.Time.now()
self.aruco_pose_timestamp = rospy.Time.now()
self.is_observation = False
# Plots to stop_count iterations
self.plot = True
self.stop_count = 12000 # 6000, means 60 sec if dt is 0.01
self.on_vehicle = True
self.count = 0
if self.plot:
self.save_time = np.empty((0, 1)) # n x timestamps
self.save_observation = np.empty(
(0, 7)) # n x 7, where 7 values are x, y, z and quaternion
self.save_ground_truth = np.empty(
(0, 7)) # n x 7, where 7 values are x, y, z and quaternion
self.save_estimate = np.empty((
0,
6)) # n x 2, where 2 values are the state estimates x and xdot
self.save_observation_time = np.empty((0, 1)) # n x timestamps
self.run()
# self.run_filter()
# self.save_data_to_file()
# self.plot_fusion(dim=3, axis=0)
def run(self):
t1 = threading.Thread(target=self.plot_fusion)
t2 = threading.Thread(target=self.run_filter)
t1.start()
t2.start()
def save_data_to_file(self):
np.save('logs/kf_with_boat_ground_truth.npy', self.save_ground_truth)
np.save('logs/kf_with_boat_observations.npy', self.save_observation)
np.save('logs/kf_with_boat_observation_time.npy',
self.save_observation_time)
np.save('logs/kf_with_boat_estimates.npy', self.save_estimate)
def aruco_pose_subscriber(self, msg):
""" This subscriber publishes the timestamp of the aruco detection.
This will make sure the rate of Kalman update is synchronized to image detections
"""
self.aruco_pose_timestamp = rospy.Time(secs=msg.header.stamp.secs,
nsecs=msg.header.stamp.nsecs)
def run_filter(self):
rate = rospy.Rate(100.0)
# print(kf.x)
while not rospy.is_shutdown():
try:
# (trans, rot) = listener.lookupTransform('to', 'from', rospy.Time(0))
# timestamp is used to make sure update happens only after an observation
if (self.aruco_pose_timestamp -
self.aruco_previous_timestamp) > rospy.Duration(0):
self.is_observation = True
(trans, rot) = self.listener.lookupTransform(
'/udrone', '/world', self.aruco_pose_timestamp)
self.aruco_previous_timestamp = self.aruco_pose_timestamp
print("Aruco Observation: ", trans)
if self.plot:
(gtrans, grot) = self.listener.lookupTransform(
'/ground_truth_udrone', '/ground_truth_heron',
rospy.Time(0))
print("Ground Truth: ", gtrans)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as Ex:
print(Ex)
rate.sleep()
continue
self.kf.predict()
print("Predict :",
self.kf.x.T.flatten()[0],
self.kf.x.T.flatten()[2],
self.kf.x.T.flatten()[4])
# Do kalman update call only when the camera detects an aruco marker
if self.is_observation:
self.kf.update(np.array([trans]))
self.is_observation = False
if self.plot:
self.save_observation_time = np.append(
self.save_observation_time,
np.array([[self.count]]),
axis=0)
self.save_observation = np.append(self.save_observation,
np.array([trans + rot]),
axis=0)
print("Update :",
self.kf.x.T.flatten()[0],
self.kf.x.T.flatten()[2],
self.kf.x.T.flatten()[4])
if self.plot:
self.count += 1
self.save_time = np.append(self.save_time,
np.array([[self.count]]),
axis=0)
self.save_estimate = np.append(self.save_estimate,
np.array(
[self.kf.x.T.flatten()]),
axis=0)
self.save_ground_truth = np.append(self.save_ground_truth,
np.array([gtrans + grot]),
axis=0)
print(self.count)
# if you are running this filter on vehicle, disable plot and stop count
if self.count > self.stop_count and not self.on_vehicle:
break
# Publish message for the controller
msg = UdroneStates()
state = self.kf.x.T.flatten()
msg.x = state[0]
msg.xvel = state[1]
msg.y = state[2]
msg.yvel = state[3]
msg.z = state[4]
msg.zvel = state[5]
msg.boatxvel = 0
msg.boatyvel = 0
self.pub_states.publish(msg)
rate.sleep()
def plot_fusion(self, dim=2, axis=0):
if dim == 2:
# 2D plots
ax = plt.subplot(111)
if self.save_observation_time.shape[0] > 0:
ax.scatter(self.save_observation_time,
self.save_observation[:, axis],
marker='o',
c="green",
label='Sensor Observation')
# 0 is x-axis, 1 is x-axis vel
# 2 is y-axis, 3 is y-axis vel
# 4 is z-axis, 5 is z-axis vel
ax.plot(self.save_time,
self.save_estimate[:, axis * 2],
ls='-',
label='State Estimate')
ax.plot(self.save_time,
self.save_ground_truth[:, axis],
ls='-.',
label='Ground Truth')
plt.title("Sensor observation v/s State estimate v/s Ground truth")
ax.set_xlabel("Time")
ax.set_ylabel("Position")
ax.legend()
plt.legend()
plt.show()
elif dim == 3:
# 3D plots
# x-axis is the time
# y-axis and z-axis will show positions
fig = plt.figure()
ax = plt.axes(projection='3d')
# Plot estimates
x = self.save_time
y = self.save_estimate[:, axis]
z = self.save_estimate[:, axis + 2] # x, x_dot, y, y_dot, z, z_dot
ax.plot3D(x, y, z, 'blue')
# Plot ground truth
x = self.save_time
y = self.save_ground_truth[:, axis]
z = self.save_ground_truth[:, axis + 1] # x, y, z
ax.plot3D(x, y, z, 'orange')
# Plot observations
x = self.save_observation_time
y = self.save_observation[:, axis]
z = self.save_observation[:, axis + 1] # x, y, z
ax.scatter(x, y, z, color='r')
ax.set_xlabel('Time')
ax.set_ylabel('X-axis position')
ax.set_zlabel('Y-axis position')
ax.set_title(
'Sensor observation v/s State estimate v/s Ground truth')
plt.show()
def animate(self):
x = np.random.normal(size=(100, 3))
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
gs = ax.scatter([], [], [], c='red')
es = ax.scatter([], [], [], c='blue')
def update(i):
# print(self.save_time)
length = self.save_time.shape[0] - 100
# print(length)
if length > 5:
time = self.save_time[length:length + 101].flatten()
ground_truth = self.save_ground_truth[length:length + 101, :2]
ax.set_xlim3d([time[0], time[len(time) - 1]])
gs._offsets3d = (time[:i], ground_truth[:i, 0].flatten(),
ground_truth[:i, 1].flatten())
estimate = self.save_estimate[length:length + 101, :2]
es._offsets3d = (time[:i], estimate[:i, 0].flatten(),
estimate[:i, 1].flatten())
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_xlim(-100, 100)
ax.set_ylim(-5, 5)
ax.set_zlim(-5, 5)
ani = matplotlib.animation.FuncAnimation(fig,
update,
frames=100,
interval=10)
plt.tight_layout()
plt.show()
def KFtest():
dt = 0.001
my_filter = KalmanFilter(dim_x=2, dim_z=1)
my_filter.x = np.array([[0.0], [0.0]])
# initial state (location and velocity)
my_filter.F = np.array([[1.0, dt], [0.0, 1.0]])
# state transition matrix
my_filter.H = np.array([[1.0, 0.0]])
# Measurement function
# my_filter.P = 1000.
# covariance matrix
my_filter.R = 0.1
# state uncertainty
my_filter.Q = Q_discrete_white_noise(2, dt, 1e5)
# process uncertainty
x_list = list()
k = static_var()
tmp = k.get_some_measurement()
for i in tmp:
my_filter.predict()
# print ( i )
my_filter.update(i[0])
# do something with the output
# x_list = my_filter.x
# print( my_filter.x)
x_list.append(my_filter.x[0][0].tolist())
# plt.figure(1)
# plt.plot( tmp )
a = tmp[:, 0]
b = np.asarray(x_list)
c = np.vstack((a, b)).T
plt.figure(2)
# tmp = np.asarray(tmp).T
plt.plot(c)
# plt.plot( np.hstack((x_list, tmp)) )
plt.show()
input()
def test_default_dims():
kf = KalmanFilter(dim_x=3, dim_z=1)
kf.predict()
kf.update(np.array([[1.]]).T)
class KalmanBoxTracker(object):
"""
This class represents the internal state of individual tracked objects observed as bbox.
"""
count = 0
def __init__(self, bbox):
"""
Initialize a tracker using initial bounding box
Parameter 'bbox' must have 'detected class' int number at the -1 position.
"""
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.5
self.kf.Q[4:, 4:] *= 0.5
self.kf.x[:4] = convert_bbox_to_z(bbox) # STATE VECTOR
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
# keep yolov5 detected class information
self.detclass = bbox[5]
def update(self, bbox):
"""
Updates the state vector with observed bbox
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate
# test
arr1 = np.array([[1,2,3,4]])
arr2 = np.array([0])
arr3 = np.expand_dims(arr2, 0)
np.concatenate((arr1,arr3), axis=1)
"""
arr_detclass = np.expand_dims(np.array([self.detclass]), 0)
arr_u_dot = np.expand_dims(self.kf.x[4], 0)
arr_v_dot = np.expand_dims(self.kf.x[5], 0)
arr_s_dot = np.expand_dims(self.kf.x[6], 0)
return np.concatenate((convert_x_to_bbox(self.kf.x), arr_detclass, arr_u_dot, arr_v_dot, arr_s_dot), axis=1)
class Flodometry(object):
"""Main class of Node flodometry
Attributes:
kf (KalmanFilter): A kalman filter to filter sensor values and estimate
odometry
pub (rospy.Publisher): Publisher to push to odom
"""
def __init__(self):
"""Initialize the node set up the Kalman filter and set up subscriber
and publisher.
"""
self.setup_kalman()
# Subscribe to the adns topic
if __name__ == '__main__':
self.setup_ros()
def setup_ros(self):
self.tf_odom = tf.TransformBroadcaster()
rospy.init_node('flodometry')
rospy.Subscriber("/optical_flow", motion_read, self.update_flow)
rospy.Subscriber("/encoder_values", rpm, self.update_encoders)
rospy.Subscriber("/gyro", gyro, self.update_gyro)
# Initialize the publisher
self.pub = rospy.Publisher("/odom", Odometry, queue_size=10)
self.vel_pub = rospy.Publisher("/vel", vel_update, queue_size=10)
self.rate = rospy.Rate(60) # 60hz
def setup_kalman(self):
# Optical Flow velocity in the x direction
self.flow_x = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z)
self._setup_kalman(self.flow_x, cfg.flow)
# OPtical Flow velocity in the y
self.flow_y = KalmanFilter(dim_x=cfg.flow.dim_x, dim_z=cfg.flow.dim_z)
self._setup_kalman(self.flow_y, cfg.flow)
# Velocity of left wheels from encoder
self.vel_left = KalmanFilter(dim_x=cfg.vel_left.dim_x, dim_z=cfg.vel_left.dim_z)
self._setup_kalman(self.vel_left, cfg.vel_left)
# Velocity of the right wheels from encoder
self.vel_right = KalmanFilter(dim_x=cfg.vel_right.dim_x, dim_z=cfg.vel_right.dim_z)
self._setup_kalman(self.vel_right, cfg.vel_right)
# Rotational velocity from gyroscope
self.rotation = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z)
self._setup_kalman(self.rotation, cfg.rotation)
# Overall motion with all sensors included
self.odometry = KalmanFilter(dim_x=cfg.odometry.dim_x, dim_z=cfg.odometry.dim_z)
self._setup_kalman(self.odometry, cfg.odometry)
# Rotational Velocity
self.rot_vel = KalmanFilter(dim_x=cfg.rotation.dim_x, dim_z=cfg.rotation.dim_z)
self._setup_kalman(self.rot_vel, cfg.rotation)
def _setup_kalman(self, kf, config):
"""Sets all of the kalman filter constants see config.kalman_config for
more details on each variable
Returns:
None
"""
kf.x = config.x
kf.F = config.F
kf.H = config.H
kf.P = config.P
kf.Q = config.Q
kf.R = config.R
def update_flow(self, motion):
"""Call back to subscriber on /optical flow
updates the Kalman filter with the velocity value then calls publish_updates
Args:
motion (motion_read): Message published to /optical_flow
Returns:
None
"""
self.flow_x.predict()
self.flow_x.update(motion.dx)
self.flow_y.update(motion.dy)
def update_gyro(self, data):
self.rotation.predict()
self.rotation.update(data.theta)
self.rot_vel.predict()
self.rot_vel.update(data.omega)
def update_encoders(self, enc):
l_speed = enc.left_rpm
self.vel_left.predict()
self.vel_left.update(l_speed)
r_speed = enc.right_rpm
self.vel_right.predict()
self.vel_right.update(r_speed)
def update(self):
flow_x = self.flow_x.x[0]
flow_y = self.flow_y.x[0]
l_speed = self.vel_left.x[0]
r_speed = self.vel_right.x[0]
rotation = self.rotation.x[0]
# rospy.loginfo('Rotation: {}'.format(rotation))
avg = float(l_speed+r_speed)/2
diff = r_speed-avg
# rospy.loginfo('left:{} right:{} avg:{} diff:{}'.format(l_speed, r_speed, avg, diff))
H = cfg.odometry.get_h(theta=self.rotation.x[0])
z = np.array([flow_x, flow_y, avg, rotation])
# x1 = self.odometry.x
# rospy.loginfo('Theta: {}, theta_dot: {}'.format(x1[4], x1[5]))
self.odometry.predict()
# x2 = self.odometry.x
# rospy.loginfo('Predicted Theta: {}, theta_dot: {}'.format(x2[4], x2[5]))
self.odometry.update(z, H=H)
# x3 = self.odometry.x
# rospy.loginfo('Updated Theta: {}, theta_dot: {}'.format(x3[4], x3[5]))
def publish_updates(self):
"""Publishes kalman filter data to odometry
Args:
x (List): List containing current state estimates
p (List): List containing current variance estimates
Returns:
None
"""
if __name__ == '__main__':
while not rospy.is_shutdown():
update = vel_update()
update.header.stamp = rospy.Time.now()
update.header.frame_id = '/base_link'
update.linear_x = self.flow_x.x[0]
update.angular_z = self.rot_vel.x[0]
self.vel_pub.publish(update)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = '/odom'
self.update()
x = self.odometry.x
odom.pose.pose.position.x = x[0]
odom.pose.pose.position.y = x[2]
# odom.pose.covariance[0] = p[0][0]
odom.twist.twist.linear.x = x[1]
odom.twist.twist.linear.y = x[3]
odom.twist.twist.angular.z = x[5]
# odom.twist.covariance[0] = p[1][1]
odom.pose.pose.orientation.x
# rospy.loginfo('[pos_x: {}, vel_x: {}, pos_y: {}, vel_y: {}, theta: {}, vel_theta: {}]'.format(*x))
# quaternion_from_euler will operate on whatever object is passed
# to it, that is why I am passing a copy of x[4] because it was
# changing it's value!
quaternion = quaternion_from_euler(0.0, 0.0, copy.copy(self.rotation.x[0]))
odom.pose.pose.orientation.x = quaternion[0]
odom.pose.pose.orientation.y = quaternion[1]
odom.pose.pose.orientation.z = quaternion[2]
odom.pose.pose.orientation.w = quaternion[3]
self.pub.publish(odom)
self.tf_odom.sendTransform(
(x[0], x[2], 0.0),
(quaternion[0], quaternion[1], quaternion[2], quaternion[3]),
odom.header.stamp, "base_link", "odom")
self.rate.sleep()
else:
pass
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
import pdb
my_filter = KalmanFilter(dim_x=2, dim_z=1)
my_filter.x = np.array([[2.], [0.]]) # initial state (location and velocity)
my_filter.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix
my_filter.H = np.array([[1., 0.]]) # Measurement function
my_filter.P *= 1000. # covariance matrix
my_filter.R = 5 # state uncertainty
pdb.set_trace()
my_filter.Q = Q_discrete_white_noise(2, 0.1, .1) # process uncertainty
while True:
my_filter.predict()
my_filter.update(get_some_measurement())
# do something with the output
x = my_filter.x
do_something_amazing(x)
def __init__(self,
bbox3D,
info,
covariance_id=0,
track_score=None,
tracking_name='car',
use_angular_velocity=False):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
if not use_angular_velocity:
self.kf = KalmanFilter(dim_x=10, dim_z=7)
self.kf.F = np.array([
[1, 0, 0, 0, 0, 0, 0, 1, 0, 0], # state transition matrix
[0, 1, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
])
self.kf.H = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function,
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0]
])
else:
# with angular velocity
self.kf = KalmanFilter(dim_x=11, dim_z=7)
self.kf.F = np.array([
[1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], # state transition matrix
[0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
])
self.kf.H = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], # measurement function,
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]
])
# Initialize the covariance matrix, see covariance.py for more details
self.kf.P, self.kf.Q, self.kf.R = covariance(covariance_id,
tracking_name)
if not use_angular_velocity:
self.kf.P = self.kf.P[:-1, :-1]
self.kf.Q = self.kf.Q[:-1, :-1]
self.kf.x[:7] = bbox3D.reshape((7, 1))
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 1 # number of total hits including the first detection
self.hit_streak = 1 # number of continuing hit considering the first detection
self.first_continuing_hit = 1
self.still_first = True
self.age = 0
self.info = info # other info
self.track_score = track_score
self.tracking_name = tracking_name
self.use_angular_velocity = use_angular_velocity
class VelocityEstimator:
"""Estimates base velocity of A1 robot.
The velocity estimator consists of 2 parts:
1) A state estimator for CoM velocity.
Two sources of information are used:
The integrated reading of accelerometer and the velocity estimation from
contact legs. The readings are fused together using a Kalman Filter.
2) A moving average filter to smooth out velocity readings
"""
def __init__(self,
robot,
accelerometer_variance=0.1,
sensor_variance=0.1,
initial_variance=0.1,
moving_window_filter_size=120):
"""Initiates the velocity estimator.
See filterpy documentation in the link below for more details.
https://filterpy.readthedocs.io/en/latest/kalman/KalmanFilter.html
Args:
robot: the robot class for velocity estimation.
accelerometer_variance: noise estimation for accelerometer reading.
sensor_variance: noise estimation for motor velocity reading.
initial_covariance: covariance estimation of initial state.
"""
self.robot = robot
self.filter = KalmanFilter(dim_x=3, dim_z=3, dim_u=3)
self.filter.x = np.zeros(3)
self._initial_variance = initial_variance
self.filter.P = np.eye(3) * self._initial_variance # State covariance
self.filter.Q = np.eye(3) * accelerometer_variance
self.filter.R = np.eye(3) * sensor_variance
self.filter.H = np.eye(3) # measurement function (y=H*x)
self.filter.F = np.eye(3) # state transition matrix
self.filter.B = np.eye(3)
self._window_size = moving_window_filter_size
self.moving_window_filter_x = MovingWindowFilter(
window_size=self._window_size)
self.moving_window_filter_y = MovingWindowFilter(
window_size=self._window_size)
self.moving_window_filter_z = MovingWindowFilter(
window_size=self._window_size)
self._estimated_velocity = np.zeros(3)
self._last_timestamp = 0
def reset(self):
self.filter.x = np.zeros(3)
self.filter.P = np.eye(3) * self._initial_variance
self.moving_window_filter_x = MovingWindowFilter(
window_size=self._window_size)
self.moving_window_filter_y = MovingWindowFilter(
window_size=self._window_size)
self.moving_window_filter_z = MovingWindowFilter(
window_size=self._window_size)
self._last_timestamp = 0
def _compute_delta_time(self, robot_state):
if self._last_timestamp == 0.:
# First timestamp received, return an estimated delta_time.
delta_time_s = self.robot.time_step
else:
delta_time_s = (robot_state.tick - self._last_timestamp) / 1000.
self._last_timestamp = robot_state.tick
return delta_time_s
def update(self, robot_state):
"""Propagate current state estimate with new accelerometer reading."""
delta_time_s = self._compute_delta_time(robot_state)
sensor_acc = np.array(robot_state.imu.accelerometer)
base_orientation = self.robot.GetBaseOrientation()
rot_mat = self.robot.pybullet_client.getMatrixFromQuaternion(
base_orientation)
rot_mat = np.array(rot_mat).reshape((3, 3))
calibrated_acc = rot_mat.dot(sensor_acc) + np.array([0., 0., -9.8])
self.filter.predict(u=calibrated_acc * delta_time_s)
# Correct estimation using contact legs
observed_velocities = []
foot_contact = self.robot.GetFootContacts()
for leg_id in range(4):
if foot_contact[leg_id]:
jacobian = self.robot.ComputeJacobian(leg_id)
# Only pick the jacobian related to joint motors
joint_velocities = self.robot.motor_velocities[leg_id *
3:(leg_id + 1) *
3]
leg_velocity_in_base_frame = jacobian.dot(joint_velocities)
base_velocity_in_base_frame = -leg_velocity_in_base_frame[:3]
observed_velocities.append(
rot_mat.dot(base_velocity_in_base_frame))
if observed_velocities:
observed_velocities = np.mean(observed_velocities, axis=0)
self.filter.update(observed_velocities)
vel_x = self.moving_window_filter_x.calculate_average(self.filter.x[0])
vel_y = self.moving_window_filter_y.calculate_average(self.filter.x[1])
vel_z = self.moving_window_filter_z.calculate_average(self.filter.x[2])
self._estimated_velocity = np.array([vel_x, vel_y, vel_z])
@property
def estimated_velocity(self):
return self._estimated_velocity.copy()
class Kalman:
"""
This is just a wrapper around the filterpy.kalman.KalmanFilter class to be used in our experiments, adding the likelihood functionality
"""
def __init__(self, model, measurements, print_results=False):
"""
model: SimpleModel or StochasticModelTauLeaping
measurements: the precomputed measurements of the model, for e.g. [model.measure(t) for t in range(time)]
(optional) print_results: useful for debugging purposes
"""
self.kf = KF(dim_x=model.state_dim, dim_z=model.measurement_dim) # instantiating the underlying KF from filterpy
self.measurements = measurements
self.kf.x = model.states[0]
self.kf.F = model.F
self.kf.H = model.H
self.kf.Q = model.Q
self.kf.R = model.R
self.model = model
self.print_res = print_results
def run(self):
"""
Running the KF and computing the likelihood. Returns the posterior means, variances and overall likelihood
"""
# Actually running the KF
post_means, post_covs, prior_means, prior_covs = self.kf.batch_filter(self.measurements)
# Computing the likelihood, according to Wikipedia's Kalman Filter > Marginal Likelihood equations and following their notations
# https://en.wikipedia.org/wiki/Kalman_filter#Marginal_likelihood
loglikelihood = 0
for t, z in enumerate(self.measurements):
Sk = self.model.H @ prior_covs[t] @ self.model.H.T + self.model.R
yk = z - self.model.H @ prior_means[t]
sign, logdet = np.linalg.slogdet(Sk)
logdet *= sign
loglikelihood -= 0.5 * (yk.T @ np.linalg.inv(Sk) @ yk + logdet + self.model.measurement_dim * np.log(2*np.pi))
means = np.array(post_means)
variances = np.array(list(map(lambda x: np.sqrt([x[i][i] for i in range(self.model.state_dim)]), post_covs)))
if self.print_res:
# Printing some results, highly customizable
print("Loglikelihood: ", loglikelihood)
fig = plt.figure()
# plt.title('Kalman Filter Results', fontsize=16, y=1.08)
measurements = np.array(self.measurements)
times = range(len(self.measurements))
for i in range(self.model.state_dim):
ax = fig.add_subplot(self.model.state_dim, 1, i+1)
# ax.set_title(f"dimension {i}")
ax.plot(times, means[:, i], 'm--', color='red', label='KF means')
# ax.plot(times, means[:, i] + variances[:, i], 'c--', label='KF variance')
# ax.plot(times, means[:, i] - variances[:, i], 'c--')
ax.fill_between(times, means[:, i] + variances[:, i], means[:, i] - variances[:, i], alpha=0.2)
ax.plot(times, measurements[:, i], 'ko', label='measurements', markersize=3)
ax.set_xlabel("timestep")
ax.set_ylabel("value")
ax.legend()
fig.tight_layout()
plt.savefig('KF_1D_run.pdf')
plt.show()
print("KF ran successfully")
return means, variances, loglikelihood
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 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[:4]) # convert_bbox_to_z(bbox)
self.time_since_update = 0
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
self.objConfidence = bbox[4]
self.objclass = bbox[5]
self.Region_path = []
self.Frame_count_path = []
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
"""
Updates the state vector with observed bbox.
"""
def update(self, bbox):
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
def predict(self):
"""
Realiza la prediccion de la siguiente posicion del filtro.
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
retorna la caja de deteccion estimada.
"""
return convert_x_to_bbox(self.kf.x)
def get_point_of_interest(self):
"""
Retorna el centroide de la deteccion, la cual se define
como el punto de interes.
"""
return [round(float(i[0]), 2) for i in self.kf.x[:2]]
def get_last_region(self):
return self.Region_path[-1]
def get_path(self):
return self.Region_path
def get_frame_count(self):
return self.Frame_count_path
def get_id(self):
return self.id
def set_path(self,path):
self.Region_path = path
def set_frame_count(self,frame_count):
self.Frame_count_path = frame_count
def add_regions_name_to_path(self,region_name):
self.Region_path.append(region_name)
def eliminate_None_from_path_and_frame_count(self):
path = []
frames = []
for (p,f) in zip(self.get_path(),self.get_frame_count()):
if not p is None:
path.append(p)
frames.append(f)
self.set_path(path)
self.set_frame_count(frames)
def print(self):
print("###########################")
print("state:", self.get_state())
print("path:", self.get_path())
print("fcount:", self.get_path())
print("hits:", self.hits)
print("hit_streak:", self.hit_streak)
print("age:", self.age)
print("time_since_update:", self.time_since_update)
print("###########################")
