class SigmaPointIplf(Filter):
"""Iterated posterior linearisation filter (IPLF)"""
def __init__(self, motion_model, meas_model, sigma_point_method):
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._current_means = None
self._current_covs = None
self._cache = SlrCache(self._motion_model.map_set,
self._meas_model.map_set, self._slr)
def _update_estimates(self, means, covs, cache=None):
self._current_means = means.copy()
self._current_covs = covs.copy()
if cache is None:
self._update_slr_cache()
else:
self._cache = cache
def _update_slr_cache(self):
self._cache.update(self._current_means, self._current_covs)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.proc_lin[time_step - 1]
def _meas_lin(self, _mean, _cov, time_step):
return self._cache.meas_lin[time_step - 1]
def _proc_noise(self, time_step):
return self._motion_model.proc_noise(time_step)
def _meas_noise(self, time_step):
return self._meas_model.meas_noise(time_step)
def __init__(self, motion_model, meas_model, sigma_point_method, num_iter):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._sigma_point_method = sigma_point_method
self.num_iter = num_iter
self._cache = SlrCache(self._motion_model, self._meas_model, self._slr)
def __init__(self, motion_model, meas_model, sigma_point_method):
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._current_means = None
self._current_covs = None
self._cache = SlrCache(self._motion_model.map_set,
self._meas_model.map_set, self._slr)
def __init__(self, motion_model, meas_model, sigma_point_method, num_iter,
cost_improv_iter_lim, lambda_, nu):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._sigma_point_method = sigma_point_method
self.num_iter = num_iter
self._cost_improv_iter_lim = cost_improv_iter_lim
self._lambda = lambda_
self._nu = nu
self._cache = SlrCache(self._motion_model, self._meas_model, self._slr)
def check_cost(estimates, cost_fn_prototype, motion_model, meas_model,
slr_method):
costs = []
for ms, Ps, label in estimates:
cache = SlrCache(motion_model.map_set, meas_model.map_set, slr_method)
cache.update(ms, Ps)
cost_fn = partial(
cost_fn_prototype,
motion_bar=cache.proc_bar,
meas_bar=cache.meas_bar,
motion_cov=[
lin[2] + motion_model.proc_noise(k)
for k, lin in enumerate(cache.proc_lin, 1)
],
meas_cov=[
lin[2] + meas_model.meas_noise(k)
for k, lin in enumerate(cache.meas_lin, 1)
],
)
costs.append[(cost_fn(ms), label)]
class SigmaPointLsIpls(IteratedSmoother):
"""Line-search Iterated Posterior Linearisation Smoother (LS-IPLS)"""
def __init__(self, motion_model, meas_model, sigma_point_method, num_iter,
line_search_method, num_points):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._sigma_point_method = sigma_point_method
self.num_iter = num_iter
self._ls_method = line_search_method
self._cache = SlrCache(self._motion_model, self._meas_model, self._slr)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.proc_lin[time_step - 1]
def _first_iter(self, measurements, m_1_0, P_1_0, cost_fn_prototype):
smoother = SigmaPointPrLs(self._motion_model, self._meas_model,
self._sigma_point_method)
# Hack to use the generic `filter_and_smooth` method
# The cost function prototype (variable covs) cannot be specialised until the `smooth_covs` are known
# but the cost is calculated within the function (where it must be specialised).
# Solution: - Use 'None' in the `filter_and_smooth` method => conforms to the base API.
# - Calculate the actual cost here, when the smooth_covs are available.
filter_means, filter_covs, smooth_means, smooth_covs, _ = smoother.filter_and_smooth(
measurements, m_1_0, P_1_0, None)
self._update_estimates(smooth_means, smooth_covs)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (smooth_covs, self._cache.inv_cov()))
cost = cost_fn(smooth_means)
return filter_means, filter_covs, smooth_means, smooth_covs, cost
def filter_and_smooth_with_init_traj(self, measurements, m_1_0, P_1_0,
init_traj, start_iter,
cost_fn_prototype):
"""Filter and smoothing given an initial trajectory"""
current_ms, current_Ps = init_traj
# If self.num_iter is too low to enter the iter loop
mf, Pf = init_traj
cost_iter = []
if not self._is_initialised():
self._update_estimates(current_ms, current_Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._current_covs, self._cache.inv_cov()))
# Temporary fix which assumes that we use inexact linesearch, i.e. it breaks GridSearch.
dir_der_prototype = partial(
dir_der_slr_smoothing_cost,
measurements=measurements,
m_1_0=m_1_0,
P_1_0=P_1_0,
motion_fn=self._motion_model.map_set,
meas_fn=self._meas_model.map_set,
slr_method=self._slr,
)
dir_der = self._specialise_dir_der(
dir_der_prototype, (self._current_covs, self._cache.inv_cov()))
prev_cost = cost_fn(current_ms)
for iter_ in range(start_iter, self.num_iter + 1):
self._log.debug(f"Iter: {iter_}")
cost_iter.append(prev_cost)
num_iter_with_same_cost = 1
while self._terminate_inner_loop(num_iter_with_same_cost):
num_iter_with_same_cost += 1
# Note: here we want to run the base `Smoother` class method.
# I.e. we're getting the grandparent's method.
mf, Pf, current_ms, current_Ps, cost = super(
IteratedSmoother,
self).filter_and_smooth(measurements, m_1_0, P_1_0,
cost_fn)
line_search = self._ls_method(cost_fn, dir_der)
ls_ms, alpha, ls_cost = line_search.search_next(
self._current_means, current_ms)
if ls_cost > cost:
self._log.warning(
f"Line search did not decrease, defaulting to plain IPLS."
)
self._update_means_only(current_ms, None)
# Stored for later update.
Ps = self._current_covs
prev_cost = cost
else:
self._update_means_only(ls_ms, None)
# Stored for later update.
Ps = line_search.next_estimate(self._current_covs,
current_Ps, alpha)
prev_cost = ls_cost
# Update cost function with new covariances.
self._update_estimates(self._current_means, Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype,
(self._current_covs, self._cache.inv_cov()))
dir_der = self._specialise_dir_der(
dir_der_prototype,
(self._current_covs, self._cache.inv_cov()))
self._log.debug(f"Cost: {cost}, alpha: {alpha}")
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._current_covs, self._cache.inv_cov()))
return mf, Pf, current_ms, current_Ps, np.array(cost_iter)
def _filter_seq(self, measurements, m_1_0, P_1_0):
iplf = SigmaPointIplf(self._motion_model, self._meas_model,
self._sigma_point_method)
iplf._update_estimates(self._current_means, self._current_covs,
self._cache)
return iplf.filter_seq(measurements, m_1_0, P_1_0)
# traj, measurements, m_1_0, P_1_0, estimated_covs, motion_fn, meas_fn, motion_cov, meas_cov, slr_method
def _specialise_cost_fn(self, cost_fn_prototype, params):
(
estimated_covs,
(
motion_cov_inv,
meas_cov_inv,
),
) = params
return partial(
cost_fn_prototype,
estimated_covs=estimated_covs,
motion_cov_inv=motion_cov_inv,
meas_cov_inv=meas_cov_inv,
)
def _specialise_dir_der(self, dir_der_prototype, params):
(
estimated_covs,
(
motion_cov_inv,
meas_cov_inv,
),
) = params
return partial(
dir_der_prototype,
estimated_covs=estimated_covs,
motion_cov_inv=motion_cov_inv,
meas_cov_inv=meas_cov_inv,
)
def _is_initialised(self):
return self._cache.is_initialized(
) and self._current_means is not None and self._current_covs is not None
def _cost_fn_params(self):
return self._current_covs
def _terminate_inner_loop(self, inner_loop_iter):
return inner_loop_iter < 2
def _update_means_only(self, means, pre_comp_cache=None):
self._current_means = means.copy()
if pre_comp_cache is None:
self._cache.update(self._current_means, self._current_covs)
else:
self._cache = pre_comp_cache
def _update_estimates(self, means, covs):
"""The 'previous estimates' which are used in the current iteration are stored in the smoother instance.
They should only be modified through this method.
"""
super()._update_estimates(means, covs)
self._cache.update(self._current_means, self._current_covs)
class SigmaPointIpls(IteratedSmoother):
"""Iterated posterior linearisation filter"""
def __init__(self, motion_model, meas_model, sigma_point_method, num_iter):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._sigma_point_method = sigma_point_method
self.num_iter = num_iter
self._cache = SlrCache(self._motion_model, self._meas_model, self._slr)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.proc_lin[time_step - 1]
def _first_iter(self, measurements, m_1_0, P_1_0, cost_fn_prototype):
smoother = SigmaPointPrLs(self._motion_model, self._meas_model,
self._sigma_point_method)
mf, Pf, ms, Ps, _ = smoother.filter_and_smooth(measurements, m_1_0,
P_1_0, None)
self._update_estimates(ms, Ps)
if cost_fn_prototype is not None:
cost_fn = self._specialise_cost_fn(cost_fn_prototype,
self._cost_fn_params())
cost = cost_fn(ms)
else:
cost = None
return mf, Pf, ms, Ps, cost
def _filter_seq(self, measurements, m_1_0, P_1_0):
iplf = SigmaPointIplf(self._motion_model, self._meas_model,
self._sigma_point_method)
iplf._update_estimates(self._current_means, self._current_covs,
self._cache)
return iplf.filter_seq(measurements, m_1_0, P_1_0)
def _specialise_cost_fn(self, cost_fn_prototype, params):
if cost_fn_prototype is not None:
(
(motion_bar, meas_bar),
(
motion_cov_inv,
meas_cov_inv,
),
) = params
return partial(
cost_fn_prototype,
motion_bar=motion_bar,
meas_bar=meas_bar,
motion_cov_inv=motion_cov_inv,
meas_cov_inv=meas_cov_inv,
)
else:
return None
def _cost_fn_params(self):
return self._cache.bars(), self._cache.error_covs()
def _update_estimates(self, means, covs):
"""The 'previous estimates' which are used in the current iteration are stored in the smoother instance.
They should only be modified through this method.
"""
super()._update_estimates(means, covs)
self._cache.update(self._current_means, self._current_covs)
def _is_initialised(self):
return self._cache.is_initialized(
) and self._current_means is not None and self._current_covs is not None
def filter_and_smooth_with_init_traj(self, measurements, m_1_0, P_1_0,
init_traj, start_iter,
cost_fn_prototype):
"""Filter and smoothing given an initial trajectory"""
current_ms, current_Ps = init_traj
# If self.num_iter is too low to enter the iter loop
mf, Pf = init_traj
cost_iter = []
# Optimisation to only update the estimates when the estimates have changed.
# This method can be called either as part of the filter_and_smooth method,
# then the estimates are already updated in the _first_iter method,
# Or it can be called directly with an init_traj, then the update is needed
if not self._is_initialised():
self._update_estimates(current_ms, current_Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._cache.bars(), self._cache.inv_cov()))
prev_cost = cost_fn(current_ms)
for iter_ in range(start_iter, self.num_iter + 1):
self._log.debug(f"Iter: {iter_}")
loss_cand_no = 1
has_improved = False
# TODO: Impl inner loop for multiple opt of same cost fn.
while has_improved is False and loss_cand_no <= self._cost_improv_iter_lim:
# Note: here we want to run the base `Smoother` class method.
# I.e. we're getting the grandparent's method.
mf, Pf, current_ms, current_Ps, cost = super(
IteratedSmoother,
self).filter_and_smooth(measurements, m_1_0, P_1_0, None)
# Create a temporary cache. Linearisation is still done with self._cache until the present iterate
# is accepted. The cost is calculated with a hybrid: proc_bar, meas_bar from the tmp_cache but
# linearisation err. covariance from self._cache.
tmp_cache = SlrCache(self._motion_model, self._meas_model,
self._slr)
# TODO: Since we only care about the bars here, we could potentially optimise and only calc. the
# bars and skip the linearisation.
# This would only be faster if the iterate is rejected since we still do the remaining calc.
# (the linearisation) when the iterate is accepted.
tmp_cache.update(
current_ms,
self._current_covs,
)
# Note: here we take the new bars but the old error_covs.
tmp_cost_fn = self._specialise_cost_fn(
cost_fn_prototype,
(tmp_cache.bars(), self._cache.inv_cov()))
cost = tmp_cost_fn(current_ms)
self._log.debug(
f"Cost: {cost}, lambda: {self._lambda}, loss_cand_no: {loss_cand_no}"
)
if not self._lambda > 0.0:
self._log.info("lambda=0, skipping cost check")
has_improved = True
elif cost < prev_cost:
self._lambda /= self._nu
has_improved = True
else:
self._lambda *= self._nu
loss_cand_no += 1
if loss_cand_no == self._cost_improv_iter_lim + 1:
self._log.info(
f"No cost improvement for {self._cost_improv_iter_lim} iterations, returning"
)
# Update to get the most recent estimates into the stored_estimates
self._update_estimates(self._current_means, self._current_covs)
return mf, Pf, self._current_means, self._current_covs, np.array(
cost_iter)
# Full update, updating means and covs estimates.
# Which also requires an update of the cost fn.
self._update_estimates(current_ms, current_Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._cache.bars(), self._cache.inv_cov()))
prev_cost = cost_fn(current_ms)
cost_iter.append(prev_cost)
return mf, Pf, current_ms, current_Ps, np.array(cost_iter)
class SigmaPointLmIpls(IteratedSmoother):
"""Levenberg-Marquardt regularised Iterated Posterior Linearisation Smoother (LM-IPLS)"""
def __init__(self, motion_model, meas_model, sigma_point_method, num_iter,
cost_improv_iter_lim, lambda_, nu):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self._slr = SigmaPointSlr(sigma_point_method)
self._sigma_point_method = sigma_point_method
self.num_iter = num_iter
self._cost_improv_iter_lim = cost_improv_iter_lim
self._lambda = lambda_
self._nu = nu
self._cache = SlrCache(self._motion_model, self._meas_model, self._slr)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.proc_lin[time_step - 1]
# TODO: This should also have inner LM check
def _first_iter(self, measurements, m_1_0, P_1_0, cost_fn_prototype):
smoother = SigmaPointPrLs(self._motion_model, self._meas_model,
self._sigma_point_method)
# Hack to use the generic `filter_and_smooth` method
# The cost function prototype (variable covs) cannot be specialised until the `smooth_covs` are known
# but the cost is calculated within the function (where it must be specialised).
# Solution: - Use 'None' in the `filter_and_smooth` method => conforms to the base API.
# - Calculate the actual cost here, when the smooth_covs are available.
filter_means, filter_covs, smooth_means, smooth_covs, _ = smoother.filter_and_smooth(
measurements, m_1_0, P_1_0, None)
self._update_estimates(smooth_means, smooth_covs)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._cache.bars(), self._cache.inv_cov()))
cost = cost_fn(smooth_means)
return filter_means, filter_covs, smooth_means, smooth_covs, cost
def filter_and_smooth_with_init_traj(self, measurements, m_1_0, P_1_0,
init_traj, start_iter,
cost_fn_prototype):
"""Filter and smoothing given an initial trajectory"""
current_ms, current_Ps = init_traj
# If self.num_iter is too low to enter the iter loop
mf, Pf = init_traj
cost_iter = []
# Optimisation to only update the estimates when the estimates have changed.
# This method can be called either as part of the filter_and_smooth method,
# then the estimates are already updated in the _first_iter method,
# Or it can be called directly with an init_traj, then the update is needed
if not self._is_initialised():
self._update_estimates(current_ms, current_Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._cache.bars(), self._cache.inv_cov()))
prev_cost = cost_fn(current_ms)
for iter_ in range(start_iter, self.num_iter + 1):
self._log.debug(f"Iter: {iter_}")
loss_cand_no = 1
has_improved = False
# TODO: Impl inner loop for multiple opt of same cost fn.
while has_improved is False and loss_cand_no <= self._cost_improv_iter_lim:
# Note: here we want to run the base `Smoother` class method.
# I.e. we're getting the grandparent's method.
mf, Pf, current_ms, current_Ps, cost = super(
IteratedSmoother,
self).filter_and_smooth(measurements, m_1_0, P_1_0, None)
# Create a temporary cache. Linearisation is still done with self._cache until the present iterate
# is accepted. The cost is calculated with a hybrid: proc_bar, meas_bar from the tmp_cache but
# linearisation err. covariance from self._cache.
tmp_cache = SlrCache(self._motion_model, self._meas_model,
self._slr)
# TODO: Since we only care about the bars here, we could potentially optimise and only calc. the
# bars and skip the linearisation.
# This would only be faster if the iterate is rejected since we still do the remaining calc.
# (the linearisation) when the iterate is accepted.
tmp_cache.update(
current_ms,
self._current_covs,
)
# Note: here we take the new bars but the old error_covs.
tmp_cost_fn = self._specialise_cost_fn(
cost_fn_prototype,
(tmp_cache.bars(), self._cache.inv_cov()))
cost = tmp_cost_fn(current_ms)
self._log.debug(
f"Cost: {cost}, lambda: {self._lambda}, loss_cand_no: {loss_cand_no}"
)
if not self._lambda > 0.0:
self._log.info("lambda=0, skipping cost check")
has_improved = True
elif cost < prev_cost:
self._lambda /= self._nu
has_improved = True
else:
self._lambda *= self._nu
loss_cand_no += 1
if loss_cand_no == self._cost_improv_iter_lim + 1:
self._log.info(
f"No cost improvement for {self._cost_improv_iter_lim} iterations, returning"
)
# Update to get the most recent estimates into the stored_estimates
self._update_estimates(self._current_means, self._current_covs)
return mf, Pf, self._current_means, self._current_covs, np.array(
cost_iter)
# Full update, updating means and covs estimates.
# Which also requires an update of the cost fn.
self._update_estimates(current_ms, current_Ps)
cost_fn = self._specialise_cost_fn(
cost_fn_prototype, (self._cache.bars(), self._cache.inv_cov()))
prev_cost = cost_fn(current_ms)
cost_iter.append(prev_cost)
return mf, Pf, current_ms, current_Ps, np.array(cost_iter)
def _filter_seq(self, measurements, m_1_0, P_1_0):
lm_iplf = _LmIplf(self._motion_model, self._meas_model,
self._sigma_point_method, self._lambda)
lm_iplf._update_estimates(self._current_means, self._current_covs,
self._cache)
return lm_iplf.filter_seq(measurements, m_1_0, P_1_0)
def _specialise_cost_fn(self, cost_fn_prototype, params):
(
(motion_bar, meas_bar),
(
motion_cov_inv,
meas_cov_inv,
),
) = params
return partial(
cost_fn_prototype,
motion_bar=motion_bar,
meas_bar=meas_bar,
motion_cov_inv=motion_cov_inv,
meas_cov_inv=meas_cov_inv,
)
def _is_initialised(self):
return self._cache.is_initialized(
) and self._current_means is not None and self._current_covs is not None
def _cost_fn_params(self):
return self._current_covs
def _terminate_inner_loop(self, loss_cand_no):
return loss_cand_no > 1
def _update_means_only(self, means, pre_comp_cache=None):
self._current_means = means.copy()
if pre_comp_cache is None:
self._cache.update(self._current_means, self._current_covs)
else:
self._cache = pre_comp_cache
def _update_estimates(self, means, covs):
"""The 'previous estimates' which are used in the current iteration are stored in the smoother instance.
They should only be modified through this method.
"""
super()._update_estimates(means, covs)
self._cache.update(self._current_means, self._current_covs)
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 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))