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