def filter_kalman(kf, x_data, y_data, y_width):
xs, ts, pr, ps = [], [], [], []
first = True
last_z = 0
n_multip = 0
for idx in range(len(track_z)):
z, x, width = x_data[idx], y_data[idx], y_width[idx]
dz = z - last_z
if n_multip != 0:
n_multip -= 1
continue
# if we have way too large gaps, just stop it
if np.abs(dz) > 150. and not first:
break
kf.F[0, 1] = dz
kf.R = width
kf.Q = Q_discrete_white_noise(2, dz, .05)
pred, _ = kf.get_prediction()
delta = np.abs(pred[0] - x)
if not first:
# print("{} value at: {}, {}".format(idx, last_z, z))
# if the prediction is too far off the expected, we assume this is an outlier
if delta > np.abs(dz) * 0.5 / 0.3:
continue
# if we have multiple hits on the wire, choose the closest one to the prediction as the next measurement
multip, n_multip = lark.check_multiplicity(track_z[idx:])
if multip:
# print("multi value: at {} {} {}".format(z, n_multip, track_x[idx:idx + n_multip+1]))
closest_idx, closest_val = lark.choose_nearest(
track_x[idx:idx + n_multip + 1], pred[0])
x = closest_val
kf.predict()
kf.update(x)
xs.append(kf.x.T[0])
ts.append(z)
pr.append(pred[0])
ps.append(kf.P.diagonal()[0])
first = False
last_z = z
return xs, ts, pr, ps
def test_false(self):
a = np.array([0, 1, 2, 2, 4])
flag, n = check_multiplicity(a)
assert (flag == False)
assert (n == 0)
def test_single_values(self):
a = np.array([1, 1, 1, 1])
flag, n = check_multiplicity(a)
assert (flag == True)
assert (n == 3)
def test_short(self):
a = np.array([1])
flag, n = check_multiplicity(a)
assert (flag == False)
assert (n == 0)
def test_true_multiple(self):
a = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2])
flag, n = check_multiplicity(a)
assert (flag == True)
assert (n == 8)
def test_true_one(self):
a = np.array([0, 0, 2, 2])
flag, n = check_multiplicity(a)
assert (flag == True)
assert (n == 1)