def gen_dummy_data(num_samples, sampling_period, meas_model, R): omega = np.zeros((num_samples + 1)) omega[200:401] = -np.pi / 201 / sampling_period # Initial state initial_state = np.array([0, 0, 20, 0, omega[0]]) # Allocate memory true_states = np.zeros((num_samples + 1, initial_state.shape[0])) true_states[0, :] = initial_state coord_turn = CoordTurn(sampling_period, None) # Create true track for k in range(1, num_samples + 1): new_state = coord_turn.mean(true_states[k - 1, :]) true_states[k, :] = new_state true_states[k, 4] = omega[k] return true_states, gen_non_lin_meas(true_states, meas_model, R)
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) _, measurements, ss_mf, _ = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.Extended, None) measurements = measurements[:, :2] ekf = Ekf(motion_model, meas_model) mf, Pf, _, _ = ekf.filter_seq(measurements, prior_mean, prior_cov) self.assertTrue(np.allclose(mf, ss_mf))
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) _, measurements, _, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, 10) measurements = measurements[:, :2] self.assertAlmostEqual( analytical_smoothing_cost(ss_ms, measurements, prior_mean, prior_cov, motion_model, meas_model), 1.039569495177240e03, )
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, :2] cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) ieks = Ieks(motion_model, meas_model, num_iter) mf, Pf, ms, Ps, _iter_cost = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertTrue(np.allclose(mf, ss_mf)) self.assertTrue(np.allclose(ms, ss_ms)) num_iter = 10 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, :2] ieks = Ieks(motion_model, meas_model, num_iter=num_iter) mf, Pf, ms, Ps, _iter_cost = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertTrue(np.allclose(mf, ss_mf)) self.assertTrue(np.allclose(ms, ss_ms))
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorBearings(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 10 _, measurements, _, _ = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, 2:] K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) dir_der_fn = partial( dir_der_analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ls_method = ArmijoLineSearch(cost_fn, dir_der_fn, c_1=0.1) ls_ieks = LsIeks(motion_model, meas_model, num_iter, ls_method) mf, Pf, ms, Ps, _iter_cost = ls_ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertAlmostEqual(ms.sum(), 1.223943641943313e03)
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, :2] cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) ieks = Ieks(motion_model, meas_model, num_iter) _, _, x_0, _, _ = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) _, _, x_1, _, _ = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, (x_0, None), 1, cost_fn) self.assertEqual(x_0.shape, x_1.shape) num_grid_points = 10 ls = GridSearch(cost_fn, num_grid_points) next_x, alpha, _ = ls.search_next(x_0, x_1) self.assertEqual(next_x.shape, x_0.shape) self.assertTrue(np.allclose(next_x, x_0 + alpha * (x_1 - x_0)))
def test_lambda_zero_results_in_plain_ieks(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 3 states, measurements = simulate_data(motion_model, meas_model, prior_mean[:-1], 20) lambda_ = 0.0 nu = 10 cost_improv_iter_lim = 10 cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) ieks = Ieks(motion_model, meas_model, num_iter) mf, Pf, ms, Ps, _iter_cost = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) lm_ieks = LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim, lambda_, nu) lm_mf, lm_Pf, lm_ms, lm_Ps, _iter_cost = lm_ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertTrue(np.allclose(mf, lm_mf)) self.assertTrue(np.allclose(ms, lm_ms))
def test_lambda_zero_results_in_plain_ipls(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 3 states, measurements = simulate_data(motion_model, meas_model, prior_mean[:-1], 20) sigma_point_method = SphericalCubature() cost_fn = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) ipls = SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter) mf, Pf, ms, Ps, _ = ipls.filter_and_smooth(measurements, prior_mean, prior_cov, cost_fn) lm_ipls = SigmaPointLmIpls(motion_model, meas_model, sigma_point_method, num_iter, 10, 0.0, 10) lm_mf, lm_Pf, lm_ms, lm_Ps, _ = lm_ipls.filter_and_smooth( measurements, prior_mean, prior_cov, cost_fn) # lm_ieks = LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim, lambda_, nu) # lm_mf, lm_Pf, lm_ms, lm_Ps, _iter_cost = lm_ieks.filter_and_smooth_with_init_traj( # measurements, prior_mean, prior_cov, init_traj, 1, cost_fn # ) self.assertTrue(np.allclose(mf, lm_mf)) self.assertTrue(np.allclose(ms, lm_ms))
def test_cmp_time_indep_and_time_dep(self): dt = 0.01 time_steps = 2 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) states, measurements = simulate_data(motion_model, meas_model, prior_mean[:-1], time_steps) a = 1 + dt * 10 * np.cumsum(np.random.randn(1, time_steps)) states = np.column_stack((states, a)) measurements = measurements[:, :2] self.assertAlmostEqual( analytical_smoothing_cost(states, measurements, prior_mean, prior_cov, motion_model, meas_model), analytical_smoothing_cost_time_dep(states, measurements, prior_mean, prior_cov, motion_model, meas_model), )
def test_cmp_dir_der_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorBearings(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, 2:] K = measurements.shape[0] D_x = prior_mean.shape[0] init_traj = (np.zeros((K, D_x)), None) # Test with simple search dir # From SS matlab impl # He does not use scale the cost with 1/2, making the ref value twice bigger than our target. search_dir = np.ones((K, D_x)) ref_val = 4.423579239781962e02 comp_dir_dev = dir_der_analytical_smoothing_cost( init_traj[0], search_dir, measurements, prior_mean, prior_cov, motion_model, meas_model) self.assertAlmostEqual(ref_val, 2 * comp_dir_dev) # comp_grad = grad_analytical_smoothing_cost( # init_traj[0], measurements, prior_mean, prior_cov, motion_model, meas_model # ) # search_dir = np.ones((K * D_x,)) # self.assertAlmostEqual(ref_val, search_dir.T @ comp_grad * 2) # Test with actual search dir cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) m_0 = init_traj[0] ieks = Ieks(motion_model, meas_model, num_iter) _, _, m_1, _, _ = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) # Sanity check that we use the correct points to test the line search. self.assertAlmostEqual(m_1.sum(), -23.808794681191973) search_dir = m_1 - m_0 ref_val = -6.406867572871646e02 comp_dir_dev = dir_der_analytical_smoothing_cost( m_0, search_dir, measurements, prior_mean, prior_cov, motion_model, meas_model) self.assertAlmostEqual(ref_val, 2 * comp_dir_dev) # comp_grad = grad_analytical_smoothing_cost(m_0, measurements, prior_mean, prior_cov, motion_model, meas_model) # self.assertAlmostEqual(ref_val, search_dir.flatten() @ comp_grad * 2) _, _, m_2, _, _ = ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) # Sanity check that we use the correct points to test the line search. self.assertAlmostEqual(m_2.sum(), 4.551162489010954e03) search_dir = m_2 - m_1 ref_val = -42.612770542340790 comp_dir_dev = dir_der_analytical_smoothing_cost( m_1, search_dir, measurements, prior_mean, prior_cov, motion_model, meas_model) self.assertAlmostEqual(ref_val, 2 * comp_dir_dev)
def main(): log = logging.getLogger(__name__) experiment_name = "tunnel_simulation" setup_logger(f"logs/{experiment_name}.log", logging.DEBUG) log.info(f"Running experiment: {experiment_name}") np.random.seed(2) num_iter = 10 # Motion model sampling_period = 0.1 v_scale = 7 omega_scale = 15 sigma_v = v_scale * 1 sigma_omega = omega_scale * np.pi / 180 eps = 0.1 Q = np.diag([eps, eps, sampling_period * sigma_v ** 2, eps, sampling_period * sigma_omega ** 2]) motion_model = CoordTurn(sampling_period, Q) # Meas model pos = np.array([100, -100]) # sigma_r = 2 # sigma_phi = 0.5 * np.pi / 180 noise_factor = 4 sigma_r = 2 * noise_factor sigma_phi = noise_factor * 0.5 * np.pi / 180 R = np.diag([sigma_r ** 2, sigma_phi ** 2]) meas_model = RangeBearing(pos, R) # Generate data range_ = (0, None) tunnel_segment = [140, 175] # tunnel_segment = [None, None] states, measurements = get_states_and_meas(meas_model, R, range_, tunnel_segment) measurements = [meas for meas in measurements] cartes_meas = np.apply_along_axis(partial(to_cartesian_coords, pos=pos), 1, measurements) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) lambda_ = 1e-2 nu = 10 grid_search_points = 10 results = [] cost_fn_eks = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) sigma_point_method = SphericalCubature() cost_fn_ipls = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) ms_gn_ieks, Ps_gn_ieks, cost_ieks, rmses_ieks, neeses_ieks = run_smoothing( Ieks(motion_model, meas_model, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_eks ) results.append((ms_gn_ieks, Ps_gn_ieks, cost_ieks[1:], "IEKS")) ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, rmses_lm_ieks, neeses_lm_ieks = run_smoothing( LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) results.append((ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks[1:], "LM-IEKS")) ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks, tmp_rmse, tmp_nees = run_smoothing( LsIeks(motion_model, meas_model, num_iter, GridSearch(cost_fn_eks, grid_search_points)), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) results.append((ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks[1:], "LS-IEKS")) ms_gn_ipls, Ps_gn_ipls, cost_ipls, rmses_ipls, neeses_ipls = run_smoothing( SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_ipls, ) results.append((ms_gn_ipls, Ps_gn_ipls, cost_ipls[1:], "IPLS")) ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, rmses_lm_ipls, neeses_lm_ipls = run_smoothing( SigmaPointLmIpls( motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu ), states, measurements, prior_mean, prior_cov, cost_fn_ipls, ) results.append((ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls[1:], "LM-IPLS")) ls_cost_fn = partial( slr_smoothing_cost_means, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), motion_fn=motion_model.map_set, meas_fn=meas_model.map_set, slr_method=SigmaPointSlr(sigma_point_method), ) ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, tmp_rmse, tmp_nees = run_smoothing( SigmaPointLsIpls(motion_model, meas_model, sigma_point_method, num_iter, GridSearch, grid_search_points), states, measurements, prior_mean, prior_cov, ls_cost_fn, ) results.append((ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls[1:], "LS-IPLS")) plot_results( states, results, cartes_meas, skip_cov=10, ) plot_metrics( [ (cost_ieks[1:], "IEKS"), (cost_lm_ieks[1:], "LM-IEKS"), (cost_ipls[1:], "IPLS"), (cost_lm_ipls[0:], "LM-IPLS"), ], [ (rmses_ieks, "IEKS"), (rmses_lm_ieks, "LM-IEKS"), (rmses_ipls, "IPLS"), (rmses_lm_ipls, "LM-IPLS"), ], [ (neeses_ieks, "IEKS"), (neeses_lm_ieks, "LM-IEKS"), (neeses_ipls, "IPLS"), (neeses_lm_ipls, "LM-IPLS"), ], )
def main(): dt = 0.01 qc = 0.01 qw = 10 Q = np.array( [ [qc * dt ** 3 / 3, 0, qc * dt ** 2 / 2, 0, 0], [0, qc * dt ** 3 / 3, 0, qc * dt ** 2 / 2, 0], [qc * dt ** 2 / 2, 0, qc * dt, 0, 0], [0, qc * dt ** 2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ] ) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std ** 2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) _, measurements, _, ss_ms = get_specific_states_from_file(Path.cwd() / "data/lm_ieks_paper", Type.GN, 10) measurements = measurements[:, :2] K = measurements.shape[0] covs = np.array([prior_cov] * K) slr_cache = SlrCache(motion_model.map_set, meas_model.map_set, SigmaPointSlr(SphericalCubature())) slr_cache.update(ss_ms, covs) new_proto = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, ) new = partial( new_proto, traj=ss_ms, proc_bar=slr_cache.proc_bar, meas_bar=slr_cache.meas_bar, proc_cov=np.array( [err_cov_k + motion_model.proc_noise(k) for k, (_, _, err_cov_k) in enumerate(slr_cache.proc_lin)] ), meas_cov=np.array( [err_cov_k + meas_model.meas_noise(k) for k, (_, _, err_cov_k) in enumerate(slr_cache.meas_lin)] ), ) old = partial( slr_smoothing_cost, ss_ms, covs, measurements, prior_mean, prior_cov, motion_model, meas_model, SigmaPointSlr(SphericalCubature()), ) num_samples = 10 old_time = timeit(old, number=num_samples) / num_samples print(old_time) new_time = timeit(new, number=num_samples) / num_samples print(new_time) assert np.allclose(new(), old())
def main(): args = parse_args() log = logging.getLogger(__name__) experiment_name = "analyse_tricky_ct" setup_logger(f"logs/{experiment_name}.log", logging.DEBUG) log.info(f"Running experiment: {experiment_name}") dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) lambda_ = 1e-2 num_iter = args.num_iter meas_model = MultiSensorBearings(sensors, R) data_dir = Path.cwd() / "data/lm_ieks_paper/tricky" states = np.genfromtxt(data_dir / "lm_div_states.csv", dtype=float) measurements = np.genfromtxt(data_dir / "lm_div_meas.csv", dtype=float) estimates = [] costs = [] neeses = [] rmses = [] cost_fn_eks = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ms_ieks, Ps_ieks, cost_ieks, rmses_ieks, neeses_ieks = run_smoothing( Ieks(motion_model, meas_model, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) estimates.append((ms_ieks, Ps_ieks, cost_ieks[1:], "IEKS"), ) costs.append((cost_ieks, "IEKS"), ) rmses.append((rmses_ieks, "IEKS"), ) neeses.append((neeses_ieks, "IEKS"), ) ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, rmses_lm_ieks, neeses_lm_ieks = run_smoothing( LmIeks(motion_model, meas_model, num_iter, 10, lambda_, 10), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) estimates.append((ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks[1:], "LM-IEKS"), ) costs.append((cost_lm_ieks, "LM-IEKS"), ) rmses.append((rmses_lm_ieks, "LM-IEKS"), ) neeses.append((neeses_lm_ieks, "LM-IEKS"), ) # ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks, rmses_ls_ieks, neeses_ls_ieks = run_smoothing( # LsIeks(motion_model, meas_model, num_iter, GridSearch(cost_fn_eks, 20)), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_eks, # ) # estimates.append( # (ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks[1:], "LS-IEKS"), # ) # costs.append( # (cost_ls_ieks, "LS-IEKS"), # ) # rmses.append( # (rmses_ls_ieks, "LS-IEKS"), # ) # neeses.append( # (neeses_ls_ieks, "LS-IEKS"), # ) # sigma_point_method = SphericalCubature() # ls_cost_fn = partial( # slr_smoothing_cost_means, # measurements=measurements, # m_1_0=prior_mean, # P_1_0_inv=np.linalg.inv(prior_cov), # motion_fn=motion_model.map_set, # meas_fn=meas_model.map_set, # slr_method=SigmaPointSlr(sigma_point_method), # ) # pre_comp_cost = partial( # slr_smoothing_cost_pre_comp, # measurements=measurements, # m_1_0=prior_mean, # P_1_0_inv=np.linalg.inv(prior_cov), # ) # ms_ipls, Ps_ipls, cost_ipls, rmses_ipls, neeses_ipls = run_smoothing( # SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), # states, # measurements, # prior_mean, # prior_cov, # pre_comp_cost, # ) # estimates.append( # (ms_ipls, Ps_ipls, cost_ipls, "IPLS"), # ) # costs.append( # (cost_ipls, "IPLS"), # ) # rmses.append( # (rmses_ipls, "IPLS"), # ) # neeses.append( # (neeses_ipls, "IPLS"), # ) # ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, rmses_lm_ipls, neeses_lm_ipls = run_smoothing( # SigmaPointLmIpls( # motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=10 # ), # states, # measurements, # prior_mean, # prior_cov, # pre_comp_cost, # ) # estimates.append( # (ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, "LM-IPLS"), # ) # costs.append( # (cost_lm_ipls, "LM-IPLS"), # ) # rmses.append( # (rmses_lm_ipls, "LM-IPLS"), # ) # neeses.append( # (neeses_lm_ipls, "LM-IPLS"), # ) # ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, rmses_ls_ipls, neeses_ls_ipls = run_smoothing( # SigmaPointLsIpls(motion_model, meas_model, sigma_point_method, num_iter, GridSearch, 10), # states, # measurements, # prior_mean, # prior_cov, # ls_cost_fn, # ) # estimates.append( # (ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, "LS-IPLS"), # ) # costs.append( # (cost_ls_ipls, "LS-IPLS"), # ) # rmses.append( # (rmses_ls_ipls, "LS-IPLS"), # ) # neeses.append( # (neeses_ls_ipls, "LS-IPLS"), # ) plot_scalar_metric(costs, "Cost") plot_scalar_metric(neeses, "NEES") plot_scalar_metric(rmses, "RMSE") plot_results( states, estimates, None, )
def main(): log = logging.getLogger(__name__) args = parse_args() experiment_name = "tunnel_simulation" setup_logger(f"logs/{experiment_name}.log", logging.INFO) log.info(f"Running experiment: {experiment_name}") np.random.seed(2) num_iter = args.num_iter # Motion model sampling_period = 0.1 v_scale = 7 omega_scale = 15 sigma_v = v_scale * 1 sigma_omega = omega_scale * np.pi / 180 eps = 0.1 Q = np.diag([ eps, eps, sampling_period * sigma_v**2, eps, sampling_period * sigma_omega**2 ]) motion_model = CoordTurn(sampling_period, Q) # Meas model pos = np.array([100, -100]) # sigma_r = 2 # sigma_phi = 0.5 * np.pi / 180 noise_factor = 4 sigma_r = 2 * noise_factor sigma_phi = noise_factor * 0.5 * np.pi / 180 R = np.diag([sigma_r**2, sigma_phi**2]) meas_model = RangeBearing(pos, R) # Generate data range_ = (0, None) tunnel_segment = [140, 175] # tunnel_segment = [None, None] prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) lambda_ = 1e-2 nu = 10 grid_search_points = 10 num_mc_samples = args.num_mc_samples rmses_ieks = np.zeros((num_mc_samples, num_iter)) rmses_lm_ieks = np.zeros((num_mc_samples, num_iter)) rmses_ls_ieks = np.zeros((num_mc_samples, num_iter)) rmses_ipls = np.zeros((num_mc_samples, num_iter)) rmses_lm_ipls = np.zeros((num_mc_samples, num_iter)) rmses_ls_ipls = np.zeros((num_mc_samples, num_iter)) neeses_gn_ieks = np.zeros((num_mc_samples, num_iter)) neeses_lm_ieks = np.zeros((num_mc_samples, num_iter)) neeses_ls_ieks = np.zeros((num_mc_samples, num_iter)) neeses_gn_ipls = np.zeros((num_mc_samples, num_iter)) neeses_lm_ipls = np.zeros((num_mc_samples, num_iter)) neeses_ls_ipls = np.zeros((num_mc_samples, num_iter)) for mc_iter in range(num_mc_samples): log.info(f"MC iter: {mc_iter+1}/{num_mc_samples}") states, measurements = get_states_and_meas(meas_model, R, range_, tunnel_segment) cost_fn_eks = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) sigma_point_method = SphericalCubature() cost_fn_ipls = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) ms_gn_ieks, Ps_gn_ieks, cost_gn_ieks, tmp_rmse, tmp_nees = run_smoothing( Ieks(motion_model, meas_model, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_eks) rmses_ieks[mc_iter, :] = tmp_rmse neeses_gn_ieks[mc_iter, :] = tmp_nees ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, tmp_rmse, tmp_nees = run_smoothing( LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) rmses_lm_ieks[mc_iter, :] = tmp_rmse neeses_lm_ieks[mc_iter, :] = tmp_nees ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks, tmp_rmse, tmp_nees = run_smoothing( LsIeks(motion_model, meas_model, num_iter, GridSearch(cost_fn_eks, grid_search_points)), states, measurements, prior_mean, prior_cov, cost_fn_eks, ) rmses_ls_ieks[mc_iter, :] = tmp_rmse neeses_ls_ieks[mc_iter, :] = tmp_nees ms_gn_ipls, Ps_gn_ipls, cost_gn_ipls, tmp_rmse, tmp_nees = run_smoothing( SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), states, measurements, prior_mean, prior_cov, None, ) rmses_ipls[mc_iter, :] = tmp_rmse neeses_gn_ipls[mc_iter, :] = tmp_nees ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, tmp_rmse, tmp_nees = run_smoothing( SigmaPointLmIpls(motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu), states, measurements, prior_mean, prior_cov, cost_fn_ipls, ) rmses_lm_ipls[mc_iter, :] = tmp_rmse neeses_lm_ipls[mc_iter, :] = tmp_nees ls_cost_fn = partial( slr_smoothing_cost_means, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), motion_fn=motion_model.map_set, meas_fn=meas_model.map_set, slr_method=SigmaPointSlr(sigma_point_method), ) ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, tmp_rmse, tmp_nees = run_smoothing( SigmaPointLsIpls(motion_model, meas_model, sigma_point_method, num_iter, GridSearch, grid_search_points), states, measurements, prior_mean, prior_cov, ls_cost_fn, ) rmses_ls_ipls[mc_iter, :] = tmp_rmse neeses_ls_ipls[mc_iter, :] = tmp_nees label_ieks, label_lm_ieks, label_ls_ieks, label_ipls, label_lm_ipls, label_ls_ipls = ( "IEKS", "LM-IEKS", "LS-IEKS", "IPLS", "LM-IPLS", "LS-IPLS", ) rmse_stats = [ (rmses_ieks, label_ieks), (rmses_lm_ieks, label_lm_ieks), (rmses_ls_ieks, label_ls_ieks), (rmses_ipls, label_ipls), (rmses_lm_ipls, label_lm_ipls), (rmses_ls_ipls, label_ls_ipls), ] nees_stats = [ (neeses_gn_ieks, label_ieks), (neeses_lm_ieks, label_lm_ieks), (neeses_ls_ieks, label_ls_ieks), (neeses_gn_ipls, label_ipls), (neeses_lm_ipls, label_lm_ipls), (neeses_ls_ipls, label_ls_ipls), ] save_stats(Path.cwd() / "results" / experiment_name, "RMSE", rmse_stats) save_stats(Path.cwd() / "results" / experiment_name, "NEES", nees_stats) plot_scalar_metric_err_bar(rmse_stats, "RMSE") plot_scalar_metric_err_bar(nees_stats, "NEES")
self.assertEqual(next_x.shape, x_0.shape) self.assertTrue(np.allclose(next_x, x_0 + alpha * (x_1 - x_0))) if __name__ == "__main__": dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, num_iter) measurements = measurements[:, :2]
def main(): log = logging.getLogger(__name__) experiment_name = "lm_ieks" setup_logger(f"logs/{experiment_name}.log", logging.WARNING) log.info(f"Running experiment: {experiment_name}") dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 np.random.seed(0) states, all_meas, _, xs_ss = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, num_iter) K = all_meas.shape[0] covs = np.array([prior_cov] * K) * (0.90 + np.random.rand() / 5) meas_model = MultiSensorRange(sensors, R) measurements = all_meas[:, :2] cost_fn_eks = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) dir_der_eks = partial( dir_der_analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) sigma_point_method = SphericalCubature() cost_fn_ipls = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) time_ieks = partial( Ieks(motion_model, meas_model, num_iter).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, noop_cost, ) time_lm_ieks = partial( LmIeks(motion_model, meas_model, num_iter, 10, 1e-2, 10).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, cost_fn_eks, ) time_ls_ieks = partial( LsIeks( motion_model, meas_model, num_iter, ArmijoLineSearch(cost_fn_eks, dir_der_eks, c_1=0.1), ).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, cost_fn_eks, ) time_ipls = partial( SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, slr_noop_cost, ) time_lm_ipls = partial( SigmaPointLmIpls(motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=1e-2, nu=10).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, cost_fn_ipls, ) cost_fn_ls_ipls = partial( slr_smoothing_cost_means, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), motion_fn=motion_model.map_set, meas_fn=meas_model.map_set, slr_method=SigmaPointSlr(sigma_point_method), ) time_ls_ipls = partial( SigmaPointLsIpls(motion_model, meas_model, sigma_point_method, num_iter, partial(ArmijoLineSearch, c_1=0.1), 10).filter_and_smooth_with_init_traj, measurements, prior_mean, prior_cov, (xs_ss, covs), 1, cost_fn_ls_ipls, ) num_samples = 10 time_ieks = timeit(time_ieks, number=num_samples) / (num_iter * num_samples) time_lm_ieks = timeit(time_lm_ieks, number=num_samples) / (num_iter * num_samples) time_ls_ieks = timeit(time_ls_ieks, number=num_samples) / (num_iter * num_samples) time_ipls = timeit(time_ipls, number=num_samples) / (num_iter * num_samples) time_lm_ipls = timeit(time_lm_ipls, number=num_samples) / (num_iter * num_samples) time_ls_ipls = timeit(time_ls_ipls, number=num_samples) / (num_iter * num_samples) print(f"IEKS: {time_ieks:.2f} s, 100.0%") print(f"LM-IEKS: {time_lm_ieks:.2f} s, {time_lm_ieks/time_ieks*100:.2f}%") print(f"LS-IEKS: {time_ls_ieks:.2f} s, {time_ls_ieks/time_ieks*100:.2f}%") print(f"IPLS: {time_ipls:.2f} s, {time_ipls/time_ieks*100:.2f}%") print(f"LM-IPLS: {time_lm_ipls:.2f} s, {time_lm_ipls/time_ieks*100:.2f}%") print(f"LS-IPLS: {time_ls_ipls:.2f} s, {time_ls_ipls/time_ieks*100:.2f}%")
def main(): log = logging.getLogger(__name__) experiment_name = "coord_turn" setup_logger(f"logs/{experiment_name}.log", logging.INFO) log.info(f"Running experiment: {experiment_name}") np.random.seed(2) range_ = (0, -1) num_iter = 5 # Motion model sampling_period = 0.1 v_scale = 2 omega_scale = 2 sigma_v = v_scale * 1 sigma_omega = omega_scale * np.pi / 180 Q = np.diag([ 0, 0, sampling_period * sigma_v**2, 0, sampling_period * sigma_omega**2 ]) motion_model = CoordTurn(sampling_period, Q) # Meas model pos = np.array([100, -100]) sigma_r = 2 sigma_phi = 0.5 * np.pi / 180 R = np.diag([sigma_r**2, sigma_phi**2]) meas_model = RangeBearing(pos, R) # Generate data true_states, measurements = get_tricky_data(meas_model, R, range_) obs_dims = true_states.shape[1] cartes_meas = np.apply_along_axis(partial(to_cartesian_coords, pos=pos), 1, measurements) # Prior distr. prior_mean = np.array([4.4, 0, 4, 0, 0]) prior_cov = np.diag( [1**2, 1**2, 1**2, (5 * np.pi / 180)**2, (1 * np.pi / 180)**2]) cost_fn_ipls = partial( slr_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, slr=SigmaPointSlr(SphericalCubature()), ) smoother = SigmaPointIpls(motion_model, meas_model, SphericalCubature(), num_iter) mf, Pf, ms, Ps, _ = smoother.filter_and_smooth(measurements, prior_mean, prior_cov, cost_fn_ipls) vis.plot_nees_and_2d_est( true_states[range_[0]:range_[1], :], cartes_meas, [ (mf[:, :obs_dims], Pf[:, :obs_dims, :obs_dims], "filter"), (ms[:, :obs_dims], Ps[:, :obs_dims, :obs_dims], "smoother"), ], sigma_level=3, skip_cov=5, )
def main(): args = parse_args() log = logging.getLogger(__name__) experiment_name = "ct_varying_sens_metrics.py" setup_logger(f"logs/{experiment_name}.log", logging.INFO) log.info(f"Running experiment: {experiment_name}") if not args.random: np.random.seed(0) dt = 0.01 qc = 0.01 qw = 10 prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) D_x = prior_mean.shape[0] K = 500 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) std = 0.5 R_uncertain = std**2 * np.eye(2) double_sensors = np.row_stack((sens_pos_1, sens_pos_2)) double_meas_model = MultiSensorBearings(double_sensors, R_uncertain) single_sensor = sens_pos_2.reshape((1, 2)) std = 0.001 R_certain = std**2 * np.eye(1) single_meas_model = MultiSensorBearings(single_sensor, R_certain) # Create a set of time steps where the original two sensor measurements are replaced with single ones. # single_meas_time_steps = set(list(range(0, 100, 5))[1:]) single_meas_time_steps = set(list(range(0, K, 50))[1:]) meas_model = BearingsVaryingSensors(double_meas_model, single_meas_model, single_meas_time_steps) num_iter = args.num_iter num_mc_samples = args.num_mc_samples rmses_ieks = np.zeros((num_mc_samples, num_iter)) rmses_lm_ieks = np.zeros((num_mc_samples, num_iter)) rmses_ls_ieks = np.zeros((num_mc_samples, num_iter)) rmses_ipls = np.zeros((num_mc_samples, num_iter)) rmses_lm_ipls = np.zeros((num_mc_samples, num_iter)) rmses_ls_ipls = np.zeros((num_mc_samples, num_iter)) neeses_ieks = np.zeros((num_mc_samples, num_iter)) neeses_lm_ieks = np.zeros((num_mc_samples, num_iter)) neeses_ls_ieks = np.zeros((num_mc_samples, num_iter)) neeses_ipls = np.zeros((num_mc_samples, num_iter)) neeses_lm_ipls = np.zeros((num_mc_samples, num_iter)) neeses_ls_ipls = np.zeros((num_mc_samples, num_iter)) lambda_ = 1e-2 nu = 10 D_x = prior_mean.shape[0] K = 500 init_traj = (np.zeros((K, D_x)), np.array(K * [prior_cov])) for mc_iter in range(num_mc_samples): log.info(f"MC iter: {mc_iter+1}/{num_mc_samples}") if args.random: states, measurements = simulate_data(motion_model, double_meas_model, prior_mean[:-1], time_steps=K) else: states, all_meas, _, xs_ss = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, num_iter) measurements = all_meas[:, 2:] # Change measurments so that some come from the alternative model. measurements = modify_meas(measurements, states, meas_model, True) cost_fn_eks = partial( analytical_smoothing_cost_time_dep, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) print(measurements[0]) return # ms_ieks, Ps_ieks, cost_ieks, tmp_rmse, tmp_nees = run_smoothing( # Ieks(motion_model, meas_model, num_iter), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_eks, # init_traj, # ) # rmses_ieks[mc_iter, :] = tmp_rmse # neeses_ieks[mc_iter, :] = tmp_nees # ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, tmp_rmse, tmp_nees = run_smoothing( # LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_eks, # init_traj, # ) # rmses_lm_ieks[mc_iter, :] = tmp_rmse # neeses_lm_ieks[mc_iter, :] = tmp_nees dir_der_eks = partial( dir_der_analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks, tmp_rmse, tmp_nees = run_smoothing( LsIeks(motion_model, meas_model, num_iter, ArmijoLineSearch(cost_fn_eks, dir_der_eks, c_1=0.1)), states, measurements, prior_mean, prior_cov, cost_fn_eks, init_traj, ) rmses_ls_ieks[mc_iter, :] = tmp_rmse neeses_ls_ieks[mc_iter, :] = tmp_nees sigma_point_method = SphericalCubature() cost_fn_ipls = partial(slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov)) # ms_ipls, Ps_ipls, cost_ipls, tmp_rmse, tmp_nees = run_smoothing( # SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), # states, # measurements, # prior_mean, # prior_cov, # None, # init_traj, # ) # rmses_ipls[mc_iter, :] = tmp_rmse # neeses_ipls[mc_iter, :] = tmp_nees # ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, tmp_rmse, tmp_nees = run_smoothing( # SigmaPointLmIpls( # motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu # ), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_ipls, # init_traj, # ) # rmses_lm_ipls[mc_iter, :] = tmp_rmse # neeses_lm_ipls[mc_iter, :] = tmp_nees ls_cost_fn = partial( slr_smoothing_cost_means, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), motion_fn=motion_model.map_set, meas_fn=meas_model.map_set, slr_method=SigmaPointSlr(sigma_point_method), ) ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, tmp_rmse, tmp_nees = run_smoothing( SigmaPointLsIpls(motion_model, meas_model, sigma_point_method, num_iter, partial(ArmijoLineSearch, c_1=0.1), 10), states, measurements, prior_mean, prior_cov, ls_cost_fn, init_traj, ) rmses_ls_ipls[mc_iter, :] = tmp_rmse neeses_ls_ipls[mc_iter, :] = tmp_nees label_ieks, label_lm_ieks, label_ls_ieks, label_ipls, label_lm_ipls, label_ls_ipls = ( "IEKS", "LM-IEKS", "LS-IEKS", "IPLS", "LM-IPLS", "LS-IPLS", ) rmse_stats = [ # (rmses_ieks, label_ieks), # (rmses_lm_ieks, label_lm_ieks), (rmses_ls_ieks, label_ls_ieks), # (rmses_ipls, label_ipls), # (rmses_lm_ipls, label_lm_ipls), (rmses_ls_ipls, label_ls_ipls), ] nees_stats = [ # (neeses_ieks, label_ieks), # (neeses_lm_ieks, label_lm_ieks), (neeses_ls_ieks, label_ls_ieks), # (neeses_ipls, label_ipls), # (neeses_lm_ipls, label_lm_ipls), (neeses_ls_ipls, label_ls_ipls), ] save_stats(Path.cwd() / "results" / experiment_name, "RMSE", rmse_stats) save_stats(Path.cwd() / "results" / experiment_name, "NEES", nees_stats) tikz_stats(Path.cwd() / "tmp_results", "RMSE", rmse_stats) tikz_stats(Path.cwd() / "tmp_results", "NEES", nees_stats) plot_scalar_metric_err_bar(rmse_stats, "RMSE") plot_scalar_metric_err_bar(nees_stats, "NEES")
def main(): args = parse_args() log = logging.getLogger(__name__) experiment_name = "ct_experiment_realisation" setup_logger(f"logs/{experiment_name}.log", logging.DEBUG) log.info(f"Running experiment: {experiment_name}") if not args.random: np.random.seed(2) dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) lambda_ = 1e-0 num_iter = args.num_iter if args.meas_type == MeasType.Range: meas_model = MultiSensorRange(sensors, R) meas_cols = np.array([0, 1]) elif args.meas_type == MeasType.Bearings: meas_model = MultiSensorBearings(sensors, R) meas_cols = np.array([2, 3]) log.info("Generating states and measurements.") if args.random: states, measurements = simulate_data(motion_model, meas_model, prior_mean[:-1], time_steps=500) else: states, all_meas, _, xs_ss = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, num_iter) measurements = all_meas[:, meas_cols] if args.var_sensors: single_sensor = sens_pos_2.reshape((1, 2)) std = 0.001 R_certain = std**2 * np.eye(1) single_meas_model = MultiSensorBearings(single_sensor, R_certain) # Create a set of time steps where the original two sensor measurements are replaced with single ones. # single_meas_time_steps = set(list(range(0, 100, 5))[1:]) single_meas_time_steps = set(list(range(0, 500, 50))[1:]) meas_model = BearingsVaryingSensors(meas_model, single_meas_model, single_meas_time_steps) # Change measurments so that some come from the alternative model. measurements = modify_meas(measurements, states, meas_model, True) results = [] cost_fn_eks = partial( analytical_smoothing_cost_time_dep, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) D_x = prior_mean.shape[0] K = len(measurements) init_traj = (np.zeros((K, D_x)), np.array(K * [prior_cov])) # log.info("Running IEKS...") # ms_ieks, Ps_ieks, cost_ieks, rmses_ieks, neeses_ieks = run_smoothing( # Ieks(motion_model, meas_model, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_eks, init_traj # ) # results.append( # (ms_ieks, Ps_ieks, cost_ieks[1:], "IEKS"), # ) # log.info("Running LM-IEKS...") # ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, rmses_lm_ieks, neeses_lm_ieks = run_smoothing( # LmIeks(motion_model, meas_model, num_iter, 10, lambda_, 10), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_eks, # init_traj, # ) # results.append( # (ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks[1:], "LM-IEKS"), # ) log.info("Running LS-IEKS...") dir_der_eks = partial( dir_der_analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks, rmses_ls_ieks, neeses_ls_ieks = run_smoothing( LsIeks( motion_model, meas_model, num_iter, ArmijoLineSearch(cost_fn_eks, dir_der_eks, c_1=0.1), ), states, measurements, prior_mean, prior_cov, cost_fn_eks, init_traj, ) results.append((ms_ls_ieks, Ps_ls_ieks, cost_ls_ieks[1:], "LS-IEKS"), ) sigma_point_method = SphericalCubature() # cost_fn_ipls = partial( # slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov) # ) # log.info("Running IPLS...") # ms_ipls, Ps_ipls, cost_ipls, rmses_ipls, neeses_ipls = run_smoothing( # SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), # states, # measurements, # prior_mean, # prior_cov, # None, # init_traj, # ) # results.append( # (ms_ipls, Ps_ipls, cost_ipls, "IPLS"), # ) # log.info("Running LM-IPLS...") # ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, rmses_lm_ipls, neeses_lm_ipls = run_smoothing( # SigmaPointLmIpls( # motion_model, meas_model, sigma_point_method, num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=10 # ), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_ipls, # init_traj, # ) # results.append( # (ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls, "LM-IPLS"), # ) # cost_fn_ls_ipls = partial( # slr_smoothing_cost_means, # measurements=measurements, # m_1_0=prior_mean, # P_1_0_inv=np.linalg.inv(prior_cov), # motion_fn=motion_model.map_set, # meas_fn=meas_model.map_set, # slr_method=SigmaPointSlr(sigma_point_method), # ) # log.info("Running LS-IPLS...") # ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, rmses_ls_ipls, neeses_ls_ipls = run_smoothing( # SigmaPointLsIpls( # motion_model, meas_model, sigma_point_method, num_iter, partial(ArmijoLineSearch, c_1=0.1), 10 # ), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_ls_ipls, # init_traj, # ) # results.append( # (ms_ls_ipls, Ps_ls_ipls, cost_ls_ipls, "LS-IPLS"), # ) for ms, _, _, label in results: tikz_2d_traj(Path.cwd() / "tikz", ms[:, :2], label) plot_results( states, results, None, )
def test_cmp_with_ss_impl(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) num_iter = 1 states, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, num_iter) measurements = measurements[:, :2] lambda_ = 1e-2 nu = 10 cost_improv_iter_lim = 10 cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) lm_ieks = LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim, lambda_, nu) mf, Pf, ms, Ps, _iter_cost = lm_ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertTrue(np.allclose(mf, ss_mf)) self.assertTrue(np.allclose(ms, ss_ms)) num_iter = 10 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, num_iter) measurements = measurements[:, :2] lm_ieks = LmIeks(motion_model, meas_model, num_iter, cost_improv_iter_lim, lambda_, nu) mf, Pf, ms, Ps, _iter_cost = lm_ieks.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn) self.assertTrue(np.allclose(mf, ss_mf, atol=1e-5)) self.assertTrue(np.allclose(ms, ss_ms)) # Summation over the time steps and columns of the cov seq. matlab_covs_sum = np.array([1.8843, 0.3417, 20.4884, 2.2148, 498.6999]) self.assertTrue( np.allclose(Ps.sum(0).sum(1), matlab_covs_sum, rtol=1e-4, atol=1e-4))
def main(): dt = 0.01 qc = 0.01 qw = 10 Q = np.array( [ [qc * dt ** 3 / 3, 0, qc * dt ** 2 / 2, 0, 0], [0, qc * dt ** 3 / 3, 0, qc * dt ** 2 / 2, 0], [qc * dt ** 2 / 2, 0, qc * dt, 0, 0], [0, qc * dt ** 2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ] ) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std ** 2 * np.eye(2) meas_model = MultiSensorBearings(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) # Weird params # prior_mean = np.array([-1, -1, 0, 0, 0]) # prior_cov = np.eye(5) num_iter = 2 _, measurements, ss_mf, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LineSearch, num_iter ) measurements = measurements[:, 2:] K = measurements.shape[0] init_traj = (np.zeros((K, prior_mean.shape[0])), None) _, _, ms_ss, Ps_ss = ls_ieks( measurements, prior_mean, prior_cov, Q, R, motion_model.mapping, partial(motion_model.jacobian, _time_step=0), meas_model.mapping, partial(meas_model.jacobian, time_step=0), num_iter, init_traj[0], 10, ) cost_fn = partial( analytical_smoothing_cost, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ls_method = GridSearch(cost_fn, 10) smoother = LsIeks(motion_model, meas_model, num_iter, ls_method) _, _, ms, Ps, costs = smoother.filter_and_smooth_with_init_traj( measurements, prior_mean, prior_cov, init_traj, 1, cost_fn )
def main(): log = logging.getLogger(__name__) experiment_name = "tunnel_simulation" setup_logger(f"logs/{experiment_name}.log", logging.WARNING) log.info(f"Running experiment: {experiment_name}") np.random.seed(2) num_iter = 3 # Meas model pos = np.array([100, -100]) # sigma_r = 2 # sigma_phi = 0.5 * np.pi / 180 sigma_r = 4 sigma_phi = 1 * np.pi / 180 R = np.diag([sigma_r**2, sigma_phi**2]) meas_model = RangeBearing(pos, R) # Generate data range_ = (0, None) tunnel_segment = [145, 165] # tunnel_segment = [None, None] states, measurements = get_states_and_meas(meas_model, R, range_, tunnel_segment) cartes_meas = np.apply_along_axis(partial(to_cartesian_coords, pos=pos), 1, measurements) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) results = [] sigma_point_method = SphericalCubature() # cost_fn_ipls = partial( # slr_smoothing_cost_pre_comp, # measurements=measurements, # m_1_0=prior_mean, # P_1_0=prior_cov, # motion_model=motion_model, # meas_model=meas_model, # slr=SigmaPointSlr(sigma_point_method), # ) vs = np.array([3, 4, 5, 6, 7]) os = np.array([15, 17.5, 20, 22.5, 25]) rmses = np.empty((vs.shape[0], os.shape[0])) sampling_period = 0.1 eps = 0.1 # v_scale = 2 # omega_scale = 2 for v_iter, v_scale in enumerate(vs): for o_iter, omega_scale in enumerate(os): # Motion model sigma_v = v_scale * 1 sigma_omega = omega_scale * np.pi / 180 Q = np.diag([ eps, eps, sampling_period * sigma_v**2, eps, sampling_period * sigma_omega**2 ]) motion_model = CoordTurn(sampling_period, Q) cost_fn_eks = partial( analytical_smoothing_cost, meas=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ms_gn_ieks, Ps_gn_ieks, cost_gn_ieks, tmp_rmse, tmp_nees = run_smoothing( Ieks(motion_model, meas_model, num_iter), states, measurements, prior_mean, prior_cov, cost_fn_eks) tmp = rmse(ms_gn_ieks[:, :2], states) print(v_scale, omega_scale, tmp) rmses[v_iter, o_iter] = tmp fig = plt.figure() ax = fig.gca(projection="3d") X, Y = np.meshgrid(vs, os) surf = ax.plot_surface(X, Y, rmses, linewidth=0, antialiased=False) from matplotlib.ticker import LinearLocator, FormatStrFormatter ax.zaxis.set_major_locator(LinearLocator(10)) ax.zaxis.set_major_formatter(FormatStrFormatter("%.02f")) ax.set_xlabel("v") ax.set_ylabel("o") # Add a color bar which maps values to colors. fig.colorbar(surf, shrink=0.5, aspect=5) plt.show() results.append((ms_gn_ieks, Ps_gn_ieks, cost_gn_ieks[1:], "GN-IEKS")) # ms_gn_ipls, Ps_gn_ipls, cost_gn_ipls, rmses_gn_ipls, neeses_gn_ipls = run_smoothing( # SigmaPointIpls(motion_model, meas_model, sigma_point_method, num_iter), # states, # measurements, # prior_mean, # prior_cov, # cost_fn_ipls, # None, # ) # results.append((ms_lm_ipls, Ps_lm_ipls, cost_lm_ipls[1:], "LM-IPLS")) plot_results( states, results, cartes_meas, ) plot_metrics( [ (cost_gn_ieks[1:], "GN-IEKS"), (cost_lm_ieks[1:], "LM-IEKS"), (cost_gn_ipls[1:], "GN-IPLS"), (cost_lm_ipls[1:], "LM-IPLS"), ], [ (rmses_gn_ieks, "GN-IEKS"), (rmses_lm_ieks, "LM-IEKS"), (rmses_gn_ipls, "LM-IPLS"), (rmses_lm_ipls, "LM-IPLS"), ], [ (neeses_gn_ieks, "GN-IEKS"), (neeses_lm_ieks, "LM-IEKS"), (neeses_gn_ipls, "LM-IPLS"), (neeses_lm_ipls, "LM-IPLS"), ], )
def test_cmp_slr_costs(self): dt = 0.01 qc = 0.01 qw = 10 Q = np.array([ [qc * dt**3 / 3, 0, qc * dt**2 / 2, 0, 0], [0, qc * dt**3 / 3, 0, qc * dt**2 / 2, 0], [qc * dt**2 / 2, 0, qc * dt, 0, 0], [0, qc * dt**2 / 2, 0, qc * dt, 0], [0, 0, 0, 0, dt * qw], ]) motion_model = CoordTurn(dt, Q) sens_pos_1 = np.array([-1.5, 0.5]) sens_pos_2 = np.array([1, 1]) sensors = np.row_stack((sens_pos_1, sens_pos_2)) std = 0.5 R = std**2 * np.eye(2) meas_model = MultiSensorRange(sensors, R) prior_mean = np.array([0, 0, 1, 0, 0]) prior_cov = np.diag([0.1, 0.1, 1, 1, 1]) _, measurements, _, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.LM, 10) measurements = measurements[:, :2] K = measurements.shape[0] np.random.seed(0) covs = np.array([prior_cov] * K) * (0.90 + np.random.rand() / 5) slr_cache = SlrCache(motion_model, meas_model, SigmaPointSlr(SphericalCubature())) pre_comp_proto = partial( slr_smoothing_cost_pre_comp, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) on_the_fly = partial( slr_smoothing_cost, covs=covs, measurements=measurements, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, slr=SigmaPointSlr(SphericalCubature()), ) slr_cache.update(ss_ms, covs) varying_means_proto = partial( slr_smoothing_cost_means, measurements=measurements, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), estimated_covs=covs, motion_fn=motion_model.map_set, meas_fn=meas_model.map_set, slr_method=SigmaPointSlr(SphericalCubature()), ) varying_means = partial( varying_means_proto, motion_cov_inv=slr_cache.proc_cov_inv, meas_cov_inv=slr_cache.meas_cov_inv, ) proc_cov_inv, meas_cov_inv = slr_cache.inv_cov() pre_comp = partial( pre_comp_proto, motion_bar=slr_cache.proc_bar, meas_bar=slr_cache.meas_bar, motion_cov_inv=proc_cov_inv, meas_cov_inv=meas_cov_inv, ) self.assertAlmostEqual(pre_comp(ss_ms), on_the_fly(ss_ms)) self.assertAlmostEqual(pre_comp(ss_ms), varying_means(ss_ms)) _, measurements, _, ss_ms = get_specific_states_from_file( Path.cwd() / "data/lm_ieks_paper", Type.GN, 1) slr_cache.update(ss_ms, covs) pre_comp = partial( pre_comp_proto, motion_bar=slr_cache.proc_bar, meas_bar=slr_cache.meas_bar, motion_cov_inv=slr_cache.proc_cov_inv, meas_cov_inv=slr_cache.meas_cov_inv, ) varying_means = partial( varying_means_proto, motion_cov_inv=slr_cache.proc_cov_inv, meas_cov_inv=slr_cache.meas_cov_inv, ) self.assertAlmostEqual(pre_comp(ss_ms), on_the_fly(ss_ms)) self.assertAlmostEqual(pre_comp(ss_ms), varying_means(ss_ms))