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)