class Iekf(Filter):
"""Iterated Extended Kalman filter (IEKF)"""
def __init__(self, motion_model, meas_model):
self._motion_model = motion_model
self._meas_model = meas_model
self._current_means = None
self._cache = ExtCache(self._motion_model, self._meas_model)
def _update_estimates(self, means, _covs, cache=None):
self._current_means = means.copy()
if cache is None:
self._cache.update(means, None)
else:
self._cache = cache
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.motion_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)
class Ieks(IteratedSmoother):
"""Iterated Extended Kalman Smoother (IEKS)"""
def __init__(self, motion_model, meas_model, num_iter):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self.num_iter = num_iter
self._cache = ExtCache(self._motion_model, self._meas_model)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.motion_lin[time_step - 1]
def _first_iter(self, measurements, m_1_0, P_1_0, cost_fn):
smoother = Eks(self._motion_model, self._meas_model)
return smoother.filter_and_smooth(measurements, m_1_0, P_1_0, cost_fn)
def _filter_seq(self, measurements, m_1_0, P_1_0):
iekf = Iekf(self._motion_model, self._meas_model)
iekf._update_estimates(self._current_means, self._current_covs,
self._cache)
return iekf.filter_seq(measurements, m_1_0, P_1_0)
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(means, None)
def _is_initialised(self):
return self._cache.is_initialized() and self._current_means is not None
class LmIeks(IteratedSmoother):
"""Levenberg-Marquardt Iterated Extended Kalman Smoother (LM-IEKS)"""
def __init__(self, motion_model, meas_model, num_iter,
cost_improv_iter_lim, lambda_, nu):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self.num_iter = num_iter
self._cost_improv_iter_lim = cost_improv_iter_lim
self._lambda = lambda_
self._nu = nu
self._cache = ExtCache(self._motion_model, self._meas_model)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.motion_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):
smoother = Eks(self._motion_model, self._meas_model)
return smoother.filter_and_smooth(measurements, m_1_0, P_1_0, cost_fn)
def filter_and_smooth_with_init_traj(self, measurements, m_1_0, P_1_0,
init_traj, start_iter, cost_fn):
"""Filter and smoothing given an initial trajectory"""
current_ms, current_Ps = init_traj
self._update_estimates(current_ms, current_Ps)
prev_cost = cost_fn(current_ms)
cost_iter = []
for iter_ in range(start_iter, self.num_iter + 1):
self._log.debug(f"Iter: {iter_}")
inner_iter = 0
has_improved = False
while has_improved is False and inner_iter < 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,
cost_fn)
self._log.debug(f"Cost: {cost}, lambda: {self._lambda}")
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
inner_iter += 1
if inner_iter == self._cost_improv_iter_lim - 1:
self._log.warning(
f"No cost improvement for {self._cost_improv_iter_lim} iterations"
)
self._update_estimates(current_ms, current_Ps)
prev_cost = cost
cost_iter.append(cost)
# _cost = cost(current_ms, measurements, m_1_0, P_1_0, self._motion_model, self._meas_model)
return mf, Pf, current_ms, current_Ps, np.array(cost_iter)
def _filter_seq(self, measurements, m_1_0, P_1_0):
lm_iekf = _LmIekf(self._motion_model, self._meas_model, self._lambda)
lm_iekf._update_estimates(self._current_means, self._current_covs,
self._cache)
return lm_iekf.filter_seq(measurements, m_1_0, P_1_0)
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(means, None)
def _is_initialised(self):
return self._cache.is_initialized() and self._current_means is not None
class LsIeks(IteratedSmoother):
"""Line-search Iterated Extended Kalman Smoother (LS-IEKS)"""
def __init__(self, motion_model, meas_model, num_iter, line_search_method):
super().__init__()
self._motion_model = motion_model
self._meas_model = meas_model
self.num_iter = num_iter
self._ls_method = line_search_method
self._cache = ExtCache(self._motion_model, self._meas_model)
def _motion_lin(self, _mean, _cov, time_step):
return self._cache.motion_lin[time_step - 1]
# TODO: This should also have inner LS check
def _first_iter(self, measurements, m_1_0, P_1_0, cost_fn):
smoother = Eks(self._motion_model, self._meas_model)
return smoother.filter_and_smooth(measurements, m_1_0, P_1_0, cost_fn)
def filter_and_smooth_with_init_traj(self, measurements, m_1_0, P_1_0,
init_traj, start_iter, cost_fn):
"""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
self._update_estimates(current_ms, current_Ps)
prev_cost = cost_fn(current_ms)
cost_iter = [prev_cost]
self._log.debug(f"Initial cost: {prev_cost}")
for iter_ in range(start_iter, self.num_iter + 1):
self._log.debug(f"Iter: {iter_}")
# Note: here we want to run the base `Smoother` class method.
# I.e. we're getting the grandparent's method.
current_mf, current_Pf, current_ms, current_Ps, cost = super(
IteratedSmoother,
self).filter_and_smooth(measurements, m_1_0, P_1_0, cost_fn)
ls_ms, alpha, ls_cost = self._ls_method.search_next(
self._current_means, current_ms)
if ls_cost > cost:
self._log.warning(
f"Line search did not decrease, defaulting to plain IEKS.")
self._update_estimates(current_ms, current_Ps)
prev_cost = cost
mf = current_mf
Pf = current_Pf
else:
self._update_estimates(ls_ms, current_Ps)
prev_cost = ls_cost
mf = mf + alpha * (current_mf - self._current_means)
cost_iter.append(prev_cost)
# _cost = cost(current_ms, measurements, m_1_0, P_1_0, self._motion_model, self._meas_model)
return mf, Pf, current_ms, current_Ps, np.array(cost_iter)
def _filter_seq(self, measurements, m_1_0, P_1_0):
iekf = Iekf(self._motion_model, self._meas_model)
iekf._update_estimates(self._current_means, self._current_covs,
self._cache)
return iekf.filter_seq(measurements, m_1_0, P_1_0)
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(means, None)
def _is_initialised(self):
return self._cache.is_initialized() and self._current_means is not None