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 = 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])
_, 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 = 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_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))
[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]
D_x = prior_mean.shape[0]
init_traj = (np.zeros((K, prior_mean.shape[0])), None)
ieks = Ieks(motion_model, meas_model, num_iter)
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 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():
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 = "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():
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 = "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_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))
