def test_ukf_estimate_map(self):
residual = np.array([[0.06583802], [0.30857565]])
P_zz = np.asarray([[1.01043425e-02, 3.41826761e-05],
[3.41826761e-05, 1.00712847e-02]])
R = estimate_noise_ukf_map(residual, P_zz, 0.5, np.ones((2, 2)), False)
assert R.shape == (2, 2)
R = estimate_noise_ukf_map(residual, P_zz, 0.5, np.ones((2, 2)), True)
assert R.shape == (2, 2)
def perform_estimation(residual, tracker, index,
old_estimate, old_mean):
average_factor = (1 - b) / (1 - b**(index + 1))
mean = (1-average_factor) * old_mean + average_factor * residual
residual -= mean
estimate = estimate_noise_ukf_map(residual, tracker.Pz, average_factor,
old_estimate, False)
return estimate, mean
def filtering(sim, tracker):
# perform sensor simulation and filtering
(readings, truths, filtered, residuals, Ps,
map_estimations, map_estimations_convergence) = (
[], [], [], [], [], [0], [0])
cmds = [np.array([[0.6],
[0.23]])] * (num_samples / 4)
cmds.extend([np.array([[2],
[-0.20]])] * (num_samples / 2))
cmds.extend([np.array([[1],
[0.2]])] * (num_samples / 4))
for index, cmd in enumerate(cmds):
sim.step(cmd)
R = R_proto * measurement_var
reading = sim.read(R)
tracker.predict(fx_args=cmd)
tracker.update(reading[:, 0])
readings.append(reading)
truths.append(sim.x)
filtered.append(copy(tracker.x))
Ps.append(copy(tracker.P))
residual = tracker.y[:, np.newaxis]
residuals.append(residual)
starting_index = skip_initial
if index > starting_index:
average_factor = (1 - map_b) / (1 -
map_b**(index - starting_index))
estimate = estimate_noise_ukf_map(
residual, tracker.Pz, average_factor,
map_estimations[-1], False)
estimate_convergence = estimate_noise_ukf_map(
residual, tracker.Pz, average_factor,
map_estimations_convergence[-1], True)
map_estimations.append(estimate)
map_estimations_convergence.append(estimate_convergence)
readings = np.asarray(readings)
truths = np.asarray(truths)
filtered = np.asarray(filtered)
Ps = np.asarray(Ps)
residuals = np.asarray(residuals)
return (readings, truths, filtered, residuals, Ps,
map_estimations[-1], map_estimations_convergence[-1])
def filtering(sim, tracker):
R = R_proto * sim_var
readings, filtered, residuals, Ps, map_estimations = [], [], [], [], [0]
r_mean = 0
estimation_started = False
for index in range(sample_size + skip_samples):
time, reading = sim.read(R)
measurement = reading[0:2]
controls = reading[2:]
# skip low velocities
if measurement[1, 0] < 0.5:
continue
tracker.predict(fx_args=controls)
tracker.update(measurement[:, 0])
readings.append(reading)
filtered.append(copy(tracker.x[:, np.newaxis]))
Ps.append(copy(tracker.P))
residual = tracker.y[:, np.newaxis]
residuals.append(residual)
if index < skip_samples:
continue
if not estimation_started:
starting_index = index
estimation_started = True
average_factor = (1 - map_b) / (1 -
map_b**(index - starting_index + 1))
r_mean = (1-average_factor) * r_mean + average_factor * residual
residual -= r_mean
estimate = estimate_noise_ukf_map(
residual, tracker.Pz, average_factor,
map_estimations[-1])
map_estimations.append(estimate)
readings = np.asarray(readings)
filtered = np.asarray(filtered)
residuals = np.asarray(residuals)
Ps = np.asarray(Ps)
return readings, filtered, residuals, Ps, map_estimations[-1]
def perform_estimation(residual, tracker, index, old_estimate):
average_factor = (1 - 0.95) / (1 - 0.95**(index + 1))
estimate = estimate_noise_ukf_map(residual, tracker.Pz, average_factor,
old_estimate, False)
return estimate