From 32ba439c764c69dc952040876d1654e6a1be5ada Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Fri, 26 Apr 2024 16:45:10 -0400 Subject: [PATCH 01/10] gsf import issue not solved --- examples/ex_gsf_se3.py | 104 ++++++++++++++++++ navlie/__init__.py | 1 + navlie/gsf.py | 243 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 348 insertions(+) create mode 100644 examples/ex_gsf_se3.py create mode 100644 navlie/gsf.py diff --git a/examples/ex_gsf_se3.py b/examples/ex_gsf_se3.py new file mode 100644 index 00000000..540828dc --- /dev/null +++ b/examples/ex_gsf_se3.py @@ -0,0 +1,104 @@ +from navlie.lib import SE2State, BodyFrameVelocity, RangePoseToAnchor +from navlie.gsf import GaussianSumFilter + +import navlie as nav +import numpy as np +from typing import List + +""" +This example runs an Interacting Multiple Model filter to estimate the process model noise matrix +for a state that is on a Lie group. The performance is compared to an EKF that knows the ground +truth process model noise. +""" + +# 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 = 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 Interacting Multiple Model Filter trial + """ + np.random.seed(trial_number) + state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True) + + x0_check = x0.plus(nav.randvec(P0)) + + estimate_list = nav.gsf.run_gsf_filter( + gsf, x0_check, P0, input_list, meas_list + ) + + results = [ + nav.imm.IMMResult(estimate_list[i], state_true[i]) + for i in range(len(estimate_list)) + ] + + return nav.imm.IMMResultList(results) + +N = 1 +results = nav.monte_carlo(gsf_trial, N) + +if __name__ == "__main__": + import matplotlib.pyplot as plt + import seaborn as sns + + sns.set_theme(style="whitegrid") + + fig, ax = plt.subplots(1, 1) + ax.plot(results.stamp, results.average_nees, label="IMM NEES") + ax.plot( + results.stamp, results.expected_nees, color="r", label="Expected NEES" + ) + ax.plot( + results.stamp, + results.nees_lower_bound(0.99), + color="k", + linestyle="--", + label="99 percent c.i.", + ) + ax.plot( + results.stamp, + results.nees_upper_bound(0.99), + color="k", + linestyle="--", + ) + ax.set_title("{0}-trial average NEES".format(results.num_trials)) + ax.set_ylim(0, None) + ax.set_xlabel("Time (s)") + ax.set_ylabel("NEES") + ax.legend() + + plt.show() \ No newline at end of file diff --git a/navlie/__init__.py b/navlie/__init__.py index 45e8085d..9fc0b8e5 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -19,6 +19,7 @@ from . import batch from . import imm from . import lib +from . import gsf from .batch import BatchEstimator from .datagen import DataGenerator, generate_measurement diff --git a/navlie/gsf.py b/navlie/gsf.py new file mode 100644 index 00000000..6527dd41 --- /dev/null +++ b/navlie/gsf.py @@ -0,0 +1,243 @@ +""" +Module containing many predict-correct style filters. +""" + +from typing import List +from navlie.types import ( + Input, + State, + ProcessModel, + Measurement, + StateWithCovariance, +) +import numpy as np +from tqdm import tqdm +from filters import ExtendedKalmanFilter +from scipy.stats import multivariate_normal +from navlie.utils import GaussianResultList, GaussianResult, state_interp +from .imm import gaussian_mixing + +class GMMState: + __slots__ = ["states", "weights"] + + def __init__( + self, + states: List[StateWithCovariance], + weights: List[float], + ): + self.states = states + self.weights = weights + + @property + def stamp(self): + return self.states[0].state.stamp + + def copy(self) -> "GMMState": + x_copy = [x.copy() for x in self.states] + return GMMState(x_copy, self.weights.copy()) + +class GSFResult(GaussianResult): + __slots__ = [ + "stamp", + "state", + "state_true", + "covariance", + "error", + "ees", + "nees", + "md", + "three_sigma", + "weights", + ] + + def __init__(self, gsf_estimate: GMMState, state_true: State): + super().__init__( + gaussian_mixing( + gsf_estimate.weights, gsf_estimate.states + ), + state_true, + ) + + self.weights = gsf_estimate.weights + + +class GaussianSumFilter: + """ + On-manifold Gaussian Sum Filter. + """ + + __slots__ = [ + "process_model", + "reject_outliers", + ] + + 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: GMMState, + u: Input, + dt: float = None, + ) -> GMMState: + """ + 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 : GMMState + 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 + ------- + GMMState + New predicted states with associated weights. + """ + + n_modes = len(x.states) + + x_check = [] + for i in range(n_modes): + x_check.append(self.ekf.predict(x.states[i], u, dt)) + return GMMState(x_check, x.weights) + + def correct( + self, + x: GMMState, + u: Input, + y: Measurement, + ) -> GMMState: + """ + Corrects the state estimate using a measurement. The user must provide + the current state and measurement. + + Parameters + ---------- + x : GMMState + The current states and their associated weights. + y : Measurement + Measurement to correct the state estimate. + + Returns + ------- + GMMState + Corrected states with associated weights. + """ + n_modes = len(x.states) + weights_check = x.weights.copy() + + x_hat = [] + weights_hat = [] + for i in range(n_modes): + x, details_dict = self.ekf.correct(x.states[i], y, u) + 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.append(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 = np.array(weights_hat) / np.sum(weights_hat) + + return GMMState(x_hat, weights_hat) + + +def run_gsf_filter( + filter: GaussianSumFilter, + x0: State, + P0: np.ndarray, + 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 : State + _description_ + P0 : np.ndarray + _description_ + input_data : List[Input] + _description_ + meas_data : List[Measurement] + _description_ + """ + x = StateWithCovariance(x0, P0) + if x.state.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.state.stamp: + input_data = input_data[idx:] + break + + for idx, y in enumerate(meas_data): + if y.stamp >= x.state.stamp: + meas_data = meas_data[idx:] + break + + meas_idx = 0 + if len(meas_data) > 0: + y = meas_data[meas_idx] + + n_modes = 1 + weights = [1.0] + x = GMMState( + [StateWithCovariance(x0, P0)] * n_modes, weights + ) + + 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 From 2cfe7ef43b9ba1d9c202ea10c6653156032982a4 Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Mon, 29 Apr 2024 09:45:46 -0400 Subject: [PATCH 02/10] Fix import issues --- examples/ex_gsf_se3.py | 3 +-- navlie/gsf.py | 5 +++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/ex_gsf_se3.py b/examples/ex_gsf_se3.py index 540828dc..45d75fa3 100644 --- a/examples/ex_gsf_se3.py +++ b/examples/ex_gsf_se3.py @@ -1,5 +1,4 @@ from navlie.lib import SE2State, BodyFrameVelocity, RangePoseToAnchor -from navlie.gsf import GaussianSumFilter import navlie as nav import numpy as np @@ -36,7 +35,7 @@ def input_profile(t, u): measurement_freq = 5 # gsf filter -gsf = GaussianSumFilter(process_model) +gsf = nav.gsf.GaussianSumFilter(process_model) dg = nav.DataGenerator( process_model, diff --git a/navlie/gsf.py b/navlie/gsf.py index 6527dd41..9e2502a5 100644 --- a/navlie/gsf.py +++ b/navlie/gsf.py @@ -12,10 +12,10 @@ ) import numpy as np from tqdm import tqdm -from filters import ExtendedKalmanFilter +from navlie.filters import ExtendedKalmanFilter from scipy.stats import multivariate_normal from navlie.utils import GaussianResultList, GaussianResult, state_interp -from .imm import gaussian_mixing +from navlie.imm import gaussian_mixing class GMMState: __slots__ = ["states", "weights"] @@ -69,6 +69,7 @@ class GaussianSumFilter: __slots__ = [ "process_model", "reject_outliers", + "ekf", ] def __init__( From 8e3018e3f0d2bccd01d26004fa2a5458dfc65d14 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Thu, 16 May 2024 15:41:05 -0400 Subject: [PATCH 03/10] gsf working fine --- examples/ex_gsf_se3.py | 52 ++++++++++++++++----------------------- navlie/gsf.py | 54 +++++++++++++++++++++++++++-------------- navlie/mixture_utils.py | 0 3 files changed, 57 insertions(+), 49 deletions(-) create mode 100644 navlie/mixture_utils.py diff --git a/examples/ex_gsf_se3.py b/examples/ex_gsf_se3.py index 45d75fa3..ddde8f9c 100644 --- a/examples/ex_gsf_se3.py +++ b/examples/ex_gsf_se3.py @@ -4,6 +4,7 @@ import numpy as np from typing import List + """ This example runs an Interacting Multiple Model filter to estimate the process model noise matrix for a state that is on a Lie group. The performance is compared to an EKF that knows the ground @@ -53,51 +54,40 @@ def gsf_trial(trial_number: int) -> List[nav.GaussianResult]: """ np.random.seed(trial_number) state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True) - - x0_check = x0.plus(nav.randvec(P0)) + + # 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] + + weights = [1, 1] + x0_check = nav.gsf.GMMState( + [nav.StateWithCovariance(_x, P0) for _x in x], weights + ) estimate_list = nav.gsf.run_gsf_filter( gsf, x0_check, P0, input_list, meas_list ) results = [ - nav.imm.IMMResult(estimate_list[i], state_true[i]) + nav.gsf.GSFResult(estimate_list[i], state_true[i]) for i in range(len(estimate_list)) ] - return nav.imm.IMMResultList(results) + return nav.gsf.GSFResultList(results) -N = 1 -results = nav.monte_carlo(gsf_trial, N) +N = 2 +results = gsf_trial(0) if __name__ == "__main__": import matplotlib.pyplot as plt import seaborn as sns sns.set_theme(style="whitegrid") - - fig, ax = plt.subplots(1, 1) - ax.plot(results.stamp, results.average_nees, label="IMM NEES") - ax.plot( - results.stamp, results.expected_nees, color="r", label="Expected NEES" - ) - ax.plot( - results.stamp, - results.nees_lower_bound(0.99), - color="k", - linestyle="--", - label="99 percent c.i.", - ) - ax.plot( - results.stamp, - results.nees_upper_bound(0.99), - color="k", - linestyle="--", - ) - ax.set_title("{0}-trial average NEES".format(results.num_trials)) - ax.set_ylim(0, None) - ax.set_xlabel("Time (s)") - ax.set_ylabel("NEES") - ax.legend() - + fig, ax = nav.plot_error(results) + 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.show() \ No newline at end of file diff --git a/navlie/gsf.py b/navlie/gsf.py index 9e2502a5..05c531a7 100644 --- a/navlie/gsf.py +++ b/navlie/gsf.py @@ -14,7 +14,7 @@ from tqdm import tqdm from navlie.filters import ExtendedKalmanFilter from scipy.stats import multivariate_normal -from navlie.utils import GaussianResultList, GaussianResult, state_interp +from navlie.utils import GaussianResult, GaussianResultList from navlie.imm import gaussian_mixing class GMMState: @@ -34,7 +34,7 @@ def stamp(self): def copy(self) -> "GMMState": x_copy = [x.copy() for x in self.states] - return GMMState(x_copy, self.weights.copy()) + return GMMState(x_copy, self.weights.copy() / np.sum(self.weights)) class GSFResult(GaussianResult): __slots__ = [ @@ -57,9 +57,31 @@ def __init__(self, gsf_estimate: GMMState, state_true: State): ), state_true, ) - self.weights = gsf_estimate.weights +class GSFResultList(GaussianResultList): + __slots__ = [ + "stamp", + "state", + "state_true", + "covariance", + "error", + "ees", + "nees", + "md", + "three_sigma", + "value", + "value_true", + "dof", + "weights", + ] + + def __init__(self, result_list: List[GSFResult]): + super().__init__(result_list) + self.weights = np.array( + [r.weights for r in result_list] + ) + class GaussianSumFilter: """ @@ -126,8 +148,8 @@ def predict( def correct( self, x: GMMState, - u: Input, y: Measurement, + u: Input, ) -> GMMState: """ Corrects the state estimate using a measurement. The user must provide @@ -145,27 +167,29 @@ def correct( GMMState Corrected states with associated weights. """ + x_check = x.copy() n_modes = len(x.states) weights_check = x.weights.copy() x_hat = [] - weights_hat = [] + weights_hat = np.zeros(n_modes) for i in range(n_modes): - x, details_dict = self.ekf.correct(x.states[i], y, u) + x, details_dict = self.ekf.correct(x_check.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.append(weights_check[i] * model_likelihood) + 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 = np.array(weights_hat) / np.sum(weights_hat) + weights_hat = weights_hat / np.sum(weights_hat) return GMMState(x_hat, weights_hat) @@ -195,8 +219,8 @@ def run_gsf_filter( meas_data : List[Measurement] _description_ """ - x = StateWithCovariance(x0, P0) - if x.state.stamp is None: + x = x0.copy() + if x.stamp is None: raise ValueError("x0 must have a valid timestamp.") # Sort the data by time @@ -205,12 +229,12 @@ def run_gsf_filter( # Remove all that are before the current time for idx, u in enumerate(input_data): - if u.stamp >= x.state.stamp: + if u.stamp >= x.stamp: input_data = input_data[idx:] break for idx, y in enumerate(meas_data): - if y.stamp >= x.state.stamp: + if y.stamp >= x.stamp: meas_data = meas_data[idx:] break @@ -218,12 +242,6 @@ def run_gsf_filter( if len(meas_data) > 0: y = meas_data[meas_idx] - n_modes = 1 - weights = [1.0] - x = GMMState( - [StateWithCovariance(x0, P0)] * n_modes, weights - ) - results_list = [] for k in tqdm(range(len(input_data) - 1), disable=disable_progress_bar): u = input_data[k] diff --git a/navlie/mixture_utils.py b/navlie/mixture_utils.py new file mode 100644 index 00000000..e69de29b From d4474b73de54a33e40c49f5fb14096f6fa2c12e3 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Thu, 16 May 2024 15:41:29 -0400 Subject: [PATCH 04/10] gsf working fine --- navlie/mixture_utils.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 navlie/mixture_utils.py diff --git a/navlie/mixture_utils.py b/navlie/mixture_utils.py deleted file mode 100644 index e69de29b..00000000 From c3642cda78e1662507aeed18cf2c7f0302f0bc12 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Tue, 25 Jun 2024 14:04:03 -0400 Subject: [PATCH 05/10] gsf working and cleaned up --- examples/ex_gsf_se3.py | 157 ++++++++------ examples/ex_imm_se3.py | 4 +- examples/ex_imm_vector.py | 4 +- navlie/__init__.py | 6 +- navlie/filters.py | 204 ++++++++++++++++-- navlie/gsf.py | 106 ++------- navlie/lib/__init__.py | 2 +- navlie/lib/states.py | 6 +- navlie/utils/__init__.py | 4 +- navlie/utils/common.py | 54 ++--- navlie/utils/mixture.py | 3 +- tests/integration/test_filter_gsf.py | 141 ++++++++++++ .../test_interacting_multiple_models.py | 6 +- tests/test_examples.py | 7 + 14 files changed, 493 insertions(+), 211 deletions(-) create mode 100644 tests/integration/test_filter_gsf.py diff --git a/examples/ex_gsf_se3.py b/examples/ex_gsf_se3.py index ddde8f9c..0d01a042 100644 --- a/examples/ex_gsf_se3.py +++ b/examples/ex_gsf_se3.py @@ -6,88 +6,113 @@ """ -This example runs an Interacting Multiple Model filter to estimate the process model noise matrix -for a state that is on a Lie group. The performance is compared to an EKF that knows the ground -truth process model noise. +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. """ -# 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.gsf.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 Interacting Multiple Model 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] - - weights = [1, 1] - x0_check = nav.gsf.GMMState( - [nav.StateWithCovariance(_x, P0) for _x in x], weights - ) +def main(): - estimate_list = nav.gsf.run_gsf_filter( - gsf, x0_check, P0, input_list, meas_list - ) + # Create the process model noise profile + Q = np.diag([0.1**2, 0.1, 0.1]) + process_model = BodyFrameVelocity(Q) - results = [ - nav.gsf.GSFResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) + + # Measurement model + R = 0.1**2 + range_models = [ + RangePoseToAnchor([-5, 5],[0, 0], R), + RangePoseToAnchor([ 5, 5],[0, 0], R), ] - return nav.gsf.GSFResultList(results) + # 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 -N = 2 -results = gsf_trial(0) + # 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 Interacting Multiple Model 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.gsf.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_est = SE2State([0, 5, 0], stamp=0.0) + x0_check = x0_est.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) + 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 a0b7b2ed..73c02242 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 @@ -40,8 +42,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..64bcb79a 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,118 @@ 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. + """ + + __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. + + 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( @@ -1101,7 +1212,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 +1233,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/gsf.py b/navlie/gsf.py index 05c531a7..4b0e6761 100644 --- a/navlie/gsf.py +++ b/navlie/gsf.py @@ -10,77 +10,12 @@ Measurement, StateWithCovariance, ) +from navlie.lib.states import MixtureState import numpy as np from tqdm import tqdm from navlie.filters import ExtendedKalmanFilter from scipy.stats import multivariate_normal -from navlie.utils import GaussianResult, GaussianResultList -from navlie.imm import gaussian_mixing - -class GMMState: - __slots__ = ["states", "weights"] - - def __init__( - self, - states: List[StateWithCovariance], - weights: List[float], - ): - self.states = states - self.weights = weights - - @property - def stamp(self): - return self.states[0].state.stamp - - def copy(self) -> "GMMState": - x_copy = [x.copy() for x in self.states] - return GMMState(x_copy, self.weights.copy() / np.sum(self.weights)) - -class GSFResult(GaussianResult): - __slots__ = [ - "stamp", - "state", - "state_true", - "covariance", - "error", - "ees", - "nees", - "md", - "three_sigma", - "weights", - ] - - def __init__(self, gsf_estimate: GMMState, state_true: State): - super().__init__( - gaussian_mixing( - gsf_estimate.weights, gsf_estimate.states - ), - state_true, - ) - self.weights = gsf_estimate.weights - -class GSFResultList(GaussianResultList): - __slots__ = [ - "stamp", - "state", - "state_true", - "covariance", - "error", - "ees", - "nees", - "md", - "three_sigma", - "value", - "value_true", - "dof", - "weights", - ] - - def __init__(self, result_list: List[GSFResult]): - super().__init__(result_list) - self.weights = np.array( - [r.weights for r in result_list] - ) +from navlie.utils import GaussianResult, GaussianResultList, gaussian_mixing class GaussianSumFilter: @@ -111,10 +46,10 @@ def __init__( def predict( self, - x: GMMState, + x: MixtureState, u: Input, dt: float = None, - ) -> GMMState: + ) -> MixtureState: """ Propagates the state forward in time using a process model. The user must provide the current state, input, and time interval @@ -125,7 +60,7 @@ def predict( Parameters ---------- - x : GMMState + x : MixtureState The current states and their associated weights. u : Input Input measurement to be given to process model @@ -134,47 +69,47 @@ def predict( with ``dt = u.stamp - x.state.stamp``. Returns ------- - GMMState + MixtureState New predicted states with associated weights. """ - n_modes = len(x.states) + n_modes = len(x.model_states) x_check = [] for i in range(n_modes): - x_check.append(self.ekf.predict(x.states[i], u, dt)) - return GMMState(x_check, x.weights) + x_check.append(self.ekf.predict(x.model_states[i], u, dt)) + return MixtureState(x_check, x.model_probabilities) def correct( self, - x: GMMState, + x: MixtureState, y: Measurement, u: Input, - ) -> GMMState: + ) -> MixtureState: """ Corrects the state estimate using a measurement. The user must provide the current state and measurement. Parameters ---------- - x : GMMState + x : MixtureState The current states and their associated weights. y : Measurement Measurement to correct the state estimate. Returns ------- - GMMState + MixtureState Corrected states with associated weights. """ x_check = x.copy() - n_modes = len(x.states) - weights_check = x.weights.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.states[i], y, u, + x, details_dict = self.ekf.correct(x_check.model_states[i], y, u, output_details=True) x_hat.append(x) z = details_dict["z"] @@ -191,13 +126,12 @@ def correct( weights_hat = weights_hat / np.sum(weights_hat) - return GMMState(x_hat, weights_hat) + return MixtureState(x_hat, weights_hat) def run_gsf_filter( filter: GaussianSumFilter, - x0: State, - P0: np.ndarray, + x0: StateWithCovariance, input_data: List[Input], meas_data: List[Measurement], disable_progress_bar: bool = False, @@ -210,9 +144,7 @@ def run_gsf_filter( ---------- filter : GaussianSumFilter _description_ - x0 : State - _description_ - P0 : np.ndarray + x0 : StateWithCovariance _description_ input_data : List[Input] _description_ 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 b86e38fd..fe66bceb 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 @@ -554,7 +554,7 @@ def nees_upper_bound(self, confidence_interval: float, double_sided=True): ) -class IMMResult(GaussianResult): +class MixtureResult(GaussianResult): __slots__ = [ "stamp", "state", @@ -568,7 +568,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 @@ -579,7 +579,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", @@ -596,7 +596,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] @@ -604,18 +604,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 @@ -624,15 +624,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) ] ) @@ -1434,7 +1434,7 @@ def nees_upper_bound(self, confidence_interval: float, double_sided=True): ) -class IMMResult(GaussianResult): +class MixtureResult(GaussianResult): __slots__ = [ "stamp", "state", @@ -1448,7 +1448,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 @@ -1459,7 +1459,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", @@ -1476,7 +1476,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] @@ -1484,18 +1484,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 @@ -1504,15 +1504,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..121a3cff 100644 --- a/navlie/utils/mixture.py +++ b/navlie/utils/mixture.py @@ -11,8 +11,7 @@ StateWithCovariance, ) import numpy as np -from navlie.lib import IMMState -from tqdm import tqdm +from navlie.lib import MixtureState # TODO. The IMM seems to have an issue when the user accidently modifies the 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..f753f618 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_se3.py """ @@ -121,3 +122,9 @@ def test_ex_slam(): from ex_slam import main main() + + +def test_ex_gsf_se3(): + from ex_gsf_se3 import main + + main() From 3e86a6129d136b81a2870794cfd57c419a2eac89 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Tue, 25 Jun 2024 14:05:59 -0400 Subject: [PATCH 06/10] test_ex_gsf_se2 name updated to correct name --- examples/{ex_gsf_se3.py => ex_gsf_se2.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{ex_gsf_se3.py => ex_gsf_se2.py} (100%) diff --git a/examples/ex_gsf_se3.py b/examples/ex_gsf_se2.py similarity index 100% rename from examples/ex_gsf_se3.py rename to examples/ex_gsf_se2.py From 09cde08f9302386dae1baebe2a7361fa067e9237 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Tue, 25 Jun 2024 14:08:43 -0400 Subject: [PATCH 07/10] all tests are passing --- tests/test_examples.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_examples.py b/tests/test_examples.py index f753f618..bb9589d8 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -124,7 +124,7 @@ def test_ex_slam(): main() -def test_ex_gsf_se3(): - from ex_gsf_se3 import main +def test_ex_gsf_se2(): + from ex_gsf_se2 import main main() From d201ac824ef43a1c10dad4e9e12f04a6456154b4 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Tue, 25 Jun 2024 14:19:53 -0400 Subject: [PATCH 08/10] deleted duplicate functions and files, tests passing --- examples/ex_gsf_se2.py | 6 +- navlie/__init__.py | 1 - navlie/gsf.py | 194 ----------------------------------------- 3 files changed, 3 insertions(+), 198 deletions(-) delete mode 100644 navlie/gsf.py diff --git a/examples/ex_gsf_se2.py b/examples/ex_gsf_se2.py index 0d01a042..a343836b 100644 --- a/examples/ex_gsf_se2.py +++ b/examples/ex_gsf_se2.py @@ -66,7 +66,7 @@ def gsf_trial(trial_number: int) -> List[nav.GaussianResult]: [nav.StateWithCovariance(_x, P0) for _x in x], [1/len(x) for _ in x] ) - estimate_list = nav.gsf.run_gsf_filter( + estimate_list = nav.run_gsf_filter( gsf, x0_check, input_list, meas_list ) @@ -89,8 +89,8 @@ def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: np.random.seed(trial_number) state_true, input_list, meas_list = dg.generate(x0, 0, t_max, noise=True) - x0_est = SE2State([0, 5, 0], stamp=0.0) - x0_check = x0_est.plus(nav.randvec(P0)) + 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) diff --git a/navlie/__init__.py b/navlie/__init__.py index 73c02242..2d805305 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -22,7 +22,6 @@ ) from . import batch from . import lib -from . import gsf from . import utils from .batch import BatchEstimator diff --git a/navlie/gsf.py b/navlie/gsf.py deleted file mode 100644 index 4b0e6761..00000000 --- a/navlie/gsf.py +++ /dev/null @@ -1,194 +0,0 @@ -""" -Module containing many predict-correct style filters. -""" - -from typing import List -from navlie.types import ( - Input, - State, - ProcessModel, - Measurement, - StateWithCovariance, -) -from navlie.lib.states import MixtureState -import numpy as np -from tqdm import tqdm -from navlie.filters import ExtendedKalmanFilter -from scipy.stats import multivariate_normal -from navlie.utils import GaussianResult, GaussianResultList, gaussian_mixing - - -class GaussianSumFilter: - """ - On-manifold Gaussian Sum Filter. - """ - - __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. - - 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_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 From 08b700a12a954b958d12acde8e6354cd9f878a14 Mon Sep 17 00:00:00 2001 From: Syed Shabbir Ahmed Date: Thu, 27 Jun 2024 10:19:03 -0400 Subject: [PATCH 09/10] added comments, cleaned up utils/mixture --- navlie/filters.py | 26 +++++++++++++++++++++----- navlie/utils/mixture.py | 8 -------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/navlie/filters.py b/navlie/filters.py index 64bcb79a..a22cbd06 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -982,13 +982,26 @@ def correct( class GaussianSumFilter: """ - On-manifold Gaussian Sum Filter. + 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", + "_ekf", ] def __init__( @@ -1004,7 +1017,7 @@ def __init__( reject_outliers : bool, optional whether to apply the NIS test to measurements, by default False """ - self.ekf = ExtendedKalmanFilter(process_model, reject_outliers) + self._ekf = ExtendedKalmanFilter(process_model, reject_outliers) def predict( self, @@ -1039,7 +1052,7 @@ def predict( x_check = [] for i in range(n_modes): - x_check.append(self.ekf.predict(x.model_states[i], u, dt)) + x_check.append(self._ekf.predict(x.model_states[i], u, dt)) return MixtureState(x_check, x.model_probabilities) def correct( @@ -1071,7 +1084,7 @@ def correct( 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, + x, details_dict = self._ekf.correct(x_check.model_states[i], y, u, output_details=True) x_hat.append(x) z = details_dict["z"] @@ -1159,6 +1172,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, diff --git a/navlie/utils/mixture.py b/navlie/utils/mixture.py index 121a3cff..38c88528 100644 --- a/navlie/utils/mixture.py +++ b/navlie/utils/mixture.py @@ -6,17 +6,9 @@ from navlie.types import ( State, - Input, - Measurement, StateWithCovariance, ) import numpy as np -from navlie.lib import MixtureState - - -# TODO. The IMM seems to have an issue when the user accidently modifies the -# provided state in the process model. - def gaussian_mixing_vectorspace( From 1f626fd87f9e2c15115fc47aff4877ac0e795d77 Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Wed, 26 Jun 2024 09:24:32 -0400 Subject: [PATCH 10/10] Minor fix in np.linalg.solve dimensions minor edit in van loans comment section fixed minor comments minor comment edit minor comment edited --- examples/ex_gsf_se2.py | 2 +- navlie/filters.py | 4 +++- navlie/utils/common.py | 11 +++++++++-- tests/test_examples.py | 2 +- 4 files changed, 14 insertions(+), 5 deletions(-) diff --git a/examples/ex_gsf_se2.py b/examples/ex_gsf_se2.py index a343836b..e6fda7f6 100644 --- a/examples/ex_gsf_se2.py +++ b/examples/ex_gsf_se2.py @@ -52,7 +52,7 @@ def input_profile(t, u): def gsf_trial(trial_number: int) -> List[nav.GaussianResult]: """ - A single Interacting Multiple Model Filter trial + A single Gaussian Sum Filter trial """ np.random.seed(trial_number) state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True) diff --git a/navlie/filters.py b/navlie/filters.py index a22cbd06..879f17e4 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -1071,7 +1071,9 @@ def correct( 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 diff --git a/navlie/utils/common.py b/navlie/utils/common.py index 81a1ca95..1bbc3943 100644 --- a/navlie/utils/common.py +++ b/navlie/utils/common.py @@ -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 @@ -672,8 +673,14 @@ def __getitem__(self, key): out.nees = out.error**2 / out.covariance.flatten() out.dof = np.ones_like(out.stamp) else: + n_times = out.covariance.shape[0] + n_error = out.covariance.shape[1] out.nees = np.sum( - out.error * np.linalg.solve(out.covariance, out.error), axis=1 + out.error + * np.linalg.solve( + out.covariance, out.error.reshape((n_times, n_error, 1)) + ).reshape((n_times, n_error)), + axis=1, ) out.dof = out.error.shape[1] * np.ones_like(out.stamp) diff --git a/tests/test_examples.py b/tests/test_examples.py index bb9589d8..90fa84a5 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -22,7 +22,7 @@ examples/ex_varying_noise.py examples/ex_slam.py examples/ex_gaussian_mixture.py -examples/ex_gsf_se3.py +examples/ex_gsf_se2.py """