def _KF_init(self): # para: Center of box used for prediction
KF = KalmanFilter(4,2)
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 do_filter_kalman_1D(X, noise_level=1, Q=0.001):
#dim_x = X.shape[1]*2
dim_x = 2
fk = KalmanFilter(dim_x=dim_x, dim_z=1)
fk.x = numpy.array([0., 1.]) # state (x and dx)
fk.F = numpy.array([[1., 1.], [0., 1.]])
fk.H = numpy.array([[1., 0.]]) # Measurement function
fk.P = 10. # covariance matrix
fk.R = noise_level # state uncertainty
fk.Q = Q # process uncertainty
X_fildered, cov, _, _ = fk.batch_filter(X)
return X_fildered[:, 0]
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U):
'''
Function creates a Kalman Filter object and assigns all the instance variables
:return: Kalman Filter
'''
kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z)
kf.x = X
kf.P = P
kf.F = A
print('=======================')
kf.Q = Q
kf.B = B
kf.U = U
kf.R = R
kf.H = H
return kf
def __init__(self, x0):
self.kf = KalmanFilter(dim_x=2, dim_z=1)
# Initial state estimate
self.kf.x = np.array([x0, 0])
# Initial Covariance matrix
self.kf.P = np.eye(2) * 2**10
# State transition function
self.kf.F = np.array([[1., 1], [0., 1.]])
# Process noise
self.kf.Q = Q_discrete_white_noise(dim=2, dt=1, var=1)
# Measurement noise
self.kf.R = np.array([[50]])
# Measurement function
self.kf.H = np.array([[1., 0.]])
def tracker1():
tracker = KalmanFilter(dim_x=2, dim_z=2)
dt = 0.002
tracker.F = np.array([[1, -1 * dt], [0, 1]])
tracker.u = 0.0
tracker.H = np.array([[1, 0]])
tracker.R = 0.05
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
tracker.Q = q
tracker.x = np.array([[0, 0]]).T
tracker.P = np.eye(2) * 5
return tracker
def setup_filter(self, detections: np.ndarray):
"""
Initialize Kalman Filter
Args:
detections: Points for initializing Kalman Filter
"""
# Validate shape of detected points
detections = validate_points_shape(detections)
# Calculates dimension of 'x' (Filter State Estimate)
# Position x Velocity x Number of Points
dim_x = 2 * 2 * self.points_count
# Calculates dimension of 'z' (Last Measurement)
dim_z = 2 * self.points_count
self.dim_z = dim_z
# Initialize the Kalman Filter
self.filter = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
# Define the F (State Transition Matrix)
self.filter.F = np.eye(dim_x)
# Update
dt = 1 # At each step we update pos with v * dt
for p in range(dim_z):
self.filter.F[p, p + dim_z] = dt
# Define the H (Measurement Function)
self.filter.H = np.eye(
dim_z,
dim_x,
)
# Define the R (Measurement Uncertainty)
self.filter.R *= 4.0
# Define the Q (Process Uncertainty/Noise)
self.filter.Q[dim_z:, dim_z:] /= 10
# Initial state: numpy.array(dim_x, 1)
self.filter.x[:dim_z] = np.expand_dims(detections.flatten(), 0).T
# Estimation uncertainty: numpy.array(dim_x, dim_x)
self.filter.P[dim_z:, dim_z:] *= 10.0
def kalman(bbox, gt, stats, Q = 0):
#define kalman filter
nx,nz = 4,4
Kal = KalmanFilter(dim_x = nx, dim_z = nz)
Kal.R = np.eye(nz) * 500
Kal.R[0,0] = stats[0]
Kal.R[1,1] = stats[1]
Kal.R[2,2] = stats[2]
Kal.R[3,3] = stats[3]
Kal.Q = np.eye(nx) * float(Q)
Kal.P *= 0
Kal.F = np.eye(nx)
#Kal.F[2,4] = 1
Kal.H = np.eye(nz,nx,0)
init = np.zeros(nx)
init[:4] = gt[0,:]
#init[4] = 1
Kal.x = init
end = h.lastindex(gt)
mem = ma.array(np.zeros((bbox.shape[0],nx)), mask = True)
mem[0,:] = init
memk = np.zeros((end,nx))
memp = np.zeros((end,nx))
for i in range(1, end):
z = bbox[i, :]
if bbox.mask[i, 0] == True:
z = None
Kal.predict()
Kal.update(z)
x = Kal.x
mem[i,:] = x[:]
for j in range(nx):
memk[i,j] = Kal.K[j,j]
for j in range(nx):
memp[i,j] = Kal.P[j,j]
return mem, memk,memp
def __init__(self, kf_obs, param):
"""
Initialises a tracker using initial position.
KF Instance variables:
x : ndarray (dim_x, 1), default = [0,0,0…0] filter state estimate
P : ndarray (dim_x, dim_x), default eye(dim_x) covariance matrix
Q : ndarray (dim_x, dim_x), default eye(dim_x) Process uncertainty/noise
R : ndarray (dim_z, dim_z), default eye(dim_x) measurement uncertainty/noise
H : ndarray (dim_z, dim_x) measurement function
F : ndarray (dim_x, dim_x) state transistion matrix
B : ndarray (dim_x, dim_u), default 0 control transition matrix
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=5, dim_z=3)
# x = [x,y,vx,vy,va]
self.kf.x[:3] = kf_obs.reshape((3, 1))
#self.kf.P[3:,3:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
self.kf.P = np.eye(5) * 50.
self.kf.R = np.eye(3) * 50
#self.kf.R = np.eye(3) * parameter
self.kf.P *= 10.
self.kf.Q[3:,3:] *= 0.01
self.kf.F = np.array([[1,0,1,0,0], # state transition matrix
[0,1,0,1,0],
[0,0,1,0,0],
[0,0,0,1,0],
[0,0,0,0,1]])
self.kf.H = np.array([[0,0,1,0,0], # measurement function,
[0,0,0,1,0],
[0,0,0,0,1]])
# self.kf.R[0:,0:] *= 10. # measurement uncertainty
self.time_since_update = 0
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
def test_noisy_11d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0]) # initial state (location and velocity)
f.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn() * 20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0])
measurements.append(z)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2., 0]]).T
f.P = np.eye(2) * 100.
m, c, _, _ = f.batch_filter(zs, update_first=False)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, 'r', alpha=0.5)
p2, = plt.plot(results, 'b')
p4, = plt.plot(m[:, 0], 'm')
p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"],
loc=4)
plt.show()
def __init__(self, bbox, label):
"""
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.0
self.kf.P[
4:,
4:] *= 1000.0 # give high uncertainty to the unobservable initial velocities
self.kf.P *= 10.0
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.labels = [label]
self.counted = False
self.locations = []
self.creation_time = int(datetime.now().timestamp() * 1000)
self.color = (
random.randint(0, 256),
random.randint(0, 256),
0,
)
def __init__(self, position, parameter):
"""
Initialises a tracker using initial position.
KF Instance variables:
x : ndarray (dim_x, 1), default = [0,0,0…0] filter state estimate
P : ndarray (dim_x, dim_x), default eye(dim_x) covariance matrix
Q : ndarray (dim_x, dim_x), default eye(dim_x) Process uncertainty/noise
R : ndarray (dim_z, dim_z), default eye(dim_x) measurement uncertainty/noise
H : ndarray (dim_z, dim_x) measurement function
F : ndarray (dim_x, dim_x) state transistion matrix
B : ndarray (dim_x, dim_u), default 0 control transition matrix
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=4, dim_z=2)
# x = [x,y,vx,vy]
self.kf.x[:2] = position.reshape((2, 1))
correct = 1000
self.kf.R = np.array([
[1000, 0], # Measurement uncertainty
[0, 1000]
])
self.kf.P *= 80
self.kf.Q = np.eye(4) * 100
self.kf.F = np.array([
[1, 0, 1, 0], # state transition matrix
[0, 1, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
self.kf.H = np.array([
[1, 0, 0, 0], # measurement function,
[0, 1, 0, 0]
])
self.time_since_update = 0
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
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 __init__(self, bbox, ids, logger, cfg):
self.abnormal1 = 0
self.abnormal2 = 0
self.Vsv = cfg["Vs"]
self.Vxyv = cfg["Vxy"]
self.logger = logger
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.Q[-1, -1] *= 0.01
self.kf.Q = np.array([[0.01, 0, 0, 0, 0.01, 0, 0],
[0, 0.01, 0, 0, 0, 0.01, 0],
[0, 0, 0.01, 0, 0, 0, 0.01],
[0, 0, 0, 0.01, 0, 0, 0],
[0.01, 0, 0, 0, 0.1, 0, 0],
[0, 0.01, 0, 0, 0, 0.1, 0],
[0, 0, 0.01, 0, 0, 0, 0.1]])
#self.kf.Q[6, 6] = 1
#self.kf.P *= 10
self.kf.x[:4] = Tracker.get_xysr_rect(bbox)
self.ids = ids
self.state = STATE.INITED
self.trackHistory = []
self.timeSinceUpdate = 0
self.hits = 1
self.age = 0
self.countRect = np.array([0, 0, 0, 0])
self.VsFlag = False
self.VxyFlag = False
def __init__(self, face):
"""Initializes a tracker using an initial detected face.
Parameters
----------
face : dict
Face to initialize tracker to, as returned by a `Detection`
instance.
"""
# 4 measurements plus 3 velocities. We don't keep a velocity for the
# ratio of the bounding box.
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.0
# Give high uncertainty to the unobservable initial velocities.
self.kf.P[4:, 4:] *= 1000.0
self.kf.P *= 10.0
self.kf.Q[-1, -1] *= 0.01
self.kf.Q[4:, 4:] *= 0.01
self.kf.x[:4] = corners_to_center(face['bbox'])
self.hits = 0
self.time_since_update = 0
self.id = KalmanTracker.count
KalmanTracker.count += 1
def compute_ground_truth(observations, initial_mean, initial_var, transition_factor, transition_noise_var, observation_factor, observation_noise_var):
fk = KalmanFilter(dim_x=len(initial_mean), dim_z=observations.shape[1])
fk.x = initial_mean
fk.P = initial_var
fk.F = transition_factor
fk.Q = transition_noise_var
fk.H = observation_factor
fk.R = observation_noise_var
mu, cov, _, _ = fk.batch_filter(observations)
means, covs, _, _ = fk.rts_smoother(mu, cov)
return means, covs
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U):
'''
:param configs tuple: all the values to define the kalman filter
:return: Kalman Filter
'''
kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z)
kf.x = X
kf.P = P
kf.F = A
print('=======================')
kf.Q = Q
kf.B = B
kf.U = U
kf.R = R
kf.H = H
return kf
def run_standard_kf(zs, dt=1.0, std_x=0.3, std_y=0.3):
kf = KalmanFilter(4, 2)
kf.x = np.array([0., 0., 0., 0.])
kf.R = np.diag([std_x**2, std_y**2])
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.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)
xs, *_ = kf.batch_filter(zs)
return xs
def build_kf(self, bbox):
kf = KalmanFilter(dim_x=7, dim_z=4)
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]])
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]])
kf.R[2:, 2:] *= 10.
kf.P[
4:,
4:] *= 1000. #give high uncertainty to the unobservable initial velocities
kf.P *= 10.
kf.Q[-1, -1] *= 0.01
kf.Q[4:, 4:] *= 0.01
kf.x[:4] = self.convert_bbox_to_z(bbox)
return kf
def filter(self):
"Filtra la corsa attraverso un filtro di Kalman"
# Crea il filtro di Kalman
f = KalmanFilter(dim_x=2, dim_z=2)
# Inizializza il filtro
f.x = np.array(self.points[0])
index = 0
while index < len(self.points) - 1:
f.F = np.array([[1, 0], [0, 1]]) # state transition matrix
f.H = np.array([[1, 0], [0, 1]]) # Measurement function
f.P *= 1.5 # covariance matrix
f.R = np.array([[1, 0], [0, 1]]) # state uncertainty
f.Q = Q_discrete_white_noise(2, 1., 1.) # process uncertainty
f.predict()
f.update(self.points[index + 1])
self.points[index + 1] = f.x
index += 1
def __init__(self, dim_state, dim_control, dim_measurement,
initial_state_mean, initial_state_covariance, matrix_F,
matrix_B, process_noise_Q, matrix_H, measurement_noise_R):
filter = KalmanFilter(dim_x=dim_state,
dim_u=dim_control,
dim_z=dim_measurement)
filter.x = initial_state_mean
filter.P = initial_state_covariance
filter.Q = process_noise_Q
filter.F = matrix_F
filter.B = matrix_B
filter.H = matrix_H
filter.R = measurement_noise_R # covariance matrix
super().__init__(filter)
def make_filter(start):
f = KalmanFilter(dim_x=6, dim_z=3)
f.x = start
f.F = np.eye(6)
f.F[:3, 3:6] = np.eye(3) * dt
f.B = np.array([0, dt**2 / 2, 0, 0, dt, 0])
g = -9.81
f.H = np.zeros((3, 6))
f.H[:, :3] = np.eye(3)
f.Q *= 0.5
f.R *= 2
f.P *= 1
return f
def setup():
# set up sensor simulator
dt = 0.1
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
x0 = np.array([[0], [0.1]])
sim = LinearSensor(x0, F, H)
# set up kalman filter
tracker = KalmanFilter(dim_x=2, dim_z=1)
tracker.F = F
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
tracker.Q = q
tracker.H = H
tracker.R = measurement_var * filter_misestimation_factor
tracker.x = np.array([[0, 0]]).T
tracker.P = np.eye(2) * 500
return sim, tracker
def __init__(self, coors: Tuple, state_noise: float = 1.0, r_scale: float = 1.0, q_var: float = 1.0):
self.filter = KalmanFilter(dim_x=4, dim_z=2)
self.filter.x = np.array([coors[0], 0, coors[1], 0])
self.dt = 1.0
self.filter.F = np.array([[1, self.dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, self.dt],
[0, 0, 0, 1]])
self.filter.H = np.array([[1, 0, 0, 0],
[0, 0, 1, 0]])
self.filter.P *= state_noise
self.filter.R = np.diag(np.ones(2)) * state_noise * r_scale
self.filter.Q = Q_discrete_white_noise(dim=2, dt=self.dt, var=q_var, block_size=2)
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1) * std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
xest = []
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append(x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def Kalman1(self):
pos = self.pos
dt = 0.01
rk = KalmanFilter(dim_x=4, dim_z=2)
#radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
# make an imperfect starting guess
rk.x = np.array(
[pos[0] - 10, self.vel[0] + 10, pos[1] + 10, self.vel[1] - 10]).T
rk.F = np.eye(4) + np.array([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1],
[0, 0, 0, 0]]) * dt
range_std = 5 # 5 pixels
rk.R = np.eye(2) * 200**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.1**2)
rk.Q = block_diag(q, q)
rk.P *= 1000
rk.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
return rk
def test_1d():
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
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.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)
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 __init__(self, pose, tracked_points_num):
"""
Initialises a tracker using initial bounding box.
"""
dim_x = 2 * 2 * tracked_points_num # We need to accomodate for velocities
dim_z = 2 * tracked_points_num # We need to accomodate for velocities
self.dim_z = dim_z
# Define constant velocity model
self.kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
# State transition matrix (models physics): numpy.array()
self.kf.F = np.eye(dim_x)
dt = 1 # At each step we update pos with v * dt
# Add position vs velocity update rules
for p in range(dim_z):
self.kf.F[p, p + dim_z] = dt
# Process uncertainty: numpy.array(dim_x, dim_x)
self.kf.Q[-1,-1] *= 0.01 # TODO check
self.kf.Q[dim_z:, dim_z:] *= 0.01
# Measurement function: numpy.array(dim_z, dim_x)
self.kf.H = np.eye(dim_z, dim_x,)
# Measurement uncertainty (sensor noise): numpy.array(dim_z, dim_z)
# NOTE: Reduced from 10 to 1 as it made our predictions lag behind our detections too much
self.kf.R *= 1.
# Initial state: numpy.array(dim_x, 1)
self.kf.x[:dim_z] = self.convert_to_kf_x_format(pose)
# Initial state covariance matrix: numpy.array(dim_x, dim_x)
self.kf.P[dim_z:, dim_z:] *= 1000. # Give high uncertainty to the unobservable initial velocities
self.kf.P *= 10. # TODO we get 10 * 1000 in velocities, is this correct?
self.time_since_update = 0
self.id = KalmanPoseTracker.count
KalmanPoseTracker.count += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
self.debug_dict = {}
# # NOTE: I am overriding our predictions by setting this huge K because when a limb dissapears
# # the kalman filter interpolates between its position and (0, 0) and f***s my code up.
# self.kf.K += 10000
self.last_detection = pose
def __init__(self,
x=0.0,
y=0.0,
z=0.0,
is_active=False,
is_dead=False,
youth=0,
strikes=0,
dt=0.16): #dt = 0.1 bcoz we getting data at 10Hz
'''
x and yare coordinates of the the obstacles in the plane they are moving. is_active determines if a marker is to be published for this object.
Youth variable is used to track the number of frames a new object appears, this is to filter out transient noise. Strikes calculate how long the object was not tracked.
'''
global color_pallete
self.id = str(rospy.Time.now())
self.x = x
self.y = y
self.z = z
self.is_active = is_active
self.is_dead = is_dead
self.youth = youth
self.strikes = strikes
self.color = color_pallete[random.randint(0, 8)]
# kalman filter object. x = [x, x_dot, y, y_dot] and z = [x_measured, y_measured]
self.kf = KalmanFilter(dim_x=4, dim_z=2)
# state transition matrix
self.kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
# process noise matrix
q = Q_discrete_white_noise(dim=2, dt=dt, var=2) # orignal value 200
self.kf.Q = block_diag(q, q)
# measurement funtion
self.kf.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
# measurement noise matrix
self.kf.R = np.array([[0.05, 0.], [0., 0.05]]) #orignal values .2
self.kf.x = np.array([[self.x, 0, self.y,
0]]).T ######it is z, not y for the time being
# self.kf.P = np.eye(4) * 100.
self.kf.P = np.array([
[1, 0, 0, 0], #######original value 2
[0, 4, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 4]
])
def __init__(self, vid_settings, maze_settings) -> None:
# read frame size from config file
# self.settings = read_yaml(conf)
self.settings = vid_settings
self.height = self.settings['frame_height'][1] - self.settings[
'frame_height'][0]
self.width = self.settings['frame_width'][1] - self.settings[
'frame_width'][0]
print(f'frame width: {self.width} frame height: {self.height}')
# setup fast corner detection
self.fast = cv.FastFeatureDetector_create(
threshold=maze_settings['fast_thresh'])
self.fast.setNonmaxSuppression(0)
# set initial points to use as a corner estimation
self.prev_tl = (0, 0)
self.prev_tr = (0, self.width - 1)
self.prev_bl = (self.height - 1, 0)
self.prev_br = (self.height - 1, self.width - 1)
# corner filtering
self.med_offset = self.settings['median_offset']
self.offsets = self.settings['offsets']
# search area when ball already known
self.area_size = self.settings['search_area']
# annotated text locations
self.text_tl = (self.settings['text_tl'][0],
self.settings['text_tl'][1])
self.text_tr = (self.settings['text_tr'][0],
self.settings['text_tr'][1])
self.text_spacing = self.settings['text_spacing']
self.text_size = self.settings['text_size']
# initialize Kalman Filter
self.kf = KalmanFilter(dim_x=4, dim_z=2)
self.kf.x = np.array([0., 0., 0., 0.])
self.kf.F = np.array([[1., 0., 1., 0.], [0., 1., 0., 1.],
[0., 0., 1., 0.], [0., 0., 0., 1.]])
self.kf.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]])
self.kf.P *= 100.
self.kf.R = 5.
def __init__(self, bbox3D, info, R, Q, P_0, delta_t):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=STATE_SIZE, dim_z=MEAS_SIZE)
if MOTION_MODEL == "CV":
self.kf.F = get_CV_F(delta_t)
elif MOTION_MODEL == "CA":
self.kf.F = get_CA_F(delta_t)
elif MOTION_MODEL == "CYRA":
self.kf.F = get_CYRA_F(delta_t)
else:
print("unknown motion model", MOTION_MODEL)
raise ValueError
# x y z theta l w h
self.kf.H = np.zeros((MEAS_SIZE, STATE_SIZE))
for i in range(min(MEAS_SIZE, STATE_SIZE)):
self.kf.H[i, i] = 1.
self.kf.R[0:, 0:] = R # measurement uncertainty
# initialisation cov
# self.kf.P[7:,7:] *= 1000. #state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
# self.kf.P *= 10.
# innov cov from pixor stats
self.kf.P = P_0
# self.kf.Q[-1,-1] *= 0.01
# self.kf.Q[7:,7:] *= 0.01 # process uncertainty
self.kf.Q = Q
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
