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 gn_ipls(motion_model, meas_model, sigma_point_method, num_iter, states,
measurements, prior_mean, prior_cov, cost_fn):
smoother = SigmaPointIpls(motion_model, meas_model, sigma_point_method,
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, smoother.num_iter)
neeses = calc_iter_metrics(
lambda means, covs, states: np.mean(
nees(means[:, :2], states, covs[:, :2, :2])),
smoother.stored_estimates(),
states,
smoother.num_iter,
)
return ms, Ps, iter_cost, rmses, neeses
def test_cmp_with_ipls_impl(self):
K = 50
trajs, noise, ms_cmp, Ps_cmp = get_specific_states_from_file(
Path.cwd() / "data/ipls_paper")
ms_cmp = ms_cmp.reshape((K, 1))
Ps_cmp = Ps_cmp.reshape((K, 1, 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)
meas_model = Quadratic(coeff=1 / 20, meas_noise=1)
# The paper simply states that "[t]he SLRs have been implemented using the unscented transform
# with N s = 2n 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)
traj = trajs[:, 0].reshape((K, 1))
meas = gen_measurements(traj, noise[:, 0], meas_model)
prior_mean = np.atleast_1d(5)
prior_cov = np.atleast_2d([4])
num_iter = 1
ipls = SigmaPointIpls(motion_model, meas_model, sigma_point_method,
num_iter)
_, _, ms_ipls, Ps_ipls, _ = ipls.filter_and_smooth(meas,
prior_mean,
prior_cov,
cost_fn=None)
self.assertTrue(np.allclose(ms_ipls, ms_cmp))
self.assertTrue(np.allclose(Ps_ipls, Ps_cmp))
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")
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():
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():
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"),
],
)