From 366f55c142928fdbb6385267634ca322358c6c10 Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Fri, 28 Jul 2023 15:42:27 -0400 Subject: [PATCH 1/2] All the examples now get tested to make sure they dont throw errors --- examples/ex_batch_se3.py | 130 +++++----- examples/ex_batch_vector.py | 137 +++++------ examples/ex_ekf_vector.py | 214 ++++++++--------- examples/ex_imm_se3.py | 209 ++++++++++++++++ examples/ex_imm_vector.py | 125 ++++++++++ examples/ex_inertial_gps.py | 105 ++++---- examples/ex_inertial_nav.py | 80 ++++--- examples/ex_interacting_multiple_model_se3.py | 225 ------------------ .../ex_interacting_multiple_model_vector.py | 217 ----------------- examples/ex_invariant_so3.py | 115 +++++---- examples/ex_iterated_ekf_se3.py | 119 +++++---- examples/ex_monte_carlo.py | 129 +++++----- examples/ex_random_walk.py | 187 ++++++++------- examples/ex_sequential_measurements.py | 109 +++++---- examples/ex_ukf_se2.py | 80 ++++--- examples/ex_ukf_vector.py | 113 ++++----- examples/ex_varying_noise.py | 172 ++++++------- navlie/__init__.py | 4 + navlie/datagen.py | 3 + navlie/imm.py | 109 ++++++--- navlie/lib/__init__.py | 3 +- navlie/lib/models.py | 10 +- navlie/utils.py | 12 +- .../test_interacting_multiple_models.py | 4 +- tests/test_examples.py | 83 +++++++ 25 files changed, 1363 insertions(+), 1331 deletions(-) create mode 100644 examples/ex_imm_se3.py create mode 100644 examples/ex_imm_vector.py delete mode 100644 examples/ex_interacting_multiple_model_se3.py delete mode 100644 examples/ex_interacting_multiple_model_vector.py create mode 100644 tests/test_examples.py diff --git a/examples/ex_batch_se3.py b/examples/ex_batch_se3.py index 88ffe1d3..ec584df9 100644 --- a/examples/ex_batch_se3.py +++ b/examples/ex_batch_se3.py @@ -3,88 +3,82 @@ process and measurement models. """ -from navlie.batch.estimator import BatchEstimator -from navlie.lib.states import SE3State -from navlie.datagen import DataGenerator -from navlie.utils import GaussianResult, GaussianResultList, randvec -from navlie.lib.models import ( +from navlie.lib import ( BodyFrameVelocity, RangePoseToAnchor, + SE3State ) -from navlie.utils import plot_error +import navlie as nav import numpy as np -from typing import List -from pylie import SE3 import matplotlib.pyplot as plt -# ############################################################################# -# Create the batch estimator with desired settings -estimator = BatchEstimator(solver_type="GN", max_iters=20) +def main(): + # ########################################################################## + # Create the batch estimator with desired settings + estimator = nav.BatchEstimator(solver_type="GN", max_iters=20) -# ############################################################################## -# Problem Setup -t_end = 4 -x0 = SE3State(SE3.Exp([0, 0, 0, 0, 0, 0]), stamp=0.0) -P0 = 0.1**2 * np.identity(6) -R = 0.1**2 -Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) -range_models = [ - RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R), -] + # ########################################################################## + # Problem Setup + t_end = 4 + x0 = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0) + P0 = 0.1**2 * np.identity(6) + R = 0.1**2 + Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + range_models = [ + RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R), + ] -range_freqs = 20 -process_model = BodyFrameVelocity(Q) -input_profile = lambda t, x: np.array( - [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] -) + range_freqs = 20 + process_model = BodyFrameVelocity(Q) + input_profile = lambda t, x: np.array( + [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] + ) -input_covariance = Q -input_freq = 100 -noise_active = True + input_freq = 100 + noise_active = True -# Generate data with no noise -dg = DataGenerator( - process_model, - input_profile, - Q, - input_freq, - range_models, - range_freqs, -) + # Generate data with no noise + dg = nav.DataGenerator( + process_model, + input_profile, + Q, + input_freq, + range_models, + range_freqs, + ) -state_true, input_list, meas_list = dg.generate(x0, 0, t_end, noise_active) + state_true, input_list, meas_list = dg.generate(x0, 0, t_end, noise_active) -# Run batch -estimate_list, opt_results = estimator.solve( - x0, - P0, - input_list, - meas_list, - process_model, - return_opt_results=True, -) + # Run batch + estimate_list, opt_results = estimator.solve( + x0, + P0, + input_list, + meas_list, + process_model, + return_opt_results=True, + ) -print(opt_results["summary"]) + print(opt_results["summary"]) -results = GaussianResultList( - [ - GaussianResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) - ] -) + results = nav.GaussianResultList.from_estimates(estimate_list, state_true) + return results + -fig, ax = plot_error(results) -ax[-1][0].set_xlabel("Time (s)") -ax[-1][1].set_xlabel("Time (s)") -ax[0][0].set_title("Orientation Error") -ax[0][1].set_title("Position Error") -plt.show() +if __name__ == "__main__": + results = main() + fig, ax = nav.plot_error(results) + ax[-1][0].set_xlabel("Time (s)") + ax[-1][1].set_xlabel("Time (s)") + ax[0][0].set_title("Orientation Error") + ax[0][1].set_title("Position Error") + plt.show() diff --git a/examples/ex_batch_vector.py b/examples/ex_batch_vector.py index 8b1c207a..e6a0c365 100644 --- a/examples/ex_batch_vector.py +++ b/examples/ex_batch_vector.py @@ -6,91 +6,84 @@ models. """ -from navlie.batch.estimator import BatchEstimator -from navlie.lib.states import VectorState -from navlie.datagen import DataGenerator -from navlie.utils import ( - GaussianResult, - GaussianResultList, - randvec, - plot_error, - associate_stamps, -) -from navlie.lib.models import SingleIntegrator, RangePointToAnchor +import navlie as nav +from navlie.lib import SingleIntegrator, RangePointToAnchor, VectorState import numpy as np from typing import List import matplotlib.pyplot as plt -# ############################################################################# -# Create the batch estimator with desired settings -estimator = BatchEstimator(solver_type="GN", max_iters=20) +def main(): + # ############################################################################# + # Create the batch estimator with desired settings + estimator = nav.BatchEstimator(solver_type="GN", max_iters=5) -# ############################################################################## -# Problem Setup + # ############################################################################## + # Problem Setup -x0 = VectorState(np.array([1, 0]), stamp=0) -P0 = np.diag([1, 1]) -R = 0.1**2 -Q = 0.1 * np.identity(2) -range_models = [ - RangePointToAnchor([0, 4], R), - RangePointToAnchor([-2, 0], R), - RangePointToAnchor([2, 0], R), -] -range_freqs = [50, 50, 50] -process_model = SingleIntegrator(Q) -input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) -input_covariance = Q -input_freq = 180 -noise_active = True + x0 = VectorState(np.array([1, 0]), stamp=0) + P0 = np.diag([1, 1]) + R = 0.1**2 + Q = 0.1 * np.identity(2) + range_models = [ + RangePointToAnchor([0, 4], R), + RangePointToAnchor([-2, 0], R), + RangePointToAnchor([2, 0], R), + ] + range_freqs = [50, 50, 50] + process_model = SingleIntegrator(Q) + input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) + input_covariance = Q + input_freq = 180 + noise_active = True -# ############################################################################## -# Data Generation + # ############################################################################## + # Data Generation -dg = DataGenerator( - process_model, - input_profile, - input_covariance, - input_freq, - range_models, - range_freqs, -) + dg = nav.DataGenerator( + process_model, + input_profile, + input_covariance, + input_freq, + range_models, + range_freqs, + ) -gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) + gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) -# ############################################################################## -# Run batch -if noise_active: - x0 = x0.plus(randvec(P0)) + # ############################################################################## + # Run batch + if noise_active: + x0 = x0.plus(nav.randvec(P0)) -estimate_list, opt_results = estimator.solve( - x0, P0, input_data, meas_data, process_model, return_opt_results=True -) + estimate_list, opt_results = estimator.solve( + x0, P0, input_data, meas_data, process_model, return_opt_results=True + ) -print(opt_results["summary"]) -# # The estimate list returns the estimates at each of the -# # interoceptive and measurement timestamps. -# Find matching timestamps -estimate_stamps = [float(x.state.stamp) for x in estimate_list] -gt_stamps = [x.stamp for x in gt_data] + print(opt_results["summary"]) + # # The estimate list returns the estimates at each of the + # # interoceptive and measurement timestamps. + # Find matching timestamps + estimate_stamps = [float(x.state.stamp) for x in estimate_list] + gt_stamps = [x.stamp for x in gt_data] -matches = associate_stamps(estimate_stamps, gt_stamps) + matches = nav.associate_stamps(estimate_stamps, gt_stamps) -est_list = [] -gt_list = [] -for match in matches: - gt_list.append(gt_data[match[1]]) - est_list.append(estimate_list[match[0]]) + est_list = [] + gt_list = [] + for match in matches: + gt_list.append(gt_data[match[1]]) + est_list.append(estimate_list[match[0]]) -# Postprocess the results and plot -results = GaussianResultList( - [GaussianResult(est_list[i], gt_list[i]) for i in range(len(est_list))] -) + # Postprocess the results and plot + results = nav.GaussianResultList.from_estimates(est_list, gt_list) + return results -fig, ax = plot_error(results) -ax[0].set_title("Position") -ax[1].set_title("Velocity") -ax[0].set_xlabel("Time (s)") -ax[1].set_xlabel("Time (s)") -plt.tight_layout() -plt.show() +if __name__ == "__main__": + results = main() + fig, ax = nav.plot_error(results) + ax[0].set_title("Position") + ax[1].set_title("Velocity") + ax[0].set_xlabel("Time (s)") + ax[1].set_xlabel("Time (s)") + plt.tight_layout() + plt.show() diff --git a/examples/ex_ekf_vector.py b/examples/ex_ekf_vector.py index b1135177..026732e1 100644 --- a/examples/ex_ekf_vector.py +++ b/examples/ex_ekf_vector.py @@ -1,9 +1,5 @@ -from navlie.filters import ExtendedKalmanFilter, IteratedKalmanFilter -from navlie.lib.states import VectorState -from navlie.datagen import DataGenerator -from navlie.types import StateWithCovariance -from navlie.utils import GaussianResult, GaussianResultList, randvec -from navlie.lib.models import SingleIntegrator, RangePointToAnchor +from navlie.lib import SingleIntegrator, RangePointToAnchor, VectorState +import navlie as nav import numpy as np from typing import List import time @@ -14,104 +10,110 @@ on that data. """ -# ############################################################################## -# Problem Setup - -x0 = VectorState(np.array([1, 0]), stamp=0) -P0 = np.diag([1, 1]) -R = 0.1**2 -Q = 0.1 * np.identity(2) -range_models = [ - RangePointToAnchor([0, 4], R), - RangePointToAnchor([-2, 0], R), - RangePointToAnchor([2, 0], R), -] -range_freqs = [50, 50, 50] -process_model = SingleIntegrator(Q) -input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) -input_covariance = Q -input_freq = 180 -noise_active = True -# ############################################################################## -# Data Generation - -dg = DataGenerator( - process_model, - input_profile, - input_covariance, - input_freq, - range_models, - range_freqs, -) - -gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) - -# ############################################################################## -# Run Filter -if noise_active: - x0 = x0.plus(randvec(P0)) - -x = StateWithCovariance(x0, P0) - -ekf = ExtendedKalmanFilter(process_model) -# ekf = IteratedKalmanFilter(process_model) # or try the IEKF! - -meas_idx = 0 -start_time = time.time() -y = meas_data[meas_idx] -results_list = [] -for k in range(len(input_data) - 1): - - u = input_data[k] - - # Fuse any measurements that have occurred. - while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): - - x = ekf.correct(x, y, u) - - # Load the next measurement - meas_idx += 1 - if meas_idx < len(meas_data): - y = meas_data[meas_idx] - - - dt = input_data[k + 1].stamp - x.state.stamp - x = ekf.predict(x, u, dt) - - results_list.append(GaussianResult(x, gt_data[k+1])) - -print("Average filter computation frequency (Hz):") -print(1 / ((time.time() - start_time) / len(input_data))) - - -# ############################################################################## -# Post processing -results = GaussianResultList(results_list) - -import matplotlib.pyplot as plt -import seaborn as sns - -sns.set_theme() -fig, ax = plt.subplots(1, 1) -ax.plot(results.value[:, 0], results.value[:, 1], label="Estimate") -ax.plot( - results.value_true[:, 0], results.value_true[:, 1], label="Ground truth" -) -ax.set_title("Trajectory") -ax.set_xlabel("x (m)") -ax.set_ylabel("y (m)") -ax.legend() - -fig, axs = plt.subplots(2, 1) -axs: List[plt.Axes] = axs -for i in range(len(axs)): - axs[i].fill_between( - results.stamp, - results.three_sigma[:, i], - -results.three_sigma[:, i], - alpha=0.5, + +def main(): + # ############################################################################## + # Problem Setup + + x0 = VectorState(np.array([1, 0]), stamp=0) + P0 = np.diag([1, 1]) + R = 0.1**2 + Q = 0.1 * np.identity(2) + range_models = [ + RangePointToAnchor([0, 4], R), + RangePointToAnchor([-2, 0], R), + RangePointToAnchor([2, 0], R), + ] + range_freqs = [50, 50, 50] + process_model = SingleIntegrator(Q) + input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) + input_covariance = Q + input_freq = 180 + noise_active = True + # ############################################################################## + # Data Generation + + dg = nav.DataGenerator( + process_model, + input_profile, + input_covariance, + input_freq, + range_models, + range_freqs, + ) + + gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) + + # ############################################################################## + # Run Filter + if noise_active: + x0 = x0.plus(nav.randvec(P0)) + + x = nav.StateWithCovariance(x0, P0) + + ekf = nav.ExtendedKalmanFilter(process_model) + # ekf = IteratedKalmanFilter(process_model) # or try the IEKF! + + meas_idx = 0 + start_time = time.time() + y = meas_data[meas_idx] + results_list = [] + for k in range(len(input_data) - 1): + + u = input_data[k] + + # Fuse any measurements that have occurred. + while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): + + x = ekf.correct(x, y, u) + + # Load the next measurement + meas_idx += 1 + if meas_idx < len(meas_data): + y = meas_data[meas_idx] + + + dt = input_data[k + 1].stamp - x.state.stamp + x = ekf.predict(x, u, dt) + + results_list.append(nav.GaussianResult(x, gt_data[k+1])) + + print("Average filter computation frequency (Hz):") + print(1 / ((time.time() - start_time) / len(input_data))) + + + # ############################################################################## + # Post processing + results = nav.GaussianResultList(results_list) + return results + +if __name__ == "__main__": + results = main() + + import matplotlib.pyplot as plt + import seaborn as sns + + sns.set_theme() + fig, ax = plt.subplots(1, 1) + ax.plot(results.value[:, 0], results.value[:, 1], label="Estimate") + ax.plot( + results.value_true[:, 0], results.value_true[:, 1], label="Ground truth" ) - axs[i].plot(results.stamp, results.error[:, i]) -axs[0].set_title("Estimation error") -axs[1].set_xlabel("Time (s)") -plt.show() + ax.set_title("Trajectory") + ax.set_xlabel("x (m)") + ax.set_ylabel("y (m)") + ax.legend() + + fig, axs = plt.subplots(2, 1) + axs: List[plt.Axes] = axs + for i in range(len(axs)): + axs[i].fill_between( + results.stamp, + results.three_sigma[:, i], + -results.three_sigma[:, i], + alpha=0.5, + ) + axs[i].plot(results.stamp, results.error[:, i]) + axs[0].set_title("Estimation error") + axs[1].set_xlabel("Time (s)") + plt.show() diff --git a/examples/ex_imm_se3.py b/examples/ex_imm_se3.py new file mode 100644 index 00000000..75c7e340 --- /dev/null +++ b/examples/ex_imm_se3.py @@ -0,0 +1,209 @@ +from navlie.lib import SE3State, BodyFrameVelocity, RangePoseToAnchor +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 +c_list = [1, 9] +Q_ref = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + + +def Q_profile(t): + if t <= t_max / 4: + c = c_list[0] + if t > t_max / 4 and t <= t_max * 3 / 4: + c = c_list[1] + if t > t_max * 3 / 4: + c = c_list[0] + Q = c * Q_ref + return Q + + +# Measurement model +R = 0.1**2 +range_models = [ + RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R), + RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R), +] + +# Setup +x0 = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0) +P0 = 0.1**2 * np.identity(6) +input_profile = lambda t, x: np.array( + [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] +) +process_model_true = BodyFrameVelocity +input_freq = 10 +dt = 1 / input_freq +t_max = dt * 100 +measurement_freq = 5 + +# The two models correspond to the BodyVelocityModel which uses two different Q matrices. +# The two different Q matrices are a Q_ref matrix which is scaled by a scalar, c. +imm_process_model_list = [ + BodyFrameVelocity(c_list[0] * Q_ref), + BodyFrameVelocity(c_list[1] * Q_ref), +] + + +class VaryingNoiseProcessModel(process_model_true): + def __init__(self, Q_profile): + self.Q_profile = Q_profile + super().__init__(Q_profile(0)) + + def covariance(self, x, u, dt) -> np.ndarray: + self._Q = self.Q_profile(x.stamp) + return super().covariance(x, u, dt) + + +N = 5 +Q_dg = np.eye(x0.value.shape[0]) +n_models = len(imm_process_model_list) + +# Kalman Filter bank +kf_list = [nav.ExtendedKalmanFilter(pm) for pm in imm_process_model_list] + +# Set up probability transition matrix +off_diag_p = 0.02 +Pi = np.ones((n_models, n_models)) * off_diag_p +Pi = Pi + (1 - off_diag_p * (n_models)) * np.diag(np.ones(n_models)) +imm = nav.imm.InteractingModelFilter(kf_list, Pi) + + +dg = nav.DataGenerator( + VaryingNoiseProcessModel(Q_profile), + input_profile, + Q_profile, + input_freq, + range_models, + measurement_freq, +) + + +def imm_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.imm.run_imm_filter( + imm, 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) + + +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 = x0.plus(nav.randvec(P0)) + ekf = nav.ExtendedKalmanFilter(VaryingNoiseProcessModel(Q_profile)) + + estimate_list = nav.run_filter(ekf, x0_check, P0, input_list, meas_list) + return nav.GaussianResultList.from_estimates(estimate_list, state_true) + + +results = nav.monte_carlo(imm_trial, N) +results_ekf = nav.monte_carlo(ekf_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_ekf.average_nees, label="EKF using GT Q 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() + + + if N < 15: + + fig, axs = plt.subplots(3, 2) + axs: List[plt.Axes] = axs + for result in results.trial_results: + nav.plot_error(result, axs=axs) + + fig.suptitle("Estimation error IMM") + axs[1,0].set_xlabel("Time (s)") + + fig, axs = plt.subplots(3, 2) + axs: List[plt.Axes] = axs + for result in results_ekf.trial_results: + nav.plot_error(result, axs=axs) + + fig.suptitle("Estimation error EKF GT") + axs[1,0].set_xlabel("Time (s)") + + average_model_probabilities = np.average( + np.array([t.model_probabilities for t in results.trial_results]), axis=0 + ) + fig, ax = plt.subplots(1, 1) + for lv1 in range(n_models): + ax.plot(results.stamp, average_model_probabilities[lv1, :]) + ax.set_xlabel("Time (s)") + ax.set_ylabel("Model Probabilities") + + fig, ax = plt.subplots(1, 1) + Q_ = np.zeros(results.stamp.shape) + for lv1 in range(n_models): + Q_ = Q_ + average_model_probabilities[lv1, :] * c_list[lv1] * Q_ref[0, 0] + + ax.plot(results.stamp, Q_, label=r"$Q_{00}$, Estimated") + ax.plot( + results.stamp, + np.array([Q_profile(t)[0, 0] for t in results.stamp]), + label=r"$Q_{00}$, GT", + ) + ax.set_title("Estimated vs. GT Process Noise") + ax.set_xlabel("Time (s)") + ax.set_ylabel(r"$Q_{00}$") + + plt.show() diff --git a/examples/ex_imm_vector.py b/examples/ex_imm_vector.py new file mode 100644 index 00000000..ba7899fe --- /dev/null +++ b/examples/ex_imm_vector.py @@ -0,0 +1,125 @@ +""" +This example runs an Interacting Multiple Model filter to estimate the process +model noise matrix for a state that is on a vector space. The performance is +compared to an EKF that knows the ground truth process model noise. +""" + +import navlie as nav +from navlie.lib.models import DoubleIntegrator, RangePointToAnchor, VectorState +from navlie.imm import InteractingModelFilter, run_imm_filter, IMMResultList +import numpy as np +from typing import List +from matplotlib import pyplot as plt + +# Measurement model +R = 0.1**4 + +range_model = RangePointToAnchor(anchor_position=0.0, R=R) + +# Process model noise setup +c_list = [1, 9] +Q_ref = np.eye(1) +input_freq = 10 +dt = 1 / input_freq +t_max = dt * 1000 + +# For data generation, assume that the process model noise Q follows this profile. +def Q_profile(t): + if t <= t_max / 4: + c = c_list[0] + if t > t_max / 4 and t <= t_max * 3 / 4: + c = c_list[1] + if t > t_max * 3 / 4: + c = c_list[0] + Q = c * Q_ref + return Q + +def main(): + + # Setup + x0 = VectorState(np.array([1, 0]), 0.0) + P0 = np.diag([1, 1]) + input_profile = lambda t, x: np.array([np.sin(t)]) + measurement_freq = 5 + + # The two models correspond to the DoubleIntegrator which uses two different Q matrices. + # The two different Q matrices are a Q_ref matrix which is scaled by a scalar, c. + imm_process_model_list = [ + DoubleIntegrator(c_list[0] * Q_ref), + DoubleIntegrator(c_list[1] * Q_ref), + ] + + + n_models = len(imm_process_model_list) + + # Kalman Filter bank + kf_list = [nav.ExtendedKalmanFilter(pm) for pm in imm_process_model_list] + + # Set up probability transition matrix + off_diag_p = 0.02 + prob_trans_matrix = np.ones((n_models, n_models)) * off_diag_p + prob_trans_matrix = prob_trans_matrix + (1 - off_diag_p * (n_models)) * np.diag(np.ones(n_models)) + imm = InteractingModelFilter(kf_list, prob_trans_matrix) + + # Generate some data with varying Q matrix + dg = nav.DataGenerator( + DoubleIntegrator(Q_ref), + input_profile, + Q_profile, + input_freq, + range_model, + measurement_freq, + ) + state_true, input_list, meas_list = dg.generate(x0, 0, t_max, True) + + + # Add some noise to the initial state + x0_check = x0.plus(nav.randvec(P0)) + + estimate_list = run_imm_filter( + imm, x0_check, P0, input_list, meas_list + ) + + results = IMMResultList.from_estimates(estimate_list, state_true) + return results + + +if __name__ == "__main__": + results = main() + import matplotlib.pyplot as plt + import seaborn as sns + + sns.set_theme(style="whitegrid") + + # NEES Plot + fig, ax = nav.plot_nees(results) + ax.set_ylim(0, None) + ax.set_xlabel("Time (s)") + ax.set_ylabel("NEES") + ax.legend() + + # Error plot + fig, axs = nav.plot_error(results) + axs[0].set_title("Estimation error IMM") + axs[0].set_ylabel("Position Error (m)") + axs[1].set_ylabel("Velocity Error (m/s)") + axs[1].set_xlabel("Time (s)") + + # Model probabilities + fig, ax = plt.subplots(1, 1) + ax.plot(results.stamp, results.model_probabilities[:, 0], label="Model 1") + ax.plot(results.stamp, results.model_probabilities[:, 1], label="Model 2") + ax.set_xlabel("Time (s)") + ax.set_ylabel("Model Probabilities") + ax.legend() + + # Process noise as estimated by the IMM + fig, ax = plt.subplots(1, 1) + estimated_Q = np.sum(c_list*results.model_probabilities, axis=1) + true_Q = np.array([Q_profile(t)[0, 0] for t in results.stamp]) + ax.plot(results.stamp, estimated_Q, label="Estimated") + ax.plot(results.stamp, true_Q, label="True") + ax.set_xlabel("Time (s)") + ax.set_ylabel("Estimated vs True Process Noise") + ax.legend() + plt.show() diff --git a/examples/ex_inertial_gps.py b/examples/ex_inertial_gps.py index 1770c2cd..62cf8051 100644 --- a/examples/ex_inertial_gps.py +++ b/examples/ex_inertial_gps.py @@ -1,55 +1,56 @@ from typing import List - -import matplotlib.pyplot as plt import numpy as np - from navlie.lib.datasets import SimulatedInertialGPSDataset -from navlie.filters import ExtendedKalmanFilter, run_filter -from navlie.utils import GaussianResultList, plot_error, randvec - -np.set_printoptions(precision=3, suppress=True, linewidth=200) -np.random.seed(0) - -# ############################################################################## -# Create simulated IMU/GPS data -data = SimulatedInertialGPSDataset() -gt_states = data.get_ground_truth() -input_data = data.get_input_data() -meas_data = data.get_meas_data() - -# Filter initialization -P0 = np.eye(15) -P0[0:3, 0:3] *= 0.1**2 -P0[3:6, 3:6] *= 0.1**2 -P0[6:9, 6:9] *= 0.1**2 -P0[9:12, 9:12] *= 0.01**2 -P0[12:15, 12:15] *= 0.01**2 -x0 = gt_states[0].plus(randvec(P0)) -# ############################################################################## -# Run filter -ekf = ExtendedKalmanFilter(data.process_model) -estimate_list = run_filter(ekf, x0, P0, input_data, meas_data) - -# Postprocess the results and plot -results = GaussianResultList.from_estimates(estimate_list, gt_states) - -# ############################################################################## -# Plot results -from navlie.utils import plot_poses - -fig = plt.figure() -ax = plt.axes(projection="3d") -states_list = [x.state for x in estimate_list] -plot_poses(states_list, ax, line_color="tab:blue", step=20, label="Estimate") -plot_poses(gt_states, ax, line_color="tab:red", step=500, label="Groundtruth") -ax.legend() - -fig, axs = plot_error(results) -axs[0, 0].set_title("Attitude") -axs[0, 1].set_title("Velocity") -axs[0, 2].set_title("Position") -axs[0, 3].set_title("Gyro bias") -axs[0, 4].set_title("Accel bias") -axs[-1, 2] - -plt.show() +import navlie as nav + +def main(): + np.set_printoptions(precision=3, suppress=True, linewidth=200) + np.random.seed(0) + + # ########################################################################## + # Load pre-developed dataset + data = SimulatedInertialGPSDataset(t_start = 0, t_end=20) + gt_states = data.get_ground_truth() + input_data = data.get_input_data() + meas_data = data.get_meas_data() + + # Filter initialization + P0 = np.eye(15) + P0[0:3, 0:3] *= 0.1**2 + P0[3:6, 3:6] *= 0.1**2 + P0[6:9, 6:9] *= 0.1**2 + P0[9:12, 9:12] *= 0.01**2 + P0[12:15, 12:15] *= 0.01**2 + x0 = gt_states[0].plus(nav.randvec(P0)) + + # ########################################################################### + # Run filter + ekf = nav.ExtendedKalmanFilter(data.process_model) + estimate_list = nav.run_filter(ekf, x0, P0, input_data, meas_data) + + # Postprocess the results and plot + results = nav.GaussianResultList.from_estimates(estimate_list, gt_states) + return results + +if __name__ == "__main__": + results = main() + + # ########################################################################## + # Plot results + import matplotlib.pyplot as plt + + fig = plt.figure() + ax = plt.axes(projection="3d") + nav.plot_poses(results.state, ax, line_color="tab:blue", step=20, label="Estimate") + nav.plot_poses(results.state_true, ax, line_color="tab:red", step=500, label="Groundtruth") + ax.legend() + + fig, axs = nav.plot_error(results) + axs[0, 0].set_title("Attitude") + axs[0, 1].set_title("Velocity") + axs[0, 2].set_title("Position") + axs[0, 3].set_title("Gyro bias") + axs[0, 4].set_title("Accel bias") + axs[-1, 2] + + plt.show() diff --git a/examples/ex_inertial_nav.py b/examples/ex_inertial_nav.py index aadd561e..888751c0 100644 --- a/examples/ex_inertial_nav.py +++ b/examples/ex_inertial_nav.py @@ -1,21 +1,24 @@ +""" +A slightly more complicated example of a robot localizing itself from relative +position measurements to known landmarks. +""" from typing import List -import matplotlib.pyplot as plt import numpy as np from pylie import SE23 -from navlie.filters import ExtendedKalmanFilter, run_filter -from navlie.lib.models import ( +from navlie.lib import ( + IMU, + IMUState, + IMUKinematics, InvariantMeasurement, PointRelativePosition, ) -from navlie.lib.imu import IMU, IMUState, IMUKinematics -from navlie.utils import GaussianResult, GaussianResultList, plot_error, randvec -from navlie.datagen import DataGenerator +import navlie as nav # ############################################################################## # Problem Setup t_start = 0 -t_end = 30 +t_end = 15 imu_freq = 100 # IMU noise parameters @@ -123,7 +126,7 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: # Generate a random input to drive the bias random walk Q_bias = Q_noise[6:, 6:] - bias_noise = randvec(Q_bias) + bias_noise = nav.randvec(Q_bias) u = IMU(omega, accel, stamp, bias_noise[0:3], bias_noise[3:6]) return u @@ -138,7 +141,7 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: meas_model_list = [PointRelativePosition(pos, meas_cov) for pos in landmarks] # Create data generator -data_gen = DataGenerator( +data_gen = nav.DataGenerator( process_model, input_func=input_profile, input_covariance=Q_noise, @@ -165,9 +168,7 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: P0[12:15, 12:15] *= sigma_ba_init**2 # Generate all data -states_true, input_list, meas_list = data_gen.generate( - x0, t_start, t_end, noise=True -) +states_true, input_list, meas_list = data_gen.generate(x0, t_start, t_end, noise=True) # **************** Conversion to Invariant Measurements ! ********************* invariants = [InvariantMeasurement(meas, "right") for meas in meas_list] @@ -179,32 +180,35 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: u.bias_accel_walk = np.array([0, 0, 0]) -ekf = ExtendedKalmanFilter(process_model) +ekf = nav.ExtendedKalmanFilter(process_model) -estimate_list = run_filter(ekf, x0, P0, input_list, invariants) +estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants) # Postprocess the results and plot -results = GaussianResultList.from_estimates(estimate_list, states_true) - -from navlie.utils import plot_poses -import seaborn as sns - -fig = plt.figure() -ax = plt.axes(projection="3d") -landmarks = np.array(landmarks) -ax.scatter(landmarks[:, 0], landmarks[:, 1], landmarks[:, 2]) -states_list = [x.state for x in estimate_list] -plot_poses(states_list, ax, line_color="tab:blue", step=500, label="Estimate") -plot_poses(states_true, ax, line_color="tab:red", step=500, label="Groundtruth") -ax.legend() - -sns.set_theme() -fig, axs = plot_error(results) -axs[0, 0].set_title("Attitude") -axs[0, 1].set_title("Velocity") -axs[0, 2].set_title("Position") -axs[0, 3].set_title("Gyro bias") -axs[0, 4].set_title("Accel bias") -axs[-1, 2] - -plt.show() +results = nav.GaussianResultList.from_estimates(estimate_list, states_true) + +if __name__ == "__main__": + import matplotlib.pyplot as plt + import seaborn as sns + + fig = plt.figure() + ax = plt.axes(projection="3d") + landmarks = np.array(landmarks) + ax.scatter(landmarks[:, 0], landmarks[:, 1], landmarks[:, 2]) + states_list = [x.state for x in estimate_list] + nav.plot_poses(results.state, ax, line_color="tab:blue", step=500, label="Estimate") + nav.plot_poses( + results.state_true, ax, line_color="tab:red", step=500, label="Groundtruth" + ) + ax.legend() + + sns.set_theme() + fig, axs = nav.plot_error(results) + axs[0, 0].set_title("Attitude") + axs[0, 1].set_title("Velocity") + axs[0, 2].set_title("Position") + axs[0, 3].set_title("Gyro bias") + axs[0, 4].set_title("Accel bias") + axs[-1, 2] + + plt.show() diff --git a/examples/ex_interacting_multiple_model_se3.py b/examples/ex_interacting_multiple_model_se3.py deleted file mode 100644 index 90e824b3..00000000 --- a/examples/ex_interacting_multiple_model_se3.py +++ /dev/null @@ -1,225 +0,0 @@ -import pytest -from navlie.filters import ExtendedKalmanFilter, run_filter -from navlie.lib.states import SE3State -from navlie.datagen import DataGenerator -from navlie.utils import GaussianResult, GaussianResultList -from navlie.utils import randvec - -from navlie.utils import monte_carlo, plot_error -from navlie.lib.models import BodyFrameVelocity -from navlie.lib.models import RangePoseToAnchor -from pylie import SE3 - -import numpy as np -from typing import List -from matplotlib import pyplot as plt -from navlie.imm import InteractingModelFilter, run_interacting_multiple_model_filter -from navlie.imm import IMMResultList -from navlie.imm import IMMResult - -"""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 -c_list = [1, 9] -Q_ref = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) - - -def Q_profile(t): - if t <= t_max / 4: - c = c_list[0] - if t > t_max / 4 and t <= t_max * 3 / 4: - c = c_list[1] - if t > t_max * 3 / 4: - c = c_list[0] - Q = c * Q_ref - return Q - - -# Measurement model -R = 0.1**2 -range_models = [ - RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R), - RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R), -] - -# Setup -x0 = SE3State(SE3.Exp([0, 0, 0, 0, 0, 0]), stamp=0.0) -P0 = 0.1**2 * np.identity(6) -input_profile = lambda t, x: np.array( - [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] -) -process_model_true = BodyFrameVelocity -input_freq = 10 -dt = 1 / input_freq -t_max = dt * 100 -measurement_freq = 5 - -# The two models correspond to the BodyVelocityModel which uses two different Q matrices. -# The two different Q matrices are a Q_ref matrix which is scaled by a scalar, c. -imm_process_model_list = [ - BodyFrameVelocity(c_list[0] * Q_ref), - BodyFrameVelocity(c_list[1] * Q_ref), -] - - -class VaryingNoiseProcessModel(process_model_true): - def __init__(self, Q_profile): - self.Q_profile = Q_profile - super().__init__(Q_profile(0)) - - def covariance(self, x, u, dt) -> np.ndarray: - self._Q = self.Q_profile(x.stamp) - return super().covariance(x, u, dt) - - -N = 5 -Q_dg = np.eye(x0.value.shape[0]) -n_models = len(imm_process_model_list) - -# Kalman Filter bank -kf_list = [ExtendedKalmanFilter(pm) for pm in imm_process_model_list] - -# Set up probability transition matrix -off_diag_p = 0.02 -Pi = np.ones((n_models, n_models)) * off_diag_p -Pi = Pi + (1 - off_diag_p * (n_models)) * np.diag(np.ones(n_models)) -imm = InteractingModelFilter(kf_list, Pi) - - -dg = DataGenerator( - VaryingNoiseProcessModel(Q_profile), - input_profile, - Q_profile, - input_freq, - range_models, - measurement_freq, -) - - -def imm_trial(trial_number: int) -> List[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(randvec(P0)) - - estimate_list = run_interacting_multiple_model_filter( - imm, x0_check, P0, input_list, meas_list - ) - - results = [ - IMMResult(estimate_list[i], state_true[i]) for i in range(len(estimate_list)) - ] - - return IMMResultList(results) - - -def ekf_trial(trial_number: int) -> List[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 = x0.plus(randvec(P0)) - ekf = ExtendedKalmanFilter(VaryingNoiseProcessModel(Q_profile)) - - estimate_list = run_filter(ekf, x0_check, P0, input_list, meas_list) - results = [ - GaussianResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) - ] - - return GaussianResultList(results) - - -results = monte_carlo(imm_trial, N) -results_ekf = monte_carlo(ekf_trial, N) - -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_ekf.average_nees, label="EKF using GT Q 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() - - -if N < 15: - - fig, axs = plt.subplots(3, 2) - axs: List[plt.Axes] = axs - for result in results.trial_results: - plot_error(result, axs=axs) - - fig.suptitle("Estimation error IMM") - axs[1,0].set_xlabel("Time (s)") - - fig, axs = plt.subplots(3, 2) - axs: List[plt.Axes] = axs - for result in results_ekf.trial_results: - plot_error(result, axs=axs) - - fig.suptitle("Estimation error EKF GT") - axs[1,0].set_xlabel("Time (s)") - - average_model_probabilities = np.average( - np.array([t.model_probabilities for t in results.trial_results]), axis=0 - ) - fig, ax = plt.subplots(1, 1) - for lv1 in range(n_models): - ax.plot(results.stamp, average_model_probabilities[lv1, :]) - ax.set_xlabel("Time (s)") - ax.set_ylabel("Model Probabilities") - -fig, ax = plt.subplots(1, 1) -Q_ = np.zeros(results.stamp.shape) -for lv1 in range(n_models): - Q_ = Q_ + average_model_probabilities[lv1, :] * c_list[lv1] * Q_ref[0, 0] - -ax.plot(results.stamp, Q_, label=r"$Q_{00}$, Estimated") -ax.plot( - results.stamp, - np.array([Q_profile(t)[0, 0] for t in results.stamp]), - label=r"$Q_{00}$, GT", -) -ax.set_xlabel("Time (s)") -ax.set_ylabel(r"$Q_{00}$") - -plt.show() diff --git a/examples/ex_interacting_multiple_model_vector.py b/examples/ex_interacting_multiple_model_vector.py deleted file mode 100644 index d3510dc5..00000000 --- a/examples/ex_interacting_multiple_model_vector.py +++ /dev/null @@ -1,217 +0,0 @@ -import pytest -from navlie.filters import ExtendedKalmanFilter, run_filter -from navlie.lib.states import VectorState -from navlie.datagen import DataGenerator -from navlie.utils import GaussianResult, GaussianResultList -from navlie.utils import randvec - -from navlie.utils import monte_carlo, plot_error -from navlie.lib.models import DoubleIntegrator, OneDimensionalPositionVelocityRange - -import numpy as np -from typing import List -from matplotlib import pyplot as plt -from navlie.imm import InteractingModelFilter, run_interacting_multiple_model_filter -from navlie.imm import IMMResultList, IMMResult - - -"""This example runs an Interacting Multiple Model filter to estimate the process model noise matrix -for a state that is on a vector space. The performance is compared to an EKF that knows the ground -truth process model noise. -""" -# TODO. Remove monte carlo. this makes the example more complicated than it needs to be -# and is not necessary to demonstrate the IMM filter. - -# TODO. The IMM seems to have an issue when the user accidently modifies the -# provided state in the process model. - -# Measurement model -R = 0.1**4 -range_models = [OneDimensionalPositionVelocityRange(R)] - -# Process model noise setup -c_list = [1, 9] -Q_ref = np.eye(1) -input_freq = 10 -dt = 1 / input_freq -t_max = dt * 1000 - - -def Q_profile(t): - if t <= t_max / 4: - c = c_list[0] - if t > t_max / 4 and t <= t_max * 3 / 4: - c = c_list[1] - if t > t_max * 3 / 4: - c = c_list[0] - Q = c * Q_ref - return Q - - -# Setup -x0 = VectorState(np.array([1, 0]), 0.0) -P0 = np.diag([1, 1]) -input_profile = lambda t, x: np.array([np.sin(t)]) -process_model_true = DoubleIntegrator -measurement_freq = 5 - -# The two models correspond to the DoubleIntegrator which uses two different Q matrices. -# The two different Q matrices are a Q_ref matrix which is scaled by a scalar, c. -imm_process_model_list = [ - DoubleIntegrator(c_list[0] * Q_ref), - DoubleIntegrator(c_list[1] * Q_ref), -] - - -class VaryingNoiseProcessModel(process_model_true): - def __init__(self, Q_profile): - self.Q_profile = Q_profile - super().__init__(Q_profile(0)) - - def covariance(self, x, u, dt) -> np.ndarray: - self._Q = self.Q_profile(x.stamp) - return super().covariance(x, u, dt) - - -N = 5 -Q_dg = np.eye(x0.value.shape[0]) -n_inputs = input_profile(0, np.zeros(x0.value.shape[0])).shape[0] -n_models = len(imm_process_model_list) - -# Kalman Filter bank -kf_list = [ExtendedKalmanFilter(pm) for pm in imm_process_model_list] - -# Set up probability transition matrix -off_diag_p = 0.02 -Pi = np.ones((n_models, n_models)) * off_diag_p -Pi = Pi + (1 - off_diag_p * (n_models)) * np.diag(np.ones(n_models)) -imm = InteractingModelFilter(kf_list, Pi) - -dg = DataGenerator( - VaryingNoiseProcessModel(Q_profile), - input_profile, - Q_profile, - input_freq, - range_models, - measurement_freq, -) - - -def imm_trial(trial_number: int) -> List[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(randvec(P0)) - - estimate_list = run_interacting_multiple_model_filter( - imm, x0_check, P0, input_list, meas_list - ) - - results = [ - IMMResult(estimate_list[i], state_true[i]) for i in range(len(estimate_list)) - ] - - return IMMResultList(results) - - -def ekf_trial(trial_number: int) -> List[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 = x0.plus(randvec(P0)) - ekf = ExtendedKalmanFilter(VaryingNoiseProcessModel(Q_profile)) - - estimate_list = run_filter(ekf, x0_check, P0, input_list, meas_list) - results = [ - GaussianResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) - ] - - return GaussianResultList(results) - - -results = monte_carlo(imm_trial, N) -results_ekf = monte_carlo(ekf_trial, N) - -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_ekf.average_nees, label="EKF using GT Q 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() - - -if N < 15: - - fig, axs = plt.subplots(2, 1) - axs: List[plt.Axes] = axs - for result in results.trial_results: - plot_error(result, axs=axs) - - axs[0].set_title("Estimation error IMM") - axs[1].set_xlabel("Time (s)") - - fig, axs = plt.subplots(2, 1) - axs: List[plt.Axes] = axs - for result in results_ekf.trial_results: - plot_error(result, axs=axs) - - axs[0].set_title("Estimation error EKF GT") - axs[1].set_xlabel("Time (s)") - - average_model_probabilities = np.average( - np.array([t.model_probabilities for t in results.trial_results]), axis=0 - ) - fig, ax = plt.subplots(1, 1) - for lv1 in range(n_models): - ax.plot(results.stamp, average_model_probabilities[lv1, :]) - ax.set_xlabel("Time (s)") - ax.set_ylabel("Model Probabilities") - -fig, ax = plt.subplots(1, 1) -Q_ = np.zeros(results.stamp.shape) -for lv1 in range(n_models): - Q_ = Q_ + average_model_probabilities[lv1, :] * c_list[lv1] * Q_ref[0, 0] - -ax.plot(results.stamp, Q_, label=r"$Q_{00}$, Estimated") -ax.plot( - results.stamp, - np.array([Q_profile(t)[0, 0] for t in results.stamp]), - label=r"$Q_{00}$, GT", -) -ax.set_xlabel("Time (s)") -ax.set_ylabel(r"$Q_{00}$") - -plt.show() diff --git a/examples/ex_invariant_so3.py b/examples/ex_invariant_so3.py index bfaf2488..7fe196f3 100644 --- a/examples/ex_invariant_so3.py +++ b/examples/ex_invariant_so3.py @@ -1,80 +1,73 @@ -from navlie.lib.states import SO3State -from navlie.lib.models import ( +from navlie.lib import ( BodyFrameVelocity, InvariantMeasurement, Magnetometer, Gravitometer, + SO3State, ) -from navlie.datagen import DataGenerator -from navlie.filters import ExtendedKalmanFilter, run_filter -from navlie.utils import GaussianResult, GaussianResultList, plot_error, randvec +import navlie as nav from pylie import SO3 import numpy as np -import matplotlib.pyplot as plt -# ############################################################################## -# Problem Setup -# Define the initial state -x0 = SO3State(SO3.random(), 0.0, direction="left") -P0 = 0.3**2 * np.identity(3) -Q = 0.1**2 * np.identity(3) -noise_active = True +def main(): + # ########################################################################## + # Problem Setup -# Define the process model and measurement models. -process_model = BodyFrameVelocity(Q) -mag_model = Magnetometer(0.1**2 * np.identity(3)) -grav_model = Gravitometer(0.1**2 * np.identity(3)) + # Define the initial state + x0 = SO3State(SO3.random(), 0.0, direction="left") + P0 = 0.5**2 * np.identity(3) + Q = 0.1**2 * np.identity(3) + noise_active = True + # Define the process model and measurement models. + process_model = BodyFrameVelocity(Q) + mag_model = Magnetometer(0.1**2 * np.identity(3)) + grav_model = Gravitometer(0.1**2 * np.identity(3)) -# ############################################################################## -# Data generation + # ########################################################################## + # Data generation -dg = DataGenerator( - process_model, - lambda t, x: np.array([1, 2, 3]), - Q, - 100, - [mag_model, grav_model], - 1, -) -state_true, input_list, meas_list = dg.generate(x0, 0, 30, noise_active) + dg = nav.DataGenerator( + process_model, + lambda t, x: np.array([1, 2, 3]), + Q, + 100, + [mag_model, grav_model], + 1, + ) + state_true, input_list, meas_list = dg.generate(x0, 0, 30, noise_active) -if noise_active: - x0 = x0.plus(randvec(P0)) -# ############################################################################## -# Run the regular filter -x0.direction = "left" -ekf = ExtendedKalmanFilter(process_model=process_model) -estimate_list = run_filter(ekf, x0, P0, input_list, meas_list) -results = GaussianResultList( - [ - GaussianResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) - ] -) + if noise_active: + x0 = x0.plus(nav.randvec(P0)) -fig, axs = plot_error(results) -fig.suptitle("Regular EKF") + # ########################################################################## + # Run the regular filter + ekf = nav.ExtendedKalmanFilter(process_model=process_model) + estimate_list = nav.run_filter(ekf, x0, P0, input_list, meas_list) + results_ekf = nav.GaussianResultList.from_estimates(estimate_list, state_true) -# ############################################################################## -# Run the invariant filter -# TODO. Why does this give the exact same thing as the regular EKF? -# **************** Conversion to Invariant Measurements ! ********************* -invariants = [InvariantMeasurement(meas, "right") for meas in meas_list] -# ***************************************************************************** + # ########################################################################## + # Run the invariant filter + # TODO. Why does this give the exact same thing as the regular EKF? + # **************** Conversion to Invariant Measurements ! ****************** + invariants = [InvariantMeasurement(meas) for meas in meas_list] + # ************************************************************************** -x0.direction = "left" -ekf = ExtendedKalmanFilter(process_model=process_model) -estimate_list = run_filter(ekf, x0, P0, input_list, invariants) + ekf = nav.ExtendedKalmanFilter(process_model=process_model) + estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants) -results_invariant = GaussianResultList( - [ - GaussianResult(estimate_list[i], state_true[i]) - for i in range(len(estimate_list)) - ] -) + results_invariant = nav.GaussianResultList.from_estimates(estimate_list, state_true) + return results_ekf, results_invariant + + +if __name__ == "__main__": + results_ekf, results_invariant = main() + import matplotlib.pyplot as plt + + fig, axs = nav.plot_error(results_ekf) + fig.suptitle("Regular EKF") -fig, axs = plot_error(results_invariant) -fig.suptitle("Invariant EKF") -plt.show() + fig, axs = nav.plot_error(results_invariant) + fig.suptitle("Invariant EKF") + plt.show() diff --git a/examples/ex_iterated_ekf_se3.py b/examples/ex_iterated_ekf_se3.py index c458625f..152d7356 100644 --- a/examples/ex_iterated_ekf_se3.py +++ b/examples/ex_iterated_ekf_se3.py @@ -1,64 +1,63 @@ -from navlie.lib.datasets import SimulatedPoseRangingDataset -from navlie.filters import IteratedKalmanFilter -from navlie.utils import GaussianResult, GaussianResultList, plot_error, randvec -from navlie.types import StateWithCovariance +import navlie as nav import time import numpy as np np.random.seed(0) -# ############################################################################## -# Create simulated pose ranging data -data = SimulatedPoseRangingDataset() -gt_states = data.get_ground_truth() -input_data = data.get_input_data() -meas_data = data.get_meas_data() - -# %% ########################################################################### -# Perturb initial groundtruth state to initialize filter -P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 1, 1, 1]) -x0 = gt_states[0].plus(randvec(P0)) -x = StateWithCovariance(x0, P0) - -# Run Filter - try an EKF or an IterEKF -# ekf = ExtendedKalmanFilter(process_model) -ekf = IteratedKalmanFilter(data.process_model) - -meas_idx = 0 -start_time = time.time() -y = meas_data[meas_idx] -results_list = [] -for k in range(len(input_data) - 1): - results_list.append(GaussianResult(x, gt_states[k])) - - u = input_data[k] - - # Fuse any measurements that have occurred. - while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): - - x = ekf.correct(x, y, u) - meas_idx += 1 - if meas_idx < len(meas_data): - y = meas_data[meas_idx] - - dt = input_data[k + 1].stamp - x.stamp - x = ekf.predict(x, u, dt) - - - -print("Average filter computation frequency (Hz):") -print(1 / ((time.time() - start_time) / len(input_data))) - -results = GaussianResultList(results_list) - -# ############################################################################## -# Plot -import seaborn as sns -import matplotlib.pyplot as plt - -sns.set_theme() -fig, axs = plot_error(results) -axs[-1][0].set_xlabel("Time (s)") -axs[-1][1].set_xlabel("Time (s)") -axs[0][0].set_title("Rotation Error") -axs[0][1].set_title("Translation Error") -plt.show() +def main(): + # ############################################################################## + # Create simulated pose ranging data + data = nav.lib.SimulatedPoseRangingDataset() + gt_states = data.get_ground_truth() + input_data = data.get_input_data() + meas_data = data.get_meas_data() + + # %% ########################################################################### + # Perturb initial groundtruth state to initialize filter + P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 1, 1, 1]) + x0 = gt_states[0].plus(nav.randvec(P0)) + x = nav.StateWithCovariance(x0, P0) + + # Run Filter - try an EKF or an IterEKF + # ekf = ExtendedKalmanFilter(process_model) + ekf = nav.IteratedKalmanFilter(data.process_model) + + meas_idx = 0 + start_time = time.time() + y = meas_data[meas_idx] + results_list = [] + for k in range(len(input_data) - 1): + results_list.append(nav.GaussianResult(x, gt_states[k])) + + u = input_data[k] + + # Fuse any measurements that have occurred. + while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): + + x = ekf.correct(x, y, u) + meas_idx += 1 + if meas_idx < len(meas_data): + y = meas_data[meas_idx] + + dt = input_data[k + 1].stamp - x.stamp + x = ekf.predict(x, u, dt) + + + + print("Average filter computation frequency (Hz):") + print(1 / ((time.time() - start_time) / len(input_data))) + + results = nav.GaussianResultList(results_list) + return results + +if __name__ == "__main__": + results = main() + import seaborn as sns + import matplotlib.pyplot as plt + + sns.set_theme() + fig, axs = nav.plot_error(results) + axs[-1][0].set_xlabel("Time (s)") + axs[-1][1].set_xlabel("Time (s)") + axs[0][0].set_title("Rotation Error") + axs[0][1].set_title("Translation Error") + plt.show() diff --git a/examples/ex_monte_carlo.py b/examples/ex_monte_carlo.py index bd19338a..af354aeb 100644 --- a/examples/ex_monte_carlo.py +++ b/examples/ex_monte_carlo.py @@ -21,66 +21,71 @@ import numpy as np from typing import List -x0_true = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0) -P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 0.3**3, 0.3**2, 0.3**2]) -Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) -process_model = BodyFrameVelocity(Q) - -def input_profile(t, x): - return np.array( - [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] - ) - - -range_models = [ - RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], 0.1**2), - RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], 0.1**2), -] -dg = DataGenerator(process_model, input_profile, Q, 100, range_models, 10) - -ekf = ExtendedKalmanFilter(process_model) - -def ekf_trial(trial_number: int) -> List[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_data, meas_data = dg.generate(x0_true, 0, 10, noise=True) - x0_check = x0_true.plus(randvec(P0)) - estimates = run_filter(ekf, x0_check, P0, input_data, meas_data, True) - return GaussianResultList.from_estimates(estimates, state_true) - - -# %% Run the monte carlo experiment - -N = 100 - -results = monte_carlo(ekf_trial, N) - -# %% Plot -import matplotlib.pyplot as plt -fig, ax = plot_nees(results) - -if N < 15: - - fig, axs = plt.subplots(3, 2) - axs: List[plt.Axes] = axs - for result in results.trial_results: - plot_error(result, axs=axs) - - fig.suptitle("Estimation error") -plt.tight_layout() -plt.show() +def main(): + + x0_true = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0) + P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 0.3**3, 0.3**2, 0.3**2]) + Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + process_model = BodyFrameVelocity(Q) + + + def input_profile(t, x): + return np.array( + [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0] + ) + + + range_models = [ + RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], 0.1**2), + RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], 0.1**2), + ] + dg = DataGenerator(process_model, input_profile, Q, 100, range_models, 10) + + ekf = ExtendedKalmanFilter(process_model) + + def ekf_trial(trial_number: int) -> List[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_data, meas_data = dg.generate(x0_true, 0, 10, noise=True) + x0_check = x0_true.plus(randvec(P0)) + estimates = run_filter(ekf, x0_check, P0, input_data, meas_data, True) + return GaussianResultList.from_estimates(estimates, state_true) + + # Run the monte carlo experiment + N = 20 + results = monte_carlo(ekf_trial, num_trials=N, num_jobs=4) + return results + +if __name__ == "__main__": + results = main() + + # Plot + import matplotlib.pyplot as plt + fig, ax = plot_nees(results) + + if results.num_trials < 15: + + fig, axs = plt.subplots(3, 2) + axs: List[plt.Axes] = axs + for result in results.trial_results: + plot_error(result, axs=axs) + + fig.suptitle("Estimation error") + + plt.tight_layout() + plt.show() diff --git a/examples/ex_random_walk.py b/examples/ex_random_walk.py index cd228e66..44b4259e 100644 --- a/examples/ex_random_walk.py +++ b/examples/ex_random_walk.py @@ -1,94 +1,97 @@ -from navlie.filters import ExtendedKalmanFilter -from navlie.lib.states import VectorState -from navlie.datagen import DataGenerator -from navlie.types import StateWithCovariance, StampedValue -from navlie.utils import GaussianResult, GaussianResultList, randvec, plot_error -from navlie.lib.models import DoubleIntegrator, RangePointToAnchor +from navlie.lib import VectorState, DoubleIntegrator, RangePointToAnchor +import navlie as nav import numpy as np -# ############################################################################## -# Problem Setup - -x0 = VectorState(np.array([1, 0, 0, 0])) -P0 = 0.1**2*np.identity(x0.dof) -R = 0.1**2 -Q = 0.1 * np.identity(2) -range_models = [ - RangePointToAnchor([0, 4], R), - RangePointToAnchor([-2, 0], R), - RangePointToAnchor([2, 0], R), -] -range_freqs = [1, 1, 1] -process_model = DoubleIntegrator(Q) - -# VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV -# The TRUE input profile is zero-mean random signal. -input_profile = lambda t, x: randvec(Q).ravel() # random walk. - -# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -input_covariance = Q -input_freq = 100 - -dg = DataGenerator( - process_model, - input_profile, - input_covariance, - input_freq, - range_models, - range_freqs, -) - -# ############################################################################## -# Trial function - -gt_data, input_data, meas_data = dg.generate(x0, 0, 50, noise=True) - -x = x0.copy() -x.plus(randvec(P0)) -x = StateWithCovariance(x, P0) - -ekf = ExtendedKalmanFilter(process_model) - -meas_idx = 0 -y = meas_data[meas_idx] -results_list = [] -for k in range(len(input_data) - 1): - - # VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV - # The data generator will add noise on top of the already random signal if - # `input_covariance` is not zero. So here we remove this. - - u: StampedValue = input_data[k] - u.value = np.zeros(u.value.shape) # Zero-out the input - - # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - - # Fuse any measurements that have occurred. - while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): - - x = ekf.correct(x, y, u) - - # Load the next measurement - meas_idx += 1 - if meas_idx < len(meas_data): - y = meas_data[meas_idx] - - x = ekf.predict(x, u) - results_list.append(GaussianResult(x, gt_data[k])) - -results = GaussianResultList(results_list) - -import matplotlib.pyplot as plt -fig, axs = plt.subplots(2,2, sharex=True) -plot_error(results, axs) -axs[0,0].set_title("Position Error") -axs[0,1].set_title("Velocity Error") -axs[1,0].set_xlabel("Time (s)") -axs[1,1].set_xlabel("Time (s)") -axs[0,0].set_ylabel("x (m)") -axs[1,0].set_ylabel("y (m)") -axs[0,1].set_ylabel("x (m/s)") -axs[1,1].set_ylabel("y (m/s)") -plt.tight_layout() -plt.show() + +def main(): + # ############################################################################## + # Problem Setup + + x0 = VectorState(np.array([1, 0, 0, 0])) + P0 = 0.1**2*np.identity(x0.dof) + R = 0.1**2 + Q = 0.1 * np.identity(2) + range_models = [ + RangePointToAnchor([0, 4], R), + RangePointToAnchor([-2, 0], R), + RangePointToAnchor([2, 0], R), + ] + range_freqs = [1, 1, 1] + process_model = DoubleIntegrator(Q) + + # VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV + # The TRUE input profile is zero-mean random signal. + input_profile = lambda t, x: nav.randvec(Q).ravel() # random walk. + + # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + input_covariance = Q + input_freq = 100 + + dg = nav.DataGenerator( + process_model, + input_profile, + input_covariance, + input_freq, + range_models, + range_freqs, + ) + + # ############################################################################## + # Trial function + + gt_data, input_data, meas_data = dg.generate(x0, 0, 50, noise=True) + + x = x0.copy() + x.plus(nav.randvec(P0)) + x = nav.StateWithCovariance(x, P0) + + ekf = nav.ExtendedKalmanFilter(process_model) + + meas_idx = 0 + y = meas_data[meas_idx] + results_list = [] + for k in range(len(input_data) - 1): + + # VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV + # The data generator will add noise on top of the already random signal if + # `input_covariance` is not zero. So here we remove this. + + u: nav.StampedValue = input_data[k] + u.value = np.zeros(u.value.shape) # Zero-out the input + + # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + # Fuse any measurements that have occurred. + while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): + + x = ekf.correct(x, y, u) + + # Load the next measurement + meas_idx += 1 + if meas_idx < len(meas_data): + y = meas_data[meas_idx] + + x = ekf.predict(x, u) + results_list.append(nav.GaussianResult(x, gt_data[k])) + + results = nav.GaussianResultList(results_list) + + return results + +if __name__ == "__main__": + results = main() + + import matplotlib.pyplot as plt + fig, axs = plt.subplots(2,2, sharex=True) + nav.plot_error(results, axs) + axs[0,0].set_title("Position Error") + axs[0,1].set_title("Velocity Error") + axs[1,0].set_xlabel("Time (s)") + axs[1,1].set_xlabel("Time (s)") + axs[0,0].set_ylabel("x (m)") + axs[1,0].set_ylabel("y (m)") + axs[0,1].set_ylabel("x (m/s)") + axs[1,1].set_ylabel("y (m/s)") + plt.tight_layout() + plt.show() diff --git a/examples/ex_sequential_measurements.py b/examples/ex_sequential_measurements.py index 4ba12e58..ee5ca92b 100644 --- a/examples/ex_sequential_measurements.py +++ b/examples/ex_sequential_measurements.py @@ -1,11 +1,3 @@ -# %% -from navlie.lib.states import VectorState -from navlie.datagen import DataGenerator -from navlie.utils import schedule_sequential_measurements -from navlie.lib.models import SingleIntegrator, RangePointToAnchor -import numpy as np -import matplotlib.pyplot as plt - """ This is an example script showing how to generate measurements using multiple measurement models that are supposed to run in sequence rather than in parallel. @@ -14,55 +6,68 @@ MeasurementModels to generate a measurement to one anchor at every time instance. """ -# ############################################################################## -# Problem Setup +from navlie.lib.states import VectorState +from navlie.datagen import DataGenerator +from navlie.utils import schedule_sequential_measurements +from navlie.lib.models import SingleIntegrator, RangePointToAnchor +import numpy as np +import matplotlib.pyplot as plt + +def main(): + + # ############################################################################## + # Problem Setup + + x0 = VectorState(np.array([1, 0]), stamp=0) + P0 = np.diag([1, 1]) + R = 0.1**2 + Q = 0.1 * np.identity(2) + range_models = [ + RangePointToAnchor([0, 4], R), # Anchor 1 + RangePointToAnchor([-2, 0], R), # Anchor 2 + RangePointToAnchor([2, 0], R), # Anchor 3 + ] + range_freqs = 100 # This defines the overall frequency in which we want the + # frequencies of the 3 measurement models to sum up to. + process_model = SingleIntegrator(Q) + input_profile = lambda t, x: np.array([0, 0]) + input_covariance = Q + input_freq = 180 + noise_active = True -x0 = VectorState(np.array([1, 0]), stamp=0) -P0 = np.diag([1, 1]) -R = 0.1**2 -Q = 0.1 * np.identity(2) -range_models = [ - RangePointToAnchor([0, 4], R), # Anchor 1 - RangePointToAnchor([-2, 0], R), # Anchor 2 - RangePointToAnchor([2, 0], R), # Anchor 3 -] -range_freqs = 100 # This defines the overall frequency in which we want the - # frequencies of the 3 measurement models to sum up to. -process_model = SingleIntegrator(Q) -input_profile = lambda t, x: np.array([0, 0]) -input_covariance = Q -input_freq = 180 -noise_active = True + # ############################################################################## + # Schedule sequential measurements -# ############################################################################## -# Schedule sequential measurements + range_offset, sequential_freq = schedule_sequential_measurements( + range_models, range_freqs + ) -range_offset, sequential_freq = schedule_sequential_measurements( - range_models, range_freqs -) + # ############################################################################## + # Data Generation -# ############################################################################## -# Data Generation + dg = DataGenerator( + process_model, + input_profile, + input_covariance, + input_freq, + range_models, + sequential_freq, # reduced frequency of each individual MeasurementModel + range_offset, # each measurement is offset so they do not start at the same time + ) -dg = DataGenerator( - process_model, - input_profile, - input_covariance, - input_freq, - range_models, - sequential_freq, # reduced frequency of each individual MeasurementModel - range_offset, # each measurement is offset so they do not start at the same time -) + _, _, meas_data = dg.generate(x0, 0, 0.1) -_, _, meas_data = dg.generate(x0, 0, 0.1) + print("The effective frequency of every measurements to every Anchor is " \ + + str(sequential_freq) + " Hz.") -print("The effective frequency of every measurements to every Anchor is " \ - + str(sequential_freq) + " Hz.") + return meas_data -# We can then see in the plot that the measurement to each anchor is being generated -# sequentially with no overlap. -plt.scatter([x.stamp for x in meas_data], [x.value for x in meas_data]) -plt.grid() -plt.ylabel(r"Measurement Value [m]") -plt.xlabel(r"Time [s]") -# %% +if __name__ == "__main__": + meas_data = main() + # We can then see in the plot that the measurement to each anchor is being generated + # sequentially with no overlap. + plt.scatter([x.stamp for x in meas_data], [x.value for x in meas_data]) + plt.grid() + plt.ylabel(r"Measurement Value [m]") + plt.xlabel(r"Time [s]") + plt.show() \ No newline at end of file diff --git a/examples/ex_ukf_se2.py b/examples/ex_ukf_se2.py index fafc799e..bd8e8b10 100644 --- a/examples/ex_ukf_se2.py +++ b/examples/ex_ukf_se2.py @@ -1,43 +1,49 @@ from navlie.lib import SE3State, BodyFrameVelocity, SimulatedPoseRangingDataset -from navlie import SigmaPointKalmanFilter, run_filter, GaussianResultList, plot_error, randvec +from navlie import SigmaPointKalmanFilter, run_filter, GaussianResultList, plot_error, randvec, UnscentedKalmanFilter import time import numpy as np np.random.seed(0) -# ############################################################################## -# Problem Setup -x0 = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0, direction="right") -P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 1, 1, 1]) -Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) -noise_active = True -process_model = BodyFrameVelocity(Q) - - -data = SimulatedPoseRangingDataset(x0=x0, Q=Q, noise_active=noise_active) -state_true = data.get_ground_truth() -input_data = data.get_input_data() -meas_data = data.get_meas_data() -if noise_active: - x0 = x0.plus(randvec(P0)) -# %% ########################################################################### -# Run Filter - -ukf = SigmaPointKalmanFilter(process_model, method = 'unscented', iterate_mean=False) - -start_time = time.time() -estimates = run_filter(ukf, x0, P0, input_data, meas_data) -print("Average filter computation frequency (Hz):") -print(1 / ((time.time() - start_time) / len(input_data))) - -results = GaussianResultList.from_estimates(estimates, state_true) - -# ############################################################################## -# Plot -import matplotlib.pyplot as plt -fig, axs = plot_error(results) -axs[-1][0].set_xlabel("Time (s)") -axs[-1][1].set_xlabel("Time (s)") -axs[0][0].set_title("Rotation Error") -axs[0][1].set_title("Translation Error") -plt.show() +def main(): + # ########################################################################## + # Problem Setup + x0 = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0, direction="right") + P0 = np.diag([0.1**2, 0.1**2, 0.1**2, 1, 1, 1]) + Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + noise_active = True + process_model = BodyFrameVelocity(Q) + + + data = SimulatedPoseRangingDataset(x0=x0, Q=Q, noise_active=noise_active) + state_true = data.get_ground_truth() + input_data = data.get_input_data() + meas_data = data.get_meas_data() + if noise_active: + x0 = x0.plus(randvec(P0)) + # %% ####################################################################### + # Run Filter + + ukf = SigmaPointKalmanFilter(process_model, method = 'unscented', iterate_mean=False) + # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! + + start_time = time.time() + estimates = run_filter(ukf, x0, P0, input_data, meas_data) + print("Average filter computation frequency (Hz):") + print(1 / ((time.time() - start_time) / len(input_data))) + + results = GaussianResultList.from_estimates(estimates, state_true) + return results + +if __name__ == "__main__": + + results = main() + # ########################################################################## + # Plot + import matplotlib.pyplot as plt + fig, axs = plot_error(results) + axs[-1][0].set_xlabel("Time (s)") + axs[-1][1].set_xlabel("Time (s)") + axs[0][0].set_title("Rotation Error") + axs[0][1].set_title("Translation Error") + plt.show() diff --git a/examples/ex_ukf_vector.py b/examples/ex_ukf_vector.py index 2a33f34c..7d4796d8 100644 --- a/examples/ex_ukf_vector.py +++ b/examples/ex_ukf_vector.py @@ -9,68 +9,69 @@ measurement model, generate data using those models, and then run an Sigma Point Kalman Filter on that data. """ +def main(): + # ############################################################################## + # Problem Setup -# ############################################################################## -# Problem Setup + x0 = VectorState(np.array([1, 0]), stamp=0) + P0 = np.diag([1, 1]) + R = 0.1**2 + Q = 0.1 * np.identity(2) + range_models = [ + RangePointToAnchor([0, 4], R), + RangePointToAnchor([-2, 0], R), + RangePointToAnchor([2, 0], R), + ] + range_freqs = [50, 50, 50] + process_model = SingleIntegrator(Q) + input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) + input_covariance = Q + input_freq = 180 + noise_active = True + # ############################################################################## + # Data Generation -x0 = VectorState(np.array([1, 0]), stamp=0) -P0 = np.diag([1, 1]) -R = 0.1**2 -Q = 0.1 * np.identity(2) -range_models = [ - RangePointToAnchor([0, 4], R), - RangePointToAnchor([-2, 0], R), - RangePointToAnchor([2, 0], R), -] -range_freqs = [50, 50, 50] -process_model = SingleIntegrator(Q) -input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)]) -input_covariance = Q -input_freq = 180 -noise_active = True -# ############################################################################## -# Data Generation + dg = DataGenerator( + process_model, + input_profile, + input_covariance, + input_freq, + range_models, + range_freqs, + ) -dg = DataGenerator( - process_model, - input_profile, - input_covariance, - input_freq, - range_models, - range_freqs, -) + gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) -gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active) + # ############################################################################## + # Run Filter + if noise_active: + x0 = x0.plus(randvec(P0)) + -# ############################################################################## -# Run Filter -if noise_active: - x0 = x0.plus(randvec(P0)) - + ukf = SigmaPointKalmanFilter(process_model, method= 'cubature', iterate_mean=False) + # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! -ukf = SigmaPointKalmanFilter(process_model, method= 'cubature', iterate_mean=False) + start_time = time.time() + estimates = run_filter(ukf, x0, P0, input_data, meas_data) + print("Average filter computation frequency (Hz):") + print(1 / ((time.time() - start_time) / len(input_data))) + results = GaussianResultList.from_estimates(estimates, gt_data) + return results -start_time = time.time() -estimates = run_filter(ukf, x0, P0, input_data, meas_data) -print("Average filter computation frequency (Hz):") -print(1 / ((time.time() - start_time) / len(input_data))) -results = GaussianResultList.from_estimates(estimates, gt_data) +if __name__ == "__main__": + results = main() + import matplotlib.pyplot as plt + fig, ax = plt.subplots(1, 1) + ax.plot(results.value[:, 0], results.value[:, 1], label="Estimate") + ax.plot( + results.value_true[:, 0], results.value_true[:, 1], label="Ground truth" + ) + ax.set_title("Trajectory") + ax.set_xlabel("x (m)") + ax.set_ylabel("y (m)") + ax.legend() -# ############################################################################## -# Post processing - -import matplotlib.pyplot as plt -fig, ax = plt.subplots(1, 1) -ax.plot(results.value[:, 0], results.value[:, 1], label="Estimate") -ax.plot( - results.value_true[:, 0], results.value_true[:, 1], label="Ground truth" -) -ax.set_title("Trajectory") -ax.set_xlabel("x (m)") -ax.set_ylabel("y (m)") -ax.legend() - -fig, axs = plot_error(results) -fig.suptitle("Estimation Error") -plt.show() + fig, axs = plot_error(results) + fig.suptitle("Estimation Error") + plt.show() diff --git a/examples/ex_varying_noise.py b/examples/ex_varying_noise.py index a957163d..61d8e226 100644 --- a/examples/ex_varying_noise.py +++ b/examples/ex_varying_noise.py @@ -16,99 +16,101 @@ ) import numpy as np from typing import List -from matplotlib import pyplot as plt -import seaborn as sns -sns.set_theme() -sns.set_style("whitegrid") - """ This is an example script showing how to define time-varying noise matrices and to then run an EKF using these same noise matrices. """ - -x0_true = VectorState(np.array([1, 0]), stamp=0.0) -P0 = np.diag([0.5, 0.5]) -R = 0.01**2 -Q = 0.1 * np.identity(1) -N = 10 # Number MC trials - -range_models = [ - RangePointToAnchor([0.0], R), -] - - -t_max = 10 -range_freqs = [10] -input_freq = 10 -dt = 1/input_freq - -def Q_profile(t): - if t <= t_max/4: - Q = 1 - if t > t_max/4 and t <= t_max*3/4: - Q = 100 - if t > t_max*3/4: - Q = 1 - Q = np.array(Q).reshape((1,1)) - return Q - -class VaryingNoiseDoubleIntegrator(DoubleIntegrator): - def __init__(self, Q_profile): - self.Q_profile = Q_profile - super().__init__(Q_profile(0)) - - def covariance(self, x, u, dt) -> np.ndarray: - self._Q = self.Q_profile(x.stamp) - return super().covariance(x, u, dt) - -# For data generation, the Q for the process model does not matter as -# only the evaluate method is used. -process_model = VaryingNoiseDoubleIntegrator(Q_profile) -input_profile = lambda t, x: np.sin(t) - -dg = DataGenerator( - process_model, - input_profile, - Q_profile, - input_freq, - range_models, - range_freqs -) - - -ekf = ExtendedKalmanFilter(process_model) - -def ekf_trial(trial_number:int) -> List[GaussianResult]: - """ - A single trial in a monte carlo experiment. This function accepts the trial - number and must return a list of GaussianResult objects. - """ - - np.random.seed(trial_number) - state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=True) - - x0_check = x0_true.plus(randvec(P0)) - - estimates = run_filter(ekf, x0_check, P0, input_data, meas_data, True) - - return GaussianResultList.from_estimates(estimates, state_true) - -# %% Run the monte carlo experiment -results = monte_carlo(ekf_trial, N) - +def main(): + x0_true = VectorState(np.array([1, 0]), stamp=0.0) + P0 = np.diag([0.5, 0.5]) + R = 0.01**2 + Q = 0.1 * np.identity(1) + N = 10 # Number MC trials + + range_models = [ + RangePointToAnchor([0.0], R), + ] + + + t_max = 10 + range_freqs = [10] + input_freq = 10 + dt = 1/input_freq + + def Q_profile(t): + if t <= t_max/4: + Q = 1 + if t > t_max/4 and t <= t_max*3/4: + Q = 100 + if t > t_max*3/4: + Q = 1 + Q = np.array(Q).reshape((1,1)) + return Q + + class VaryingNoiseDoubleIntegrator(DoubleIntegrator): + def __init__(self, Q_profile): + self.Q_profile = Q_profile + super().__init__(Q_profile(0)) + + def covariance(self, x, u, dt) -> np.ndarray: + self._Q = self.Q_profile(x.stamp) + return super().covariance(x, u, dt) + + # For data generation, the Q for the process model does not matter as + # only the evaluate method is used. + process_model = VaryingNoiseDoubleIntegrator(Q_profile) + input_profile = lambda t, x: np.sin(t) + + dg = DataGenerator( + process_model, + input_profile, + Q_profile, + input_freq, + range_models, + range_freqs + ) + + + ekf = ExtendedKalmanFilter(process_model) + + def ekf_trial(trial_number:int) -> List[GaussianResult]: + """ + A single trial in a monte carlo experiment. This function accepts the trial + number and must return a list of GaussianResult objects. + """ + + np.random.seed(trial_number) + state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=True) + + x0_check = x0_true.plus(randvec(P0)) + + estimates = run_filter(ekf, x0_check, P0, input_data, meas_data, True) + + return GaussianResultList.from_estimates(estimates, state_true) + + # %% Run the monte carlo experiment + results = monte_carlo(ekf_trial, num_trials=N) + + return results # %% Plot +if __name__ == "__main__": + results = main() + from matplotlib import pyplot as plt + import seaborn as sns + sns.set_theme() + sns.set_style("whitegrid") -fig, ax = plot_nees(results) + fig, ax = plot_nees(results) -if N < 15: + if results.num_trials < 15: - fig, axs = plt.subplots(2, 1) - axs: List[plt.Axes] = axs - for result in results.trial_results: - plot_error(result, axs = axs) + fig, axs = plt.subplots(2, 1) + axs: List[plt.Axes] = axs + for result in results.trial_results: + plot_error(result, axs = axs) - axs[0].set_title("Estimation error") - axs[1].set_xlabel("Time (s)") + axs[0].set_title("Estimation error") + axs[1].set_xlabel("Time (s)") -plt.show() \ No newline at end of file + plt.show() \ No newline at end of file diff --git a/navlie/__init__.py b/navlie/__init__.py index 1512270a..cec78541 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -12,9 +12,13 @@ ExtendedKalmanFilter, IteratedKalmanFilter, SigmaPointKalmanFilter, + UnscentedKalmanFilter, + CubatureKalmanFilter, + GaussHermiteKalmanFilter, run_filter, ) from . import batch +from . import imm from . import lib from .batch import BatchEstimator diff --git a/navlie/datagen.py b/navlie/datagen.py index 67e650cd..577b610c 100644 --- a/navlie/datagen.py +++ b/navlie/datagen.py @@ -60,6 +60,9 @@ def __init__( else: raise ValueError("Input covariance must be a function or a matrix.") + if isinstance(meas_model_list, MeasurementModel): + meas_model_list = [meas_model_list] + # Check meas frequencies were provided if (len(meas_model_list) != 0) and (meas_freq_list is None): raise ValueError("Measurement frequency must be provided.") diff --git a/navlie/imm.py b/navlie/imm.py index 1800a26d..cfea0ad4 100644 --- a/navlie/imm.py +++ b/navlie/imm.py @@ -8,14 +8,19 @@ ) import numpy as np from scipy.stats import multivariate_normal -from navlie.utils import GaussianResultList, GaussianResult +from navlie.utils import GaussianResultList, GaussianResult, state_interp from navlie.filters import ExtendedKalmanFilter +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( weights: List[float], means: List[np.ndarray], covariances: List[np.ndarray] ): - """Calculate the mean and covariance of a Gaussian mixture on a vectorspace. + """ + Calculate the mean and covariance of a Gaussian mixture on a vectorspace. Parameters ---------- @@ -37,10 +42,10 @@ def gaussian_mixing_vectorspace( x_bar = np.zeros(means[0].shape) P_bar = np.zeros(covariances[0].shape) - for (weight, x) in zip(weights, means): + for weight, x in zip(weights, means): x_bar = x_bar + weight * x - for (weight, x, P) in zip(weights, means, covariances): + for weight, x, P in zip(weights, means, covariances): dx = (x - x_bar).reshape(-1, 1) P_bar = P_bar + weight * P + weight * dx @ dx.T @@ -50,7 +55,8 @@ def gaussian_mixing_vectorspace( def reparametrize_gaussians_about_X_par( X_par: State, X_list: List[StateWithCovariance] ): - """Reparametrize each Lie group Gaussian in X_list about X_par. + """ + Reparametrize each Lie group Gaussian in X_list about X_par. A Lie group Gaussian is only relevant in the tangent space of its own mean. To mix Lie group Gaussians, a specific X, X_par, must be chosen to expand a tangent space around. Once expanded in this common tangent space, @@ -85,9 +91,10 @@ def reparametrize_gaussians_about_X_par( def update_X(X: State, mu: np.ndarray, P: np.ndarray): - """Given a Lie group Gaussian with mean mu and covariance P, expressed in the tangent space of X, - compute Lie group StateAndCovariance X_hat such that the Lie algebra Gaussian - around X_hat has zero mean. + """ + Given a Lie group Gaussian with mean mu and covariance P, expressed in the + tangent space of X, compute Lie group StateAndCovariance X_hat such that the + Lie algebra Gaussian around X_hat has zero mean. Parameters ---------- @@ -147,10 +154,13 @@ def __init__( self.model_states = model_states self.model_probabilities = model_probabilities + @property + def stamp(self): + return self.model_states[0].state.stamp + def copy(self) -> "IMMState": - return IMMState( - self.model_states.copy(), self.model_probabilities.copy() - ) + x_copy = [x.copy() for x in self.model_states] + return IMMState(x_copy, self.model_probabilities.copy()) class IMMResult(GaussianResult): @@ -197,15 +207,44 @@ class IMMResultList(GaussianResultList): def __init__(self, result_list: List[IMMResult]): super().__init__(result_list) - self.model_probabilities = [] - # Turn list of "probability at time step" into - # list of "probability of model" - n_models = result_list[0].model_probabilities.shape[0] - for lv1 in range(n_models): - self.model_probabilities.append( - np.array([r.model_probabilities[lv1] for r in result_list]) - ) + self.model_probabilities = np.array( + [r.model_probabilities for r in result_list] + ) + @staticmethod + def from_estimates( + estimate_list: List[IMMState], + 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 + + Parameters + ---------- + estimate_list : List[IMMState] + A list of IMMState objects + state_true_list : List[State] + A list of true State objects + method : "nearest" or "linear", optional + The method used to interpolate the true state when the timestamps + do not line up exactly, by default "nearest". + + Returns + ------- + IMMResultList + A IMMResultList 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( + [ + IMMResult(estimate, state_true) + for estimate, state_true in zip(estimate_list, state_true_list) + ] + ) class InteractingModelFilter: """On-manifold Interacting Multiple Model Filter (IMM). @@ -283,7 +322,8 @@ def interaction( return IMMState(x_mix, mu_models) def predict(self, x_km: IMMState, u: Input, dt: float): - """Carries out prediction step for each model of the IMM. + """ + Carries out prediction step for each model of the IMM. Parameters ---------- @@ -298,6 +338,7 @@ def predict(self, x_km: IMMState, u: Input, dt: float): ------- IMMState """ + x_km_models = x_km.model_states.copy() x_check = [] for lv1, kf in enumerate(self.kf_list): @@ -310,18 +351,19 @@ def correct( y: Measurement, u: Input, ): - """Carry out the correction step for each model and update model probabilities. + """ + Carry out the correction step for each model and update model + probabilities. Parameters ---------- - x_check: IMMState - mu_km_models : List[Float] + x_check: IMMState mu_km_models : List[Float] Probabilities for each model from previous timestep. y : Measurement Measurement to be fused into the current state estimate. u: Input - Most recent input, to be used to predict the state forward - if the measurement stamp is larger than the state stamp. + Most recent input, to be used to predict the state forward if the + measurement stamp is larger than the state stamp. Returns @@ -343,16 +385,12 @@ def correct( # Correct and update model probabilities x_hat = [] for lv1, kf in enumerate(self.kf_list): - x, details_dict = kf.correct( - x_models_check[lv1], y, u, output_details=True - ) + x, details_dict = kf.correct(x_models_check[lv1], y, u, output_details=True) x_hat.append(x) z = details_dict["z"] S = details_dict["S"] z = z.ravel() - model_likelihood = multivariate_normal.pdf( - z, mean=np.zeros(z.shape), cov=S - ) + model_likelihood = multivariate_normal.pdf(z, mean=np.zeros(z.shape), cov=S) mu_k[lv1] = model_likelihood * c_bar[lv1] # If all model likelihoods are zero to machine tolerance, np.sum(mu_k)=0 and it fails @@ -365,7 +403,7 @@ def correct( return IMMState(x_hat, mu_k) -def run_interacting_multiple_model_filter( +def run_imm_filter( filter: InteractingModelFilter, x0: State, P0: np.ndarray, @@ -422,15 +460,12 @@ def run_interacting_multiple_model_filter( [StateWithCovariance(x0, P0)] * n_models, 1.0 / n_models * np.array(np.ones(n_models)), ) - for k in range(len(input_data) - 1): + for k in tqdm(range(len(input_data) - 1)): results_list.append(x) 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 - ): - + while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): x = filter.interaction(x) x = filter.correct(x, y, u) meas_idx += 1 diff --git a/navlie/lib/__init__.py b/navlie/lib/__init__.py index fb830ec3..c2917832 100644 --- a/navlie/lib/__init__.py +++ b/navlie/lib/__init__.py @@ -28,7 +28,8 @@ Gravitometer, Magnetometer, GlobalPosition, - InvariantMeasurement + InvariantMeasurement, + PointRelativePosition ) from .preintegration import ( diff --git a/navlie/lib/models.py b/navlie/lib/models.py index 5ca69535..8e3bc83a 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -592,6 +592,10 @@ def covariance(self, x: VectorState) -> np.ndarray: class PointRelativePosition(MeasurementModel): + """ + Measurement model describing the position of a known landmark relative + to the robot, resolved in the body frame. + """ def __init__( self, landmark_position: np.ndarray, @@ -633,7 +637,8 @@ def __init__(self, y: np.ndarray, model: PointRelativePosition): self.measurement_model = model def evaluate(self, x: MatrixLieGroupState) -> np.ndarray: - """Computes the right-invariant innovation. + """ + Computes the right-invariant innovation. Parameters @@ -653,7 +658,8 @@ def evaluate(self, x: MatrixLieGroupState) -> np.ndarray: return z def jacobian(self, x: MatrixLieGroupState) -> np.ndarray: - """Compute the Jacobian of the innovation directly. + """ + Compute the Jacobian of the innovation directly. Parameters ---------- diff --git a/navlie/utils.py b/navlie/utils.py index cf918ae2..a361b082 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -393,7 +393,7 @@ def nees_upper_bound(self, confidence_interval: float, double_sided=True): def monte_carlo( trial: Callable[[int], GaussianResultList], num_trials: int, - n_jobs: int = -1, + num_jobs: int = -1, verbose: int = 10, ) -> MonteCarloResult: """ @@ -409,11 +409,11 @@ def monte_carlo( are expected to remain consistent. num_trials : int Number of Trials to execute - n_jobs: int, optional + num_jobs: int, optional The maximum number of concurrently running jobs, by default -1. If -1 all CPUs are used. If 1 is given, no parallel computing code - is used at all, which is useful for debugging. For n_jobs below -1, - (n_cpus + 1 + n_jobs) are used. Thus for n_jobs = -2, all CPUs but + is used at all, which is useful for debugging. For num_jobs below -1, + (n_cpus + 1 + num_jobs) are used. Thus for num_jobs = -2, all CPUs but one are used. verbose: int, optional The verbosity level, by default 10. If non zero, progress messages @@ -429,7 +429,7 @@ def monte_carlo( trial_results = [None] * num_trials print("Starting Monte Carlo experiment...") - trial_results = Parallel(n_jobs=n_jobs, verbose=verbose)( + trial_results = Parallel(n_jobs=num_jobs, verbose=verbose)( delayed(trial)(i) for i in range(num_trials) ) @@ -988,7 +988,7 @@ def state_interp( query_stamps = query_stamps.copy() for i, stamp in enumerate(query_stamps): - if not isinstance(stamp, float): + if not isinstance(stamp, (float, int)): if hasattr(stamp, "stamp"): stamp = stamp.stamp query_stamps[i] = stamp diff --git a/tests/integration/test_interacting_multiple_models.py b/tests/integration/test_interacting_multiple_models.py index dbcc5ad2..c4c99b5b 100644 --- a/tests/integration/test_interacting_multiple_models.py +++ b/tests/integration/test_interacting_multiple_models.py @@ -18,7 +18,7 @@ import numpy as np from typing import List -from navlie.imm import InteractingModelFilter, run_interacting_multiple_model_filter +from navlie.imm import InteractingModelFilter, run_imm_filter from navlie.imm import IMMResultList from navlie.imm import IMMResult # TODO this test is very complicated. we need to simplify this. @@ -71,7 +71,7 @@ def imm_trial(trial_number: int) -> List[GaussianResult]: x0_check = x0_true.copy() x0_check = x0_check.plus(randvec(P0)) - estimate_list = run_interacting_multiple_model_filter( + estimate_list = run_imm_filter( imm, x0_check, P0, input_list, meas_list ) diff --git a/tests/test_examples.py b/tests/test_examples.py new file mode 100644 index 00000000..642dcfda --- /dev/null +++ b/tests/test_examples.py @@ -0,0 +1,83 @@ +import os +import sys + +# Add the examples folder to the path +sys.path.append(os.path.join(os.path.dirname(__file__), '..', 'examples')) + +""" +examples/ex_batch_se3.py +examples/ex_batch_vector.py +examples/ex_ekf_vector.py +examples/ex_inertial_gps.py +examples/ex_inertial_nav.py +examples/ex_interacting_multiple_model_se3.py +examples/ex_interacting_multiple_model_vector.py +examples/ex_invariant_so3.py +examples/ex_iterated_ekf_se3.py +examples/ex_monte_carlo.py +examples/ex_random_walk.py +examples/ex_sequential_measurements.py +examples/ex_ukf_se2.py +examples/ex_ukf_vector.py +examples/ex_varying_noise.py +""" + +def test_ex_batch_se3(): + from ex_batch_se3 import main + main() + +def test_ex_batch_vector(): + from ex_batch_vector import main + main() + +def test_ex_ekf_vector(): + from ex_ekf_vector import main + main() + +def test_ex_inertial_gps(): + from ex_inertial_gps import main + main() + +def test_ex_inertial_nav(): + import ex_inertial_nav + +# def test_ex_imm_se3(): +# import ex_imm_se3 + +def test_ex_imm_vector(): + from ex_imm_vector import main + main() + +def test_ex_invariant_so3(): + from ex_invariant_so3 import main + main() + +def test_ex_iterated_ekf_se3(): + from ex_iterated_ekf_se3 import main + main() + +def test_ex_monte_carlo(): + from ex_monte_carlo import main + main() + +def test_ex_random_walk(): + from ex_random_walk import main + main() + +def test_ex_sequential_measurements(): + from ex_sequential_measurements import main + main() + +def test_ex_ukf_se2(): + from ex_ukf_se2 import main + main() + +def test_ex_ukf_vector(): + from ex_ukf_vector import main + main() + +def test_ex_varying_noise(): + from ex_varying_noise import main + main() + + \ No newline at end of file From 2cf6169c71fd8f09c2c30c8f1f97a35ddac6f836 Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Fri, 28 Jul 2023 15:44:12 -0400 Subject: [PATCH 2/2] Change all `pylie` to `pymlg` --- README.rst | 4 ++-- examples/ex_inertial_nav.py | 2 +- examples/ex_invariant_so3.py | 2 +- navlie/lib/datasets.py | 2 +- navlie/lib/imu.py | 2 +- navlie/lib/models.py | 2 +- navlie/lib/preintegration.py | 2 +- navlie/lib/states.py | 6 +++--- setup.py | 2 +- tests/integration/test_batch_noiseless.py | 2 +- tests/integration/test_filter_ekf_basic.py | 2 +- tests/integration/test_filter_ekf_noiseless.py | 2 +- tests/integration/test_interacting_multiple_models.py | 2 +- tests/integration/test_iterated_ekf.py | 2 +- tests/unit/test_batch_residuals.py | 2 +- tests/unit/test_body_velocity.py | 2 +- tests/unit/test_camera.py | 2 +- tests/unit/test_composite.py | 2 +- tests/unit/test_imu.py | 2 +- tests/unit/test_meas_models.py | 2 +- tests/unit/test_preintegration.py | 2 +- tests/unit/test_spkf.py | 2 +- tests/unit/test_states.py | 2 +- tests/unit/test_utils.py | 2 +- 24 files changed, 27 insertions(+), 27 deletions(-) diff --git a/README.rst b/README.rst index 944e5df5..2c304530 100644 --- a/README.rst +++ b/README.rst @@ -33,7 +33,7 @@ Clone this repo, change to its directory, and execute pip install -e . -This command should automatically install all dependencies, including our package `pylie` (found at https://github.com/decargroup/pylie) for back-end Lie group mathematical operations. +This command should automatically install all dependencies, including our package `pymlg` (found at https://github.com/decargroup/pymlg) for back-end Lie group mathematical operations. Examples ^^^^^^^^ @@ -105,7 +105,7 @@ As another more complicated example, a state object belonging to the SE(3) Lie g .. code-block:: python from navlie.types import State - from pylie import SE3 + from pymlg import SE3 import numpy as np class SE3State(State): diff --git a/examples/ex_inertial_nav.py b/examples/ex_inertial_nav.py index 888751c0..ff5fcb80 100644 --- a/examples/ex_inertial_nav.py +++ b/examples/ex_inertial_nav.py @@ -4,7 +4,7 @@ """ from typing import List import numpy as np -from pylie import SE23 +from pymlg import SE23 from navlie.lib import ( IMU, IMUState, diff --git a/examples/ex_invariant_so3.py b/examples/ex_invariant_so3.py index 7fe196f3..7b240225 100644 --- a/examples/ex_invariant_so3.py +++ b/examples/ex_invariant_so3.py @@ -6,7 +6,7 @@ SO3State, ) import navlie as nav -from pylie import SO3 +from pymlg import SO3 import numpy as np diff --git a/navlie/lib/datasets.py b/navlie/lib/datasets.py index c5d6f145..aea40ff4 100644 --- a/navlie/lib/datasets.py +++ b/navlie/lib/datasets.py @@ -13,7 +13,7 @@ GlobalPosition, ) import numpy as np -from pylie import SE3, SE23 +from pymlg import SE3, SE23 class SimulatedPoseRangingDataset(nav.Dataset): diff --git a/navlie/lib/imu.py b/navlie/lib/imu.py index 1a7ae6fd..586d9370 100644 --- a/navlie/lib/imu.py +++ b/navlie/lib/imu.py @@ -1,4 +1,4 @@ -from pylie import SO3, SE23 +from pymlg import SO3, SE23 import numpy as np from navlie.types import ProcessModel, Input from typing import Any, List, Tuple diff --git a/navlie/lib/models.py b/navlie/lib/models.py index 8e3bc83a..28f76387 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -10,7 +10,7 @@ MatrixLieGroupState, VectorState, ) -from pylie import SO2, SO3 +from pymlg import SO2, SO3 import numpy as np from typing import List, Any from scipy.linalg import block_diag diff --git a/navlie/lib/preintegration.py b/navlie/lib/preintegration.py index bddcbfc0..c97b86b8 100644 --- a/navlie/lib/preintegration.py +++ b/navlie/lib/preintegration.py @@ -12,7 +12,7 @@ ) from navlie.lib.states import MatrixLieGroupState, VectorState import numpy as np -from pylie import SO3, SE3, SE2, SE23, MatrixLieGroup +from pymlg import SO3, SE3, SE2, SE23, MatrixLieGroup class RelativeMotionIncrement(Input): diff --git a/navlie/lib/states.py b/navlie/lib/states.py index 4b383a28..322192a3 100644 --- a/navlie/lib/states.py +++ b/navlie/lib/states.py @@ -1,5 +1,5 @@ -from pylie import SO2, SO3, SE2, SE3, SE23, SL3 -from pylie.numpy.base import MatrixLieGroup +from pymlg import SO2, SO3, SE2, SE3, SE23, SL3 +from pymlg.numpy.base import MatrixLieGroup import numpy as np from navlie.types import State from typing import Any, List @@ -77,7 +77,7 @@ def __init__( coordinates. Otherwise, the value must be a 2D numpy array representing a direct element of the group in matrix form. group : MatrixLieGroup - A `pylie.MatrixLieGroup` class, such as `pylie.SE2` or `pylie.SO3`. + A `pymlg.MatrixLieGroup` class, such as `pymlg.SE2` or `pymlg.SO3`. stamp : float, optional timestamp, by default None state_id : Any, optional diff --git a/setup.py b/setup.py index 28b34f3d..c703c57d 100644 --- a/setup.py +++ b/setup.py @@ -15,7 +15,7 @@ "scipy>=1.7.1", "matplotlib>=3.4.3", "joblib>=1.2.0", - "pylie @ git+https://github.com/decargroup/pylie@main", + "pymlg @ git+https://github.com/decargroup/pymlg@main", "tqdm>=4.64.1", "seaborn>=0.11.2", ], diff --git a/tests/integration/test_batch_noiseless.py b/tests/integration/test_batch_noiseless.py index 4a1dd498..33e9c1c2 100644 --- a/tests/integration/test_batch_noiseless.py +++ b/tests/integration/test_batch_noiseless.py @@ -15,7 +15,7 @@ from navlie.filters import ExtendedKalmanFilter, run_filter from navlie.utils import GaussianResult, GaussianResultList, plot_error, randvec from navlie.batch.estimator import BatchEstimator -from pylie import SO3, SE3 +from pymlg import SO3, SE3 import numpy as np import matplotlib.pyplot as plt diff --git a/tests/integration/test_filter_ekf_basic.py b/tests/integration/test_filter_ekf_basic.py index c2802e56..d2d43e0b 100644 --- a/tests/integration/test_filter_ekf_basic.py +++ b/tests/integration/test_filter_ekf_basic.py @@ -19,7 +19,7 @@ Gravitometer, RangePoseToAnchor, ) -from pylie import SE3 +from pymlg import SE3 import numpy as np from typing import List diff --git a/tests/integration/test_filter_ekf_noiseless.py b/tests/integration/test_filter_ekf_noiseless.py index 067d22cd..25779480 100644 --- a/tests/integration/test_filter_ekf_noiseless.py +++ b/tests/integration/test_filter_ekf_noiseless.py @@ -18,7 +18,7 @@ Gravitometer, RangePoseToAnchor, ) -from pylie import SE3 +from pymlg import SE3 import numpy as np from typing import List diff --git a/tests/integration/test_interacting_multiple_models.py b/tests/integration/test_interacting_multiple_models.py index c4c99b5b..39ad8264 100644 --- a/tests/integration/test_interacting_multiple_models.py +++ b/tests/integration/test_interacting_multiple_models.py @@ -14,7 +14,7 @@ Gravitometer, ) from navlie.lib.models import RangePoseToAnchor -from pylie import SE3 +from pymlg import SE3 import numpy as np from typing import List diff --git a/tests/integration/test_iterated_ekf.py b/tests/integration/test_iterated_ekf.py index f34c086f..b7570ad6 100644 --- a/tests/integration/test_iterated_ekf.py +++ b/tests/integration/test_iterated_ekf.py @@ -4,7 +4,7 @@ from navlie.filters import ExtendedKalmanFilter, IteratedKalmanFilter from navlie.utils import GaussianResult, GaussianResultList, randvec, monte_carlo from navlie.types import StateWithCovariance, State -from pylie import SE3 +from pymlg import SE3 import numpy as np from typing import List import pytest diff --git a/tests/unit/test_batch_residuals.py b/tests/unit/test_batch_residuals.py index b00ef7d8..d7694127 100644 --- a/tests/unit/test_batch_residuals.py +++ b/tests/unit/test_batch_residuals.py @@ -4,7 +4,7 @@ from navlie.lib.models import SingleIntegrator, BodyFrameVelocity, GlobalPosition from navlie.batch.estimator import PriorResidual, ProcessResidual, MeasurementResidual from navlie.lib.states import VectorState, SE3State -from pylie import SE3 +from pymlg import SE3 import numpy as np def test_prior_residual_vector(): diff --git a/tests/unit/test_body_velocity.py b/tests/unit/test_body_velocity.py index 5c4fe062..f75a271d 100644 --- a/tests/unit/test_body_velocity.py +++ b/tests/unit/test_body_velocity.py @@ -4,7 +4,7 @@ RangePoseToAnchor, RelativeBodyFrameVelocity, ) -from pylie import SO2, SO3, SE3, SE2, SE3, SE23 +from pymlg import SO2, SO3, SE3, SE2, SE3, SE23 import numpy as np import pytest from navlie.types import StampedValue diff --git a/tests/unit/test_camera.py b/tests/unit/test_camera.py index c3af729c..466b2b25 100644 --- a/tests/unit/test_camera.py +++ b/tests/unit/test_camera.py @@ -1,6 +1,6 @@ from navlie.lib.camera import Camera, PoseMatrix import numpy as np -from pylie import SE3 +from pymlg import SE3 def test_valid_measurements(): diff --git a/tests/unit/test_composite.py b/tests/unit/test_composite.py index 19f745cd..ba94a7cd 100644 --- a/tests/unit/test_composite.py +++ b/tests/unit/test_composite.py @@ -6,7 +6,7 @@ RangeRelativePose, CompositeInput, ) -from pylie import SE2 +from pymlg import SE2 import numpy as np import pickle import os diff --git a/tests/unit/test_imu.py b/tests/unit/test_imu.py index 678d75b1..153cca6f 100644 --- a/tests/unit/test_imu.py +++ b/tests/unit/test_imu.py @@ -10,7 +10,7 @@ G_matrix, G_matrix_inv, ) -from pylie import SE23, SO3 +from pymlg import SE23, SO3 import numpy as np from math import factorial import pytest diff --git a/tests/unit/test_meas_models.py b/tests/unit/test_meas_models.py index 77bc38bf..0664cdbe 100644 --- a/tests/unit/test_meas_models.py +++ b/tests/unit/test_meas_models.py @@ -19,7 +19,7 @@ GlobalVelocity, ) from navlie.types import Measurement, MeasurementModel -from pylie import SO3, SE3, SE2, SE3, SE23 +from pymlg import SO3, SE3, SE2, SE3, SE23 import numpy as np import pytest diff --git a/tests/unit/test_preintegration.py b/tests/unit/test_preintegration.py index 2696cffa..deb0dd19 100644 --- a/tests/unit/test_preintegration.py +++ b/tests/unit/test_preintegration.py @@ -11,7 +11,7 @@ from navlie.filters import ExtendedKalmanFilter from navlie.lib.states import SE3State, VectorState import numpy as np -from pylie import SE23, SE2, SE3, SO3 +from pymlg import SE23, SE2, SE3, SO3 from navlie.types import StampedValue, StateWithCovariance import pytest diff --git a/tests/unit/test_spkf.py b/tests/unit/test_spkf.py index 282427f7..a61915c3 100644 --- a/tests/unit/test_spkf.py +++ b/tests/unit/test_spkf.py @@ -1,7 +1,7 @@ from navlie.lib.states import MatrixLieGroupState, VectorState, SE3State import numpy as np from navlie.filters import mean_state, generate_sigmapoints -from pylie import SE3 +from pymlg import SE3 np.random.seed(0) def test_mean_state_vector(): diff --git a/tests/unit/test_states.py b/tests/unit/test_states.py index 8e11314d..7731ca94 100644 --- a/tests/unit/test_states.py +++ b/tests/unit/test_states.py @@ -10,7 +10,7 @@ MatrixLieGroupState ) from navlie.types import State -from pylie import SO3, SE3 +from pymlg import SO3, SE3 import numpy as np import pytest import sys diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 3a1ab247..f5248983 100644 --- a/tests/unit/test_utils.py +++ b/tests/unit/test_utils.py @@ -7,7 +7,7 @@ SE3State, SO3State, ) -from pylie import ( +from pymlg import ( SE23, SE3, SO3 ) import numpy as np