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 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)