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_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 gn_ieks(motion_model, meas_model, num_iter, states, measurements, prior_mean, prior_cov, cost_fn): smoother = Ieks(motion_model, meas_model, num_iter) _, _, ms, Ps, iter_cost = smoother.filter_and_smooth( measurements, prior_mean, prior_cov, cost_fn) rmses = calc_iter_metrics( lambda means, covs, states: rmse(means[:, :2], states), smoother.stored_estimates(), states) neeses = calc_iter_metrics( lambda means, covs, states: np.mean( nees(means[:, :2], states, covs[:, :2, :2])), smoother.stored_estimates(), states, ) return ms, Ps, iter_cost, rmses, neeses
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 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")
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] D_x = prior_mean.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) num_grid_points = 6 ls = GridSearch(cost_fn, num_grid_points) x_next, alpha = ls.search_next(x_0, x_1) alphas = np.linspace(0, 1, num_grid_points) cands = np.array([alpha * (x_1 - x_0) + x_0 for alpha in alphas]) alph_costs = [cost_fn(cand) for cand in cands] plt.plot([alpha], [cost_fn(x_next)], "rx", label="method") plt.plot(alphas, alph_costs, "b*", label="alph") plt.show()
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_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 = "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(): np.random.seed(1) args = parse_args() log = logging.getLogger(__name__) experiment_name = "ipls" setup_logger(f"logs/{experiment_name}.log", logging.DEBUG) log.info(f"Running experiment: {experiment_name}") K = 50 D_x = 1 motion_model = NonStationaryGrowth(alpha=0.9, beta=10, gamma=8, delta=1.2, proc_noise=1) meas_model = (Cubic(coeff=1 / 20, meas_noise=1) if args.meas_type == MeasType.Cubic else Quadratic(coeff=1 / 20, meas_noise=1)) # LM hyper params lambda_ = 1e-2 nu = 10 prior_mean = np.atleast_1d(5) prior_cov = np.atleast_2d([4]) # MC DATA # num_mc_runs = 1000 # 1000 in original exp # num_mc_per_traj = 50 # num_trajs = num_mc_runs // num_mc_per_traj # trajs, noise, _, _ = get_specific_states_from_file(Path.cwd() / "data/ipls_paper") # assert trajs.shape == (K, num_trajs) # assert noise.shape == (K, num_mc_runs) # meas = gen_measurements(traj, noise[:, 0], meas_model) states, meas = simulate_data(K, prior_mean, prior_cov, motion_model, meas_model) # The paper simply states that "[t]he SLRs have been implemented using the unscented transform # with N = 2 D_x + 1 sigma-points and the weight of the sigma-point located on the mean is 1/3." # The following settings ensures that: # a) w_0 (the weight of the mean sigma point) is 1/3 # b) there is no difference for the weights for the mean and cov estimation sigma_point_method = UnscentedTransform(1, 0, 1 / 2) assert sigma_point_method.weights(D_x)[0][0] == 1 / 3 assert np.allclose( sigma_point_method.weights(D_x)[0], sigma_point_method.weights(D_x)[1]) # traj_idx = _mc_iter_to_traj_idx(0, num_mc_per_traj) # traj = trajs[:, traj_idx].reshape((K, 1)) # traj = traj[:min_K, :] results = [] cost_fn_eks = partial( analytical_smoothing_cost_time_dep, measurements=meas, m_1_0=prior_mean, P_1_0=prior_cov, motion_model=motion_model, meas_model=meas_model, ) ieks = Ieks(motion_model, meas_model, args.num_iter) ms_ieks, Ps_ieks, cost_ieks, rmses_ieks, neeses_ieks = ieks.filter_and_smooth( meas, prior_mean, prior_cov, cost_fn_eks, ) results.append((ms_ieks, Ps_ieks, "IEKS"), ) lm_ieks = LmIeks(motion_model, meas_model, args.num_iter, 10, lambda_=lambda_, nu=nu) ms_lm_ieks, Ps_lm_ieks, cost_lm_ieks, rmses_lm_ieks, neeses_lm_ieks = lm_ieks.filter_and_smooth( meas, prior_mean, prior_cov, cost_fn_eks, ) results.append((ms_lm_ieks, Ps_lm_ieks, "LM-IEKS"), ) cost_fn_ipls = partial( slr_smoothing_cost_pre_comp, measurements=meas, m_1_0=prior_mean, P_1_0_inv=np.linalg.inv(prior_cov), ) ipls = SigmaPointIpls(motion_model, meas_model, sigma_point_method, args.num_iter) _, _, ipls_ms, ipls_Ps, _ = ipls.filter_and_smooth(meas, prior_mean, prior_cov, cost_fn=cost_fn_ipls) results.append((ipls_ms, ipls_Ps, "IPLS")) lm_ipls = SigmaPointLmIpls(motion_model, meas_model, sigma_point_method, args.num_iter, cost_improv_iter_lim=10, lambda_=lambda_, nu=nu) _, _, lm_ipls_ms, lm_ipls_Ps, _ = lm_ipls.filter_and_smooth( meas, prior_mean, prior_cov, cost_fn=cost_fn_ipls) results.append((lm_ipls_ms, lm_ipls_Ps, "LM-IPLS")) # tikz_results(states, meas, results) plot_results(states, meas, results)
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__) 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"), ], )