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_negative_vals(self):
a = np.array([-1., 2., 3.])
val = 0.
idx, val = choose_nearest(a, val)
assert (idx == 0)
assert (val == -1.)
def test_multiple(self):
a = np.array([1., 1., 1.])
val = 2.
idx, val = choose_nearest(a, val)
assert (idx == 0)
assert (val == 1.)
def test_simple(self):
a = np.array([1., 2., 3.])
val = 2.
idx, val = choose_nearest(a, val)
assert (idx == 1)
assert (val == 2.)