diff --git a/examples/ex_gsf_se2.py b/examples/ex_gsf_se2.py new file mode 100644 index 00000000..e6fda7f6 --- /dev/null +++ b/examples/ex_gsf_se2.py @@ -0,0 +1,118 @@ +from navlie.lib import SE2State, BodyFrameVelocity, RangePoseToAnchor + +import navlie as nav +import numpy as np +from typing import List + + +""" +This example runs a Gaussian Sum filter to estimate the state +that is on a Lie group. The performance is compared to an EKF that is +initialized at a wrong state. +""" + +def main(): + + # Create the process model noise profile + Q = np.diag([0.1**2, 0.1, 0.1]) + process_model = BodyFrameVelocity(Q) + + + # Measurement model + R = 0.1**2 + range_models = [ + RangePoseToAnchor([-5, 5],[0, 0], R), + RangePoseToAnchor([ 5, 5],[0, 0], R), + ] + + # Setup + x0 = SE2State([0, -5, 0], stamp=0.0) # theta, x, y + P0 = np.diag([0.1**2, 0.1, 0.1]) + + def input_profile(t, u): + return np.array( [0.0, 0.1, np.cos(t)]) + + input_freq = 10 + dt = 1 / input_freq + t_max = dt * 100 + measurement_freq = 5 + + # gsf filter + gsf = nav.GaussianSumFilter(process_model) + + dg = nav.DataGenerator( + process_model, + input_profile, + Q, + input_freq, + range_models, + measurement_freq, + ) + + + def gsf_trial(trial_number: int) -> List[nav.GaussianResult]: + """ + A single Gaussian Sum Filter trial + """ + np.random.seed(trial_number) + state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True) + + # Initial state estimates + x = [SE2State([0, -5, 0], stamp=0.0), + SE2State([0, 5, 0], stamp=0.0)] + x = [x_.plus(nav.randvec(P0)) for x_ in x] + + x0_check = nav.lib.MixtureState( + [nav.StateWithCovariance(_x, P0) for _x in x], [1/len(x) for _ in x] + ) + + estimate_list = nav.run_gsf_filter( + gsf, x0_check, input_list, meas_list + ) + + results = [ + nav.MixtureResult(estimate_list[i], state_true[i]) + for i in range(len(estimate_list)) + ] + + return nav.MixtureResultList(results) + + def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: + """ + A single trial in a monte carlo experiment. This function accepts the trial + number and must return a list of GaussianResult objects. + """ + + # By using the trial number as the seed for the random generator, we can + # make sure our experiments are perfectly repeatable, yet still have + # independent noise samples from trial-to-trial. + np.random.seed(trial_number) + + state_true, input_list, meas_list = dg.generate(x0, 0, t_max, noise=True) + x0_check = SE2State([0, 5, 0], stamp=0.0) + x0_check = x0_check.plus(nav.randvec(P0)) + ekf = nav.ExtendedKalmanFilter(BodyFrameVelocity(Q)) + + estimate_list = nav.run_filter(ekf, x0_check, P0, input_list, meas_list) + return nav.GaussianResultList.from_estimates(estimate_list, state_true) + + N = 1 # Trial number + return gsf_trial(N), ekf_trial(N) + +if __name__ == "__main__": + results_gsf, results_ekf = main() + + + import matplotlib.pyplot as plt + import seaborn as sns + + sns.set_theme(style="whitegrid") + fig, ax = nav.plot_error(results_gsf, label = 'gsf') + nav.plot_error(results_ekf, axs=ax, label = 'ekf') + ax[0].set_title("Error plots") + ax[0].set_ylabel("Error (rad)") + ax[1].set_ylabel("Error (m)") + ax[2].set_ylabel("Error (m)") + ax[2].set_xlabel("Time (s)") + plt.legend() + plt.show() \ No newline at end of file diff --git a/examples/ex_imm_se3.py b/examples/ex_imm_se3.py index dc3ac81a..5998736c 100644 --- a/examples/ex_imm_se3.py +++ b/examples/ex_imm_se3.py @@ -107,11 +107,11 @@ def imm_trial(trial_number: int) -> List[nav.GaussianResult]: ) results = [ - nav.IMMResult(estimate_list[i], state_true[i]) + nav.MixtureResult(estimate_list[i], state_true[i]) for i in range(len(estimate_list)) ] - return nav.IMMResultList(results) + return nav.MixtureResultList(results) def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: diff --git a/examples/ex_imm_vector.py b/examples/ex_imm_vector.py index 51054c7f..6150a7cd 100644 --- a/examples/ex_imm_vector.py +++ b/examples/ex_imm_vector.py @@ -7,7 +7,7 @@ import navlie as nav from navlie.lib.models import DoubleIntegrator, RangePointToAnchor, VectorState from navlie.filters import InteractingModelFilter, run_imm_filter -from navlie.utils import IMMResultList +from navlie.utils import MixtureResultList import numpy as np from typing import List from matplotlib import pyplot as plt @@ -80,7 +80,7 @@ def main(): estimate_list = run_imm_filter(imm, x0_check, P0, input_list, meas_list) - results = IMMResultList.from_estimates(estimate_list, state_true) + results = MixtureResultList.from_estimates(estimate_list, state_true) return results diff --git a/navlie/__init__.py b/navlie/__init__.py index 8faeec6b..2d805305 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -15,8 +15,10 @@ CubatureKalmanFilter, GaussHermiteKalmanFilter, InteractingModelFilter, + GaussianSumFilter, run_filter, run_imm_filter, + run_gsf_filter, ) from . import batch from . import lib @@ -39,8 +41,8 @@ GaussianResult, GaussianResultList, MonteCarloResult, - IMMResult, - IMMResultList, + MixtureResult, + MixtureResultList, monte_carlo, randvec, van_loans, diff --git a/navlie/filters.py b/navlie/filters.py index 37d0244c..879f17e4 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -10,7 +10,7 @@ Measurement, StateWithCovariance, ) -from navlie.lib.states import IMMState +from navlie.lib.states import MixtureState from navlie.utils.mixture import gaussian_mixing import numpy as np from scipy.stats.distributions import chi2 @@ -859,17 +859,17 @@ def __init__( def interaction( self, - x: IMMState, + x: MixtureState, ): """The interaction (mixing) step of the IMM. Parameters ---------- - x : IMMState + x : MixtureState Returns ------- - IMMState + MixtureState """ x_km_models = x.model_states.copy() @@ -890,15 +890,15 @@ def interaction( weights = list(mu_mix[:, j]) x_mix.append(gaussian_mixing(weights, x_km_models)) - return IMMState(x_mix, mu_models) + return MixtureState(x_mix, mu_models) - def predict(self, x_km: IMMState, u: Input, dt: float): + def predict(self, x_km: MixtureState, u: Input, dt: float): """ Carries out prediction step for each model of the IMM. Parameters ---------- - x_km : IMMState + x_km : MixtureState Model estimates from previous timestep, after mixing. u : Input Input @@ -907,18 +907,18 @@ def predict(self, x_km: IMMState, u: Input, dt: float): Returns ------- - IMMState + MixtureState """ x_km_models = x_km.model_states.copy() x_check = [] for lv1, kf in enumerate(self.kf_list): x_check.append(kf.predict(x_km_models[lv1], u, dt)) - return IMMState(x_check, x_km.model_probabilities) + return MixtureState(x_check, x_km.model_probabilities) def correct( self, - x_check: IMMState, + x_check: MixtureState, y: Measurement, u: Input, ): @@ -928,7 +928,7 @@ def correct( Parameters ---------- - x_check: IMMState mu_km_models : List[Float] + x_check: MixtureState mu_km_models : List[Float] Probabilities for each model from previous timestep. y : Measurement Measurement to be fused into the current state estimate. @@ -939,7 +939,7 @@ def correct( Returns ------- - IMMState + MixtureState Corrected state estimates and probabilities """ x_models_check = x_check.model_states.copy() @@ -977,7 +977,133 @@ def correct( mu_k = mu_k / np.sum(mu_k) - return IMMState(x_hat, mu_k) + return MixtureState(x_hat, mu_k) + + +class GaussianSumFilter: + """ + On-manifold Gaussian Sum Filter (GSF). + + References for the GSF: + + D. Alspach and H. Sorenson, "Nonlinear Bayesian estimation using + Gaussian sum approximations," in IEEE Transactions on Automatic + Control, vol. 17, no. 4, pp. 439-448, August 1972. + + The GSF involves Gaussian mixtures. Reference for mixing Gaussians on + manifolds: + + J. Ćesić, I. Marković and I. Petrović, "Mixture Reduction on Matrix Lie + Groups," in IEEE Signal Processing Letters, vol. 24, no. 11, pp. + 1719-1723, Nov. 2017, doi: 10.1109/LSP.2017.2723765. + """ + + __slots__ = [ + "process_model", + "reject_outliers", + "_ekf", + ] + + def __init__( + self, + process_model: ProcessModel, + reject_outliers=False, + ): + """ + Parameters + ---------- + process_models : List[ProcessModel] + process models to be used in the prediction step + reject_outliers : bool, optional + whether to apply the NIS test to measurements, by default False + """ + self._ekf = ExtendedKalmanFilter(process_model, reject_outliers) + + def predict( + self, + x: MixtureState, + u: Input, + dt: float = None, + ) -> MixtureState: + """ + Propagates the state forward in time using a process model. The user + must provide the current state, input, and time interval + + .. note:: + If the time interval ``dt`` is not provided in the arguments, it will + be taken as the difference between the input stamp and the state stamp. + + Parameters + ---------- + x : MixtureState + The current states and their associated weights. + u : Input + Input measurement to be given to process model + dt : float, optional + Duration to next time step. If not provided, dt will be calculated + with ``dt = u.stamp - x.state.stamp``. + Returns + ------- + MixtureState + New predicted states with associated weights. + """ + + n_modes = len(x.model_states) + + x_check = [] + for i in range(n_modes): + x_check.append(self._ekf.predict(x.model_states[i], u, dt)) + return MixtureState(x_check, x.model_probabilities) + + def correct( + self, + x: MixtureState, + y: Measurement, + u: Input, + ) -> MixtureState: + """ + Corrects the state estimate using a measurement. The user must provide + the current state and measurement. + + Parameters + ---------- + x : MixtureState + The current states and their associated weights. + y : Measurement + Measurement to correct the state estimate. + u : Input + Input measurement to be given to process model + + Returns + ------- + MixtureState + Corrected states with associated weights. + """ + x_check = x.copy() + n_modes = len(x.model_states) + weights_check = x.model_probabilities.copy() + + x_hat = [] + weights_hat = np.zeros(n_modes) + for i in range(n_modes): + x, details_dict = self._ekf.correct(x_check.model_states[i], y, u, + output_details=True) + x_hat.append(x) + z = details_dict["z"] + S = details_dict["S"] + model_likelihood = multivariate_normal.pdf( + z.ravel(), mean=np.zeros(z.shape), cov=S + ) + weights_hat[i] = weights_check[i] * model_likelihood + + # If all model likelihoods are zero to machine tolerance, np.sum(mu_k)=0 and it fails + # Add this fudge factor to get through those cases. + if np.allclose(weights_hat, np.zeros(weights_hat.shape)): + weights_hat = 1e-10 * np.ones(weights_hat.shape) + + weights_hat = weights_hat / np.sum(weights_hat) + + return MixtureState(x_hat, weights_hat) def run_filter( @@ -1048,6 +1174,9 @@ def run_filter( return results_list +# TODO. The IMM seems to have an issue when the user accidently modifies the +# provided state in the process model. + def run_imm_filter( filter: InteractingModelFilter, x0: State, @@ -1101,7 +1230,7 @@ def run_imm_filter( results_list = [] n_models = filter.transition_matrix.shape[0] - x = IMMState( + x = MixtureState( [StateWithCovariance(x0, P0)] * n_models, 1.0 / n_models * np.array(np.ones(n_models)), ) @@ -1122,3 +1251,68 @@ def run_imm_filter( x = filter.predict(x, u, dt) return results_list + + +def run_gsf_filter( + filter: GaussianSumFilter, + x0: StateWithCovariance, + input_data: List[Input], + meas_data: List[Measurement], + disable_progress_bar: bool = False, +) -> List[StateWithCovariance]: + """ + Executes a predict-correct-style filter given lists of input and measurement + data. + + Parameters + ---------- + filter : GaussianSumFilter + _description_ + x0 : StateWithCovariance + _description_ + input_data : List[Input] + _description_ + meas_data : List[Measurement] + _description_ + """ + x = x0.copy() + if x.stamp is None: + raise ValueError("x0 must have a valid timestamp.") + + # Sort the data by time + input_data.sort(key=lambda x: x.stamp) + meas_data.sort(key=lambda x: x.stamp) + + # Remove all that are before the current time + for idx, u in enumerate(input_data): + if u.stamp >= x.stamp: + input_data = input_data[idx:] + break + + for idx, y in enumerate(meas_data): + if y.stamp >= x.stamp: + meas_data = meas_data[idx:] + break + + meas_idx = 0 + if len(meas_data) > 0: + y = meas_data[meas_idx] + + results_list = [] + for k in tqdm(range(len(input_data) - 1), disable=disable_progress_bar): + u = input_data[k] + # Fuse any measurements that have occurred. + if len(meas_data) > 0: + while y.stamp < input_data[k + 1].stamp and meas_idx < len( + meas_data + ): + x = filter.correct(x, y, u) + meas_idx += 1 + if meas_idx < len(meas_data): + y = meas_data[meas_idx] + + results_list.append(x) + dt = input_data[k + 1].stamp - x.stamp + x = filter.predict(x, u, dt) + + return results_list \ No newline at end of file diff --git a/navlie/lib/__init__.py b/navlie/lib/__init__.py index 51d37087..15a1145f 100644 --- a/navlie/lib/__init__.py +++ b/navlie/lib/__init__.py @@ -10,7 +10,7 @@ SO3State, SE23State, SL3State, - IMMState, + MixtureState, CompositeState, MatrixLieGroupState, VectorInput, diff --git a/navlie/lib/states.py b/navlie/lib/states.py index 8bbb08c2..355a29e5 100644 --- a/navlie/lib/states.py +++ b/navlie/lib/states.py @@ -653,7 +653,7 @@ def __init__( super().__init__(value, self.group, stamp, state_id, direction) -class IMMState: +class MixtureState: __slots__ = ["model_states", "model_probabilities"] def __init__( @@ -668,9 +668,9 @@ def __init__( def stamp(self): return self.model_states[0].state.stamp - def copy(self) -> "IMMState": + def copy(self) -> "MixtureState": x_copy = [x.copy() for x in self.model_states] - return IMMState(x_copy, self.model_probabilities.copy()) + return MixtureState(x_copy, self.model_probabilities.copy()) class VectorInput(Input): diff --git a/navlie/utils/__init__.py b/navlie/utils/__init__.py index 81941e7f..91ac3632 100644 --- a/navlie/utils/__init__.py +++ b/navlie/utils/__init__.py @@ -3,8 +3,8 @@ GaussianResult, GaussianResultList, MonteCarloResult, - IMMResult, - IMMResultList, + MixtureResult, + MixtureResultList, monte_carlo, randvec, van_loans, diff --git a/navlie/utils/common.py b/navlie/utils/common.py index f7300787..1bbc3943 100644 --- a/navlie/utils/common.py +++ b/navlie/utils/common.py @@ -5,7 +5,7 @@ from typing import Callable, List, Tuple, Union, Any from joblib import Parallel, delayed from navlie.types import State, StateWithCovariance -from navlie.lib.states import IMMState +from navlie.lib.states import MixtureState from navlie.utils.mixture import gaussian_mixing import numpy as np from scipy.interpolate import interp1d @@ -175,7 +175,8 @@ def van_loans( Q_c: np.ndarray, dt: float, ) -> Tuple[np.ndarray, np.ndarray]: - """Van Loan's method for computing the discrete-time A and Q matrices. + """ + Van Loan's method for computing the discrete-time A and Q matrices. Given a continuous-time system of the form @@ -900,7 +901,7 @@ def nees_upper_bound(self, confidence_interval: float, double_sided=True): ) -class IMMResult(GaussianResult): +class MixtureResult(GaussianResult): __slots__ = [ "stamp", "state", @@ -914,7 +915,7 @@ class IMMResult(GaussianResult): "model_probabilities", ] - def __init__(self, imm_estimate: IMMState, state_true: State): + def __init__(self, imm_estimate: MixtureState, state_true: State): super().__init__( gaussian_mixing( imm_estimate.model_probabilities, imm_estimate.model_states @@ -925,7 +926,7 @@ def __init__(self, imm_estimate: IMMState, state_true: State): self.model_probabilities = imm_estimate.model_probabilities -class IMMResultList(GaussianResultList): +class MixtureResultList(GaussianResultList): __slots__ = [ "stamp", "state", @@ -942,7 +943,7 @@ class IMMResultList(GaussianResultList): "model_probabilities", ] - def __init__(self, result_list: List[IMMResult]): + def __init__(self, result_list: List[MixtureResult]): super().__init__(result_list) self.model_probabilities = np.array( [r.model_probabilities for r in result_list] @@ -950,18 +951,18 @@ def __init__(self, result_list: List[IMMResult]): @staticmethod def from_estimates( - estimate_list: List[IMMState], + estimate_list: List[MixtureState], state_true_list: List[State], method="nearest", ): """ - A convenience function that creates a IMMResultList from a list of - IMMState and a list of true State objects + A convenience function that creates a MixtureResultList from a list of + MixtureState and a list of true State objects Parameters ---------- - estimate_list : List[IMMState] - A list of IMMState objects + estimate_list : List[MixtureState] + A list of MixtureState objects state_true_list : List[State] A list of true State objects method : "nearest" or "linear", optional @@ -970,15 +971,15 @@ def from_estimates( Returns ------- - IMMResultList - A IMMResultList object + MixtureResultList + A MixtureResultList object """ stamps = [r.model_states[0].stamp for r in estimate_list] state_true_list = state_interp(stamps, state_true_list, method=method) - return IMMResultList( + return MixtureResultList( [ - IMMResult(estimate, state_true) + MixtureResult(estimate, state_true) for estimate, state_true in zip(estimate_list, state_true_list) ] ) diff --git a/navlie/utils/mixture.py b/navlie/utils/mixture.py index f1ebe27a..38c88528 100644 --- a/navlie/utils/mixture.py +++ b/navlie/utils/mixture.py @@ -6,18 +6,9 @@ from navlie.types import ( State, - Input, - Measurement, StateWithCovariance, ) import numpy as np -from navlie.lib import IMMState -from tqdm import tqdm - - -# TODO. The IMM seems to have an issue when the user accidently modifies the -# provided state in the process model. - def gaussian_mixing_vectorspace( diff --git a/tests/integration/test_filter_gsf.py b/tests/integration/test_filter_gsf.py new file mode 100644 index 00000000..d1c2b546 --- /dev/null +++ b/tests/integration/test_filter_gsf.py @@ -0,0 +1,141 @@ +import pytest +from navlie.lib.states import VectorState, SE2State, MixtureState +from navlie.datagen import DataGenerator +from navlie.utils import GaussianResult +from navlie.utils import randvec + +from navlie.utils import monte_carlo +from navlie.lib.models import SingleIntegrator, RangePointToAnchor +from navlie.lib.models import ( + BodyFrameVelocity, +) +from navlie.lib.models import RangePoseToAnchor + +import numpy as np +from typing import List +from navlie.filters import GaussianSumFilter, run_gsf_filter +from navlie.utils import MixtureResult, MixtureResultList +from navlie.types import StateWithCovariance + +def make_range_models_vector(R): + range_models = [ + RangePointToAnchor([-5, 5], R), + RangePointToAnchor([ 5, 5], R), + ] + return range_models + +def make_range_models_se2(R): + range_models = [ + RangePoseToAnchor([-5, 5],[0, 0], R), + RangePoseToAnchor([ 5, 5],[0, 0], R), + ] + return range_models + +def make_filter_trial(dg, x0_true, P0, t_max, gsf, model_states): + def gsf_trial(trial_number: int) -> List[GaussianResult]: + """ + A Gaussian Sum Filter trial + """ + np.random.seed(trial_number) + state_true, input_list, meas_list = dg.generate(x0_true, 0, t_max, True) + + x = [x_.plus(randvec(P0)) for x_ in model_states] + x0_check = MixtureState( + [StateWithCovariance(_x, P0) for _x in x], [1/len(x) for _ in x] + ) + + estimate_list = run_gsf_filter(gsf, x0_check, input_list, meas_list) + + results = [ + MixtureResult(estimate_list[i], state_true[i]) + for i in range(len(estimate_list)) + ] + + return MixtureResultList(results) + + return gsf_trial + +@pytest.mark.parametrize( + "x0, P0, input_profile, Q, process_model, measurement_model, \ + R, input_freq, measurement_freq, model_states", + [ + ( + VectorState(np.array([0, 0]), stamp=0.0), + 0.1 * np.identity(2), + lambda t, x: np.array([0.1, np.cos(t)]), + 0.1**2 * np.identity(2), + SingleIntegrator, + make_range_models_vector, + 0.1**2, + 10, + 5, + [ + VectorState(np.array([0, 0]), stamp=0.0), + VectorState(np.array([0, 10]), stamp=0.0), + ] + ), + ( + SE2State([0, -5, 0], stamp=0.0), + 0.1 * np.identity(3), + lambda t, x: np.array([0.0, 0.1, np.cos(t)]), + np.diag([0.01**2, 0.1 ,0.1]), + BodyFrameVelocity, + make_range_models_se2, + 0.1**2, + 10, + 5, + [ + SE2State([0, -5, 0], stamp=0.0), + SE2State([0, 5, 0], stamp=0.0), + ], + ), + ], +) + +def test_reasonable_nees_gsf( + x0, + P0, + input_profile, + Q, + process_model, + measurement_model, + R, + input_freq, + measurement_freq, + model_states +): + dt = 1 / input_freq + t_max = dt * 100 + N = 5 + + dg = DataGenerator( + process_model(Q), + input_profile, + Q, + input_freq, + measurement_model(R), + measurement_freq, + ) + + gsf_trial = make_filter_trial( + dg, + x0, + P0, + t_max, + GaussianSumFilter(process_model(Q)), + model_states, + ) + results = monte_carlo(gsf_trial, N) + + nees_in_correct_region = np.count_nonzero( + results.average_nees < 2 * results.nees_upper_bound(0.99) + ) + nt = results.average_nees.shape[0] + # Proportion of time NEES remains below 2*upper_bound bigger than 90% + assert nees_in_correct_region / nt > 0.90 + + # Make sure we essentially never get a completely absurd NEES. + nees_in_correct_region = np.count_nonzero( + results.average_nees < 50 * results.nees_upper_bound(0.99) + ) + assert nees_in_correct_region / nt > 0.9999 diff --git a/tests/integration/test_interacting_multiple_models.py b/tests/integration/test_interacting_multiple_models.py index 6a0eda6c..b8eba9a5 100644 --- a/tests/integration/test_interacting_multiple_models.py +++ b/tests/integration/test_interacting_multiple_models.py @@ -22,7 +22,7 @@ import numpy as np from typing import List from navlie.filters import InteractingModelFilter, run_imm_filter -from navlie.utils import IMMResult, IMMResultList +from navlie.utils import MixtureResult, MixtureResultList # TODO this test is very complicated. we need to simplify this. @@ -77,11 +77,11 @@ def imm_trial(trial_number: int) -> List[GaussianResult]: estimate_list = run_imm_filter(imm, x0_check, P0, input_list, meas_list) results = [ - IMMResult(estimate_list[i], state_true[i]) + MixtureResult(estimate_list[i], state_true[i]) for i in range(len(estimate_list)) ] - return IMMResultList(results) + return MixtureResultList(results) return imm_trial diff --git a/tests/test_examples.py b/tests/test_examples.py index 5391aee0..90fa84a5 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -22,6 +22,7 @@ examples/ex_varying_noise.py examples/ex_slam.py examples/ex_gaussian_mixture.py +examples/ex_gsf_se2.py """ @@ -121,3 +122,9 @@ def test_ex_slam(): from ex_slam import main main() + + +def test_ex_gsf_se2(): + from ex_gsf_se2 import main + + main()