From d25d40c94c2f181ca0ca8d8d7f8a4e8d46621afd Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Mon, 4 Dec 2023 20:56:03 -0800 Subject: [PATCH 1/4] Moved `CompositeState` and friends to dedicated file. Ran black. --- docs/source/fun_figs.py | 53 +- examples/ex_ekf_vector.py | 4 +- examples/ex_imm_se3.py | 16 +- examples/ex_imm_vector.py | 6 +- examples/ex_inertial_gps.py | 4 +- examples/ex_inertial_nav.py | 8 +- examples/ex_invariant_so3.py | 8 +- examples/ex_monte_carlo.py | 8 +- examples/ex_ukf_se3.py | 4 +- examples/ex_ukf_vector.py | 8 +- examples/ex_varying_noise.py | 4 +- navlie/__init__.py | 8 +- navlie/batch/problem.py | 20 +- navlie/batch/residuals.py | 4 +- navlie/composite.py | 567 ++++++++++++++++++ navlie/datagen.py | 8 +- navlie/filters.py | 53 +- navlie/imm.py | 20 +- navlie/lib/__init__.py | 2 + navlie/lib/camera.py | 4 +- navlie/lib/datasets.py | 4 +- navlie/lib/imu.py | 26 +- navlie/lib/models.py | 328 ++-------- navlie/lib/preintegration.py | 32 +- navlie/lib/states.py | 395 +----------- navlie/types.py | 30 +- navlie/utils.py | 29 +- tests/integration/test_batch_noiseless.py | 4 +- .../integration/test_filter_ekf_noiseless.py | 4 +- .../test_interacting_multiple_models.py | 3 +- tests/integration/test_iterated_ekf.py | 12 +- tests/unit/test_composite.py | 6 +- tests/unit/test_meas_models.py | 29 +- tests/unit/test_states.py | 4 +- tests/unit/test_utils.py | 21 +- tests/unit/test_van_loan.py | 4 +- 36 files changed, 786 insertions(+), 954 deletions(-) create mode 100644 navlie/composite.py diff --git a/docs/source/fun_figs.py b/docs/source/fun_figs.py index 46a311d7..9bf9e490 100644 --- a/docs/source/fun_figs.py +++ b/docs/source/fun_figs.py @@ -6,8 +6,9 @@ sns.set_style("whitegrid") + # %% Banana distribution plot -def banana_plot(ax = None): +def banana_plot(ax=None): N = 500 x0 = nav.lib.SE2State([0.3, 3, 4], direction="right") covariance = np.diag([0.2**2, 0.05**2, 0.05**2]) @@ -38,15 +39,15 @@ def banana_plot(ax = None): # random greyscale color color = np.random.uniform(0.3, 0.9) - ax.plot(traj_pos[:, 0], traj_pos[:, 1], color=(color, color, color),zorder=1) + ax.plot(traj_pos[:, 0], traj_pos[:, 1], color=(color, color, color), zorder=1) # save the final state final_states.append(x_traj[-1]) - + final_positions = np.array([x.position for x in final_states]) ax.scatter(final_positions[:, 0], final_positions[:, 1], color="C0", zorder=2) - # Propagate the mean with EKF + # Propagate the mean with EKF kf = nav.ExtendedKalmanFilter(process_model) x0_hat = nav.StateWithCovariance(x0, covariance) @@ -59,10 +60,12 @@ def banana_plot(ax = None): ax.plot(mean_traj[:, 0], mean_traj[:, 1], color="r", zorder=3, linewidth=3) ax.set_aspect("equal") + # banana_plot() + # %% -def pose3d_plot(ax = None): +def pose3d_plot(ax=None): N = 500 x0 = nav.lib.SE3State([0.3, 3, 4, 0, 0, 0], direction="right") process_model = nav.lib.BodyFrameVelocity(np.zeros(6)) @@ -78,13 +81,14 @@ def pose3d_plot(ax = None): x = process_model.evaluate(x, u, dt) x_traj.append(x.copy()) - fig, ax = nav.plot_poses(x_traj, ax = ax) + fig, ax = nav.plot_poses(x_traj, ax=ax) # pose3d_plot() + # %% -def three_sigma_plot(axs = None): +def three_sigma_plot(axs=None): dataset = nav.lib.datasets.SimulatedPoseRangingDataset() estimates = nav.run_filter( @@ -92,10 +96,12 @@ def three_sigma_plot(axs = None): dataset.get_ground_truth()[0], np.diag([0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2]), dataset.get_input_data(), - dataset.get_measurement_data() - ) - - results = nav.GaussianResultList.from_estimates(estimates, dataset.get_ground_truth()) + dataset.get_measurement_data(), + ) + + results = nav.GaussianResultList.from_estimates( + estimates, dataset.get_ground_truth() + ) fig, axs = nav.plot_error(results[:, :3], axs=axs) axs[2].set_xlabel("Time (s)") @@ -105,7 +111,6 @@ def three_sigma_plot(axs = None): if __name__ == "__main__": - # Make one large figure which has all the plots. This will be a 1x3 grid, with the # last plot itself being a three vertically stacked plots. @@ -119,13 +124,11 @@ def three_sigma_plot(axs = None): # which will be used here: - - fig = plt.figure(figsize=(20, 6)) gs = fig.add_gridspec(1, 3, width_ratios=[1, 1, 1]) ax1 = fig.add_subplot(gs[0]) - ax2 = fig.add_subplot(gs[1], projection='3d') - + ax2 = fig.add_subplot(gs[1], projection="3d") + # The last plot is a 3x1 grid gs2 = gs[2].subgridspec(3, 1, hspace=0.1) ax3 = fig.add_subplot(gs2[0]) @@ -141,25 +144,21 @@ def three_sigma_plot(axs = None): ax2.set_yticklabels([]) ax2.set_zticklabels([]) - banana_plot(ax1) pose3d_plot(ax2) three_sigma_plot(np.array([ax3, ax4, ax5])) - # Set spacing to the above values + # Set spacing to the above values fig.subplots_adjust( - top=0.975, - bottom=0.097, - left=0.025, - right=0.992, - hspace=0.2, - wspace=0.117 + top=0.975, bottom=0.097, left=0.025, right=0.992, hspace=0.2, wspace=0.117 ) - # Save the figure with transparent background, next to this file - import os - fig.savefig(os.path.join(os.path.dirname(__file__), "fun_figs.png"), transparent=True) + import os + + fig.savefig( + os.path.join(os.path.dirname(__file__), "fun_figs.png"), transparent=True + ) plt.show() # %% diff --git a/examples/ex_ekf_vector.py b/examples/ex_ekf_vector.py index a15bc2c4..e28d7d17 100644 --- a/examples/ex_ekf_vector.py +++ b/examples/ex_ekf_vector.py @@ -93,9 +93,7 @@ def main(): sns.set_style("whitegrid") 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.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)") diff --git a/examples/ex_imm_se3.py b/examples/ex_imm_se3.py index 15a87cd2..baca262d 100644 --- a/examples/ex_imm_se3.py +++ b/examples/ex_imm_se3.py @@ -102,9 +102,7 @@ def imm_trial(trial_number: int) -> List[nav.GaussianResult]: x0_check = x0.plus(nav.randvec(P0)) - estimate_list = nav.imm.run_imm_filter( - imm, x0_check, P0, input_list, meas_list - ) + 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]) @@ -144,12 +142,8 @@ def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: 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_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), @@ -199,9 +193,7 @@ def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: 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] - ) + 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( diff --git a/examples/ex_imm_vector.py b/examples/ex_imm_vector.py index 0d09c42c..5607c67d 100644 --- a/examples/ex_imm_vector.py +++ b/examples/ex_imm_vector.py @@ -58,9 +58,9 @@ def main(): # 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)) + 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 diff --git a/examples/ex_inertial_gps.py b/examples/ex_inertial_gps.py index 8d0faae9..24505602 100644 --- a/examples/ex_inertial_gps.py +++ b/examples/ex_inertial_gps.py @@ -43,9 +43,7 @@ def main(): 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, ax, line_color="tab:blue", step=20, label="Estimate") nav.plot_poses( results.state_true, ax, diff --git a/examples/ex_inertial_nav.py b/examples/ex_inertial_nav.py index daf4ac74..ec6755f2 100644 --- a/examples/ex_inertial_nav.py +++ b/examples/ex_inertial_nav.py @@ -168,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] @@ -198,9 +196,7 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: 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, ax, line_color="tab:blue", step=500, label="Estimate") nav.plot_poses( results.state_true, ax, diff --git a/examples/ex_invariant_so3.py b/examples/ex_invariant_so3.py index c94a72ce..7b240225 100644 --- a/examples/ex_invariant_so3.py +++ b/examples/ex_invariant_so3.py @@ -45,9 +45,7 @@ def main(): # 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 - ) + results_ekf = nav.GaussianResultList.from_estimates(estimate_list, state_true) # ########################################################################## # Run the invariant filter @@ -59,9 +57,7 @@ def main(): ekf = nav.ExtendedKalmanFilter(process_model=process_model) estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants) - results_invariant = nav.GaussianResultList.from_estimates( - estimate_list, state_true - ) + results_invariant = nav.GaussianResultList.from_estimates(estimate_list, state_true) return results_ekf, results_invariant diff --git a/examples/ex_monte_carlo.py b/examples/ex_monte_carlo.py index cf7e1610..dd230d0d 100644 --- a/examples/ex_monte_carlo.py +++ b/examples/ex_monte_carlo.py @@ -29,9 +29,7 @@ def main(): 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] - ) + 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), @@ -58,9 +56,7 @@ def ekf_trial(trial_number: int) -> List[GaussianResult]: # 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 - ) + 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) diff --git a/examples/ex_ukf_se3.py b/examples/ex_ukf_se3.py index 557f0739..c3dd2928 100644 --- a/examples/ex_ukf_se3.py +++ b/examples/ex_ukf_se3.py @@ -31,9 +31,7 @@ def main(): # %% ####################################################################### # Run Filter - ukf = SigmaPointKalmanFilter( - process_model, method="unscented", iterate_mean=False - ) + ukf = SigmaPointKalmanFilter(process_model, method="unscented", iterate_mean=False) # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! start_time = time.time() diff --git a/examples/ex_ukf_vector.py b/examples/ex_ukf_vector.py index 86dedae7..ce6d510d 100644 --- a/examples/ex_ukf_vector.py +++ b/examples/ex_ukf_vector.py @@ -56,9 +56,7 @@ def main(): if noise_active: x0 = x0.plus(randvec(P0)) - ukf = SigmaPointKalmanFilter( - process_model, method="cubature", iterate_mean=False - ) + ukf = SigmaPointKalmanFilter(process_model, method="cubature", iterate_mean=False) # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! start_time = time.time() @@ -75,9 +73,7 @@ def main(): 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.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)") diff --git a/examples/ex_varying_noise.py b/examples/ex_varying_noise.py index 147eb3de..a7971587 100644 --- a/examples/ex_varying_noise.py +++ b/examples/ex_varying_noise.py @@ -79,9 +79,7 @@ def ekf_trial(trial_number: int) -> List[GaussianResult]: """ np.random.seed(trial_number) - state_true, input_data, meas_data = dg.generate( - x0_true, 0, t_max, noise=True - ) + state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=True) x0_check = x0_true.plus(randvec(P0)) diff --git a/navlie/__init__.py b/navlie/__init__.py index 9cd28a55..45e8085d 100644 --- a/navlie/__init__.py +++ b/navlie/__init__.py @@ -40,5 +40,11 @@ jacobian, ) +from .composite import ( + CompositeState, + CompositeProcessModel, + CompositeMeasurementModel, + CompositeInput, +) -from .lib.states import StampedValue # for backwards compatibility +from .lib.states import StampedValue # for backwards compatibility diff --git a/navlie/batch/problem.py b/navlie/batch/problem.py index 9f0c7034..4d8ba88f 100644 --- a/navlie/batch/problem.py +++ b/navlie/batch/problem.py @@ -262,9 +262,7 @@ def _solve_LM(self) -> Dict[Hashable, State]: # Compute the new value of the cost function after the update e, H, cost = self.compute_error_jac_cost(variables=variables_test) - gain_ratio = (prev_cost - cost) / ( - 0.5 * delta_x.T @ (mu * delta_x - b) - ) + gain_ratio = (prev_cost - cost) / (0.5 * delta_x.T @ (mu * delta_x - b)) gain_ratio = gain_ratio.item(0) # If the gain ratio is above zero, accept the step @@ -333,9 +331,7 @@ def compute_error_jac_cost( cost_list = [] # For each factor, evaluate error and Jacobian - for i, (residual, loss) in enumerate( - zip(self.residual_list, self.loss_list) - ): + for i, (residual, loss) in enumerate(zip(self.residual_list, self.loss_list)): variables_list = [variables[key] for key in residual.keys] # Do not compute Jacobian for variables that are held fixed @@ -345,9 +341,7 @@ def compute_error_jac_cost( ] # Evaluate current factor at states - error, jacobians = residual.evaluate( - variables_list, compute_jacobians - ) + error, jacobians = residual.evaluate(variables_list, compute_jacobians) # Compute the robust loss weight and then weight the error u = np.linalg.norm(error) @@ -368,9 +362,7 @@ def compute_error_jac_cost( # Correctly weight the Jacobian jacobian = sqrt_loss_weight * jacobian - H[ - self.residual_slices[i], self.variable_slices[key] - ] = jacobian + H[self.residual_slices[i], self.variable_slices[key]] = jacobian # Sum up costs from each residual cost = np.sum(np.array(cost_list)) @@ -436,9 +428,7 @@ def _correct_states( delta_xi_current = delta_x[slc, [0]] variables[key] = var.plus(delta_xi_current) - def get_covariance_block( - self, key_1: Hashable, key_2: Hashable - ) -> np.ndarray: + def get_covariance_block(self, key_1: Hashable, key_2: Hashable) -> np.ndarray: """Retrieve the covariance block corresponding to two variables. Parameters diff --git a/navlie/batch/residuals.py b/navlie/batch/residuals.py index ba2540a7..229f12c1 100644 --- a/navlie/batch/residuals.py +++ b/navlie/batch/residuals.py @@ -160,9 +160,7 @@ def evaluate( if compute_jacobians: jac_list = [None] * len(states) if compute_jacobians[0]: - jac_list[0] = -L.T @ self._process_model.jacobian( - x_km1, self._u, dt - ) + jac_list[0] = -L.T @ self._process_model.jacobian(x_km1, self._u, dt) if compute_jacobians[1]: jac_list[1] = L.T @ x_k.minus_jacobian(x_k_hat) diff --git a/navlie/composite.py b/navlie/composite.py new file mode 100644 index 00000000..e3c7b0b7 --- /dev/null +++ b/navlie/composite.py @@ -0,0 +1,567 @@ +from typing import Any, List +import numpy as np +from navlie import State, Measurement, MeasurementModel, ProcessModel, Input +from scipy.linalg import block_diag + + +class CompositeState(State): + """ + A "composite" state object intended to hold a list of State objects as a + single conceptual "state". The intended use is to hold a list of states + as a single state at a specific time, of potentially different types, + and this class will take care of defining the appropriate operations on + the composite state such as the ``plus`` and ``minus`` methods, as well + as the ``plus_jacobian`` and ``minus_jacobian`` methods. + + Each state in the provided list has an index (the index in the list), as + well as a state_id, which is found as an attribute in the corresponding State + object. + + It is possible to access sub-states in the composite states both by index + and by ID. + """ + + def __init__(self, state_list: List[State], stamp: float = None, state_id=None): + """ + Parameters + ---------- + state_list: List[State] + List of State that forms this composite state + stamp: float, optional + Timestamp of the composite state. This can technically be different + from the timestamps of the substates. + state_id: Any, optional + State ID of the composite state. This can be different from the + state IDs of the substates. + """ + if isinstance(state_list, tuple): + state_list = list(state_list) + + #:List[State]: The substates are the CompositeState's value. + self.value = state_list + + self.stamp = stamp + self.state_id = state_id + + def __getstate__(self): + """ + Get the state of the object for pickling. + """ + # When using __slots__ the pickle module expects a tuple from __getstate__. + # See https://stackoverflow.com/questions/1939058/simple-example-of-use-of-setstate-and-getstate/41754104#41754104 + return ( + None, + { + "value": self.value, + "stamp": self.stamp, + "state_id": self.state_id, + }, + ) + + def __setstate__(self, attributes): + """ + Set the state of the object for unpickling. + """ + # When using __slots__ the pickle module sends a tuple for __setstate__. + # See https://stackoverflow.com/questions/1939058/simple-example-of-use-of-setstate-and-getstate/41754104#41754104 + + attributes = attributes[1] + self.value = attributes["value"] + self.stamp = attributes["stamp"] + self.state_id = attributes["state_id"] + + @property + def dof(self): + return sum([x.dof for x in self.value]) + + def get_index_by_id(self, state_id): + """ + Get index of a particular state_id in the list of states. + """ + return [x.state_id for x in self.value].index(state_id) + + def get_slices(self) -> List[slice]: + """ + Get slices for each state in the list of states. + """ + slices = [] + counter = 0 + for state in self.value: + slices.append(slice(counter, counter + state.dof)) + counter += state.dof + + return slices + + def add_state(self, state: State, stamp: float = None, state_id=None): + """Adds a state and it's corresponding slice to the composite state.""" + self.value.append(state) + + def remove_state_by_id(self, state_id): + """Removes a given state by ID.""" + idx = self.get_index_by_id(state_id) + self.value.pop(idx) + + def get_slice_by_id(self, state_id, slices=None): + """ + Get slice of a particular state_id in the list of states. + """ + + if slices is None: + slices = self.get_slices() + + idx = self.get_index_by_id(state_id) + return slices[idx] + + def get_matrix_block_by_ids( + self, mat: np.ndarray, state_id_1: Any, state_id_2: Any = None + ) -> np.ndarray: + """Gets the portion of a matrix corresponding to two states. + + This function is useful when extract specific blocks of a covariance + matrix, for example. + + Parameters + ---------- + mat : np.ndarray + N x N matrix + state_id_1 : Any + State ID of state 1. + state_id_2 : Any, optional + State ID of state 2. If None, state_id_2 is set to state_id_1. + + Returns + ------- + np.ndarray + Subblock of mat corrsponding to + slices of state_id_1 and state_id_2. + """ + + if state_id_2 is None: + state_id_2 = state_id_1 + + slice_1 = self.get_slice_by_id(state_id_1) + slice_2 = self.get_slice_by_id(state_id_2) + + return mat[slice_1, slice_2] + + def set_matrix_block_by_ids( + self, + new_mat_block: np.ndarray, + mat: np.ndarray, + state_id_1: Any, + state_id_2: Any = None, + ) -> np.ndarray: + """Sets the portion of the covariance block corresponding to two states. + + Parameters + ---------- + new_mat_block : np.ndarray + A subblock to be entered into mat. + mat : np.ndarray + Full matrix. + state_id_1 : Any + State ID of state 1. + state_id_2 : Any, optional + State ID of state 2. If None, state_id_2 is set to state_id_1. + + Returns + ------- + np.ndarray + mat with updated subblock. + """ + + if state_id_2 is None: + state_id_2 = state_id_1 + + slice_1 = self.get_slice_by_id(state_id_1) + slice_2 = self.get_slice_by_id(state_id_2) + + mat[slice_1, slice_2] = new_mat_block + return mat + + def get_value_by_id(self, state_id) -> Any: + """ + Get state value by id. + """ + idx = self.get_index_by_id(state_id) + return self.value[idx].value + + def get_state_by_id(self, state_id) -> State: + """ + Get state object by id. + """ + idx = self.get_index_by_id(state_id) + return self.value[idx] + + def get_dof_by_id(self, state_id) -> int: + """ + Get degrees of freedom of sub-state by id. + """ + idx = self.get_index_by_id(state_id) + return self.value[idx].dof + + def get_stamp_by_id(self, state_id) -> float: + """ + Get timestamp of sub-state by id. + """ + idx = self.get_index_by_id(state_id) + return self.value[idx].stamp + + def set_stamp_by_id(self, stamp: float, state_id): + """ + Set the timestamp of a sub-state by id. + """ + idx = self.get_index_by_id(state_id) + self.value[idx].stamp = stamp + + def set_state_by_id(self, state: State, state_id): + """ + Set the whole sub-state by id. + """ + idx = self.get_index_by_id(state_id) + self.value[idx] = state + + def set_value_by_id(self, value: Any, state_id: Any): + """ + Set the value of a sub-state by id. + """ + idx = self.get_index_by_id(state_id) + self.value[idx].value = value + + def set_stamp_for_all(self, stamp: float): + """ + Set the timestamp of all substates. + """ + for state in self.value: + state.stamp = stamp + + def to_list(self): + """ + Converts the CompositeState object back into a list of states. + """ + return self.value + + def copy(self) -> "CompositeState": + """ + Returns a new composite state object where the state values have also + been copied. + """ + return self.__class__( + [state.copy() for state in self.value], self.stamp, self.state_id + ) + + def plus(self, dx, new_stamp: float = None) -> "CompositeState": + """ + Updates the value of each sub-state given a dx. Interally parses + the dx vector. + """ + new = self.copy() + for i, state in enumerate(new.value): + new.value[i] = state.plus(dx[: state.dof]) + dx = dx[state.dof :] + + if new_stamp is not None: + new.set_stamp_for_all(new_stamp) + + return new + + def minus(self, x: "CompositeState") -> np.ndarray: + dx = [] + for i, v in enumerate(x.value): + dx.append(self.value[i].minus(x.value[i]).reshape((self.value[i].dof,))) + + return np.concatenate(dx).reshape((-1, 1)) + + def plus_by_id( + self, dx, state_id: int, new_stamp: float = None + ) -> "CompositeState": + """ + Updates a specific sub-state. + """ + new = self.copy() + idx = new.get_index_by_id(state_id) + new.value[idx].plus(dx) + if new_stamp is not None: + new.set_stamp_by_id(new_stamp, state_id) + + return new + + def jacobian_from_blocks(self, block_dict: dict): + """ + Returns the jacobian of the entire composite state given jacobians + associated with some of the substates. These are provided as a dictionary + with the the keys being the substate IDs. + """ + block: np.ndarray = list(block_dict.values())[0] + m = block.shape[0] # Dimension of "y" value + jac = np.zeros((m, self.dof)) + slices = self.get_slices() + for state_id, block in block_dict.items(): + slc = self.get_slice_by_id(state_id, slices) + jac[:, slc] = block + + return jac + + def plus_jacobian(self, dx: np.ndarray) -> np.ndarray: + dof = self.dof + jac = np.zeros((dof, dof)) + counter = 0 + for state in self.value: + jac[ + counter : counter + state.dof, + counter : counter + state.dof, + ] = state.plus_jacobian(dx[: state.dof]) + dx = dx[state.dof :] + counter += state.dof + + return jac + + def minus_jacobian(self, x: "CompositeState") -> np.ndarray: + dof = self.dof + jac = np.zeros((dof, dof)) + counter = 0 + for i, state in enumerate(self.value): + jac[ + counter : counter + state.dof, + counter : counter + state.dof, + ] = state.minus_jacobian(x.value[i]) + counter += state.dof + + return jac + + def __repr__(self): + substate_line_list = [] + for v in self.value: + substate_line_list.extend(v.__repr__().split("\n")) + substates_str = "\n".join([" " + s for s in substate_line_list]) + s = [ + f"{self.__class__.__name__}(stamp={self.stamp}, state_id={self.state_id}) with substates:", + substates_str, + ] + return "\n".join(s) + + +class CompositeInput(Input): + """ + + """ + + # TODO: add tests to new methods + def __init__(self, input_list: List[Input]) -> None: + self.input_list = input_list + + @property + def dof(self) -> int: + return sum([input.dof for input in self.input_list]) + + @property + def stamp(self) -> float: + return self.input_list[0].stamp + + def get_index_by_id(self, state_id): + """ + Get index of a particular state_id in the list of inputs. + """ + return [x.state_id for x in self.input_list].index(state_id) + + def add_input(self, input: Input): + """ + Adds an input to the composite input. + """ + self.input_list.append(input) + + def remove_input_by_id(self, state_id): + """ + Removes a given input by ID. + """ + idx = self.get_index_by_id(state_id) + self.input_list.pop(idx) + + def get_input_by_id(self, state_id) -> Input: + """ + Get input object by id. + """ + idx = self.get_index_by_id(state_id) + return self.input_list[idx] + + def get_dof_by_id(self, state_id) -> int: + """ + Get degrees of freedom of sub-input by id. + """ + idx = self.get_index_by_id(state_id) + return self.input_list[idx].dof + + def get_stamp_by_id(self, state_id) -> float: + """ + Get timestamp of sub-input by id. + """ + idx = self.get_index_by_id(state_id) + return self.input_list[idx].stamp + + def set_stamp_by_id(self, stamp: float, state_id): + """ + Set the timestamp of a sub-input by id. + """ + idx = self.get_index_by_id(state_id) + self.input_list[idx].stamp = stamp + + def set_input_by_id(self, input: Input, state_id): + """ + Set the whole sub-input by id. + """ + idx = self.get_index_by_id(state_id) + self.input_list[idx] = input + + def set_stamp_for_all(self, stamp: float): + """ + Set the timestamp of all subinputs. + """ + for input in self.input_list: + input.stamp = stamp + + def to_list(self): + """ + Converts the CompositeInput object back into a list of inputs. + """ + return self.input_list + + def copy(self) -> "CompositeInput": + return CompositeInput([input.copy() for input in self.input_list]) + + def plus(self, w: np.ndarray): + new = self.copy() + temp = w + for i, input in enumerate(self.input_list): + new.input_list[i] = input.plus(temp[: input.dof]) + temp = temp[input.dof :] + + return new + + +class CompositeProcessModel(ProcessModel): + """ + + Should this be called a StackedProcessModel? + # TODO: Add documentation and tests + """ + + # TODO: This needs to be expanded and/or changed. We have come across the + # following use cases: + # 1. Applying a completely seperate process model to each sub-state. + # 2. Applying a single process model to each sub-state (seperately). + # 3. Applying a single process model to one sub-state, and leaving the rest + # unchanged. + # 4. Applying process model A to some sub-states, and process model B to + # other sub-states + # 5. Receiving a CompositeInput, which is a list of synchronously-received + # inputs, and applying each input to the corresponding sub-state. + # 6. Receiving the state-specific input asynchronously, applying to the + # corresponding sub-state, and leaving the rest unchanged. Typically happens + # with case 3. + + # What they all have in common: list of decoupled process models, one per + # substate. For coupled process models, the user will have to define their + # own process model from scratch. + + def __init__( + self, + model_list: List[ProcessModel], + shared_input: bool = False, + ): + self._model_list = model_list + self._shared_input = shared_input + + def evaluate( + self, + x: CompositeState, + u: CompositeInput, + dt: float, + ) -> CompositeState: + x = x.copy() + for i, x_sub in enumerate(x.value): + if self._shared_input: + u_sub = u + else: + u_sub = u.input_list[i] + x.value[i] = self._model_list[i].evaluate(x_sub, u_sub, dt) + + return x + + def jacobian( + self, + x: CompositeState, + u: CompositeInput, + dt: float, + ) -> np.ndarray: + jac = [] + for i, x_sub in enumerate(x.value): + if self._shared_input: + u_sub = u + else: + u_sub = u.input_list[i] + jac.append(self._model_list[i].jacobian(x_sub, u_sub, dt)) + + return block_diag(*jac) + + def covariance( + self, + x: CompositeState, + u: CompositeInput, + dt: float, + ) -> np.ndarray: + cov = [] + for i, x_sub in enumerate(x.value): + if self._shared_input: + u_sub = u + else: + u_sub = u.input_list[i] + cov.append(self._model_list[i].covariance(x_sub, u_sub, dt)) + + return block_diag(*cov) + + +class CompositeMeasurementModel(MeasurementModel): + """ + Wrapper for a standard measurement model that assigns the model to a specific + substate (referenced by `state_id`) inside a CompositeState. + """ + + def __init__(self, model: MeasurementModel, state_id): + self.model = model + self.state_id = state_id + + def __repr__(self): + return f"{self.model}(of substate {self.state_id})" + + def evaluate(self, x: CompositeState) -> np.ndarray: + return self.model.evaluate(x.get_state_by_id(self.state_id)) + + def jacobian(self, x: CompositeState) -> np.ndarray: + x_sub = x.get_state_by_id(self.state_id) + jac_sub = self.model.jacobian(x_sub) + jac = np.zeros((jac_sub.shape[0], x.dof)) + slc = x.get_slice_by_id(self.state_id) + jac[:, slc] = jac_sub + return jac + + def covariance(self, x: CompositeState) -> np.ndarray: + x_sub = x.get_state_by_id(self.state_id) + return self.model.covariance(x_sub) + + +class CompositeMeasurement(Measurement): + def __init__(self, y: Measurement, state_id: Any): + """ + Converts a standard Measurement into a CompositeMeasurement, which + replaces the model with a CompositeMeasurementModel. + + Parameters + ---------- + y : Measurement + Measurement to be converted. + state_id : Any + ID of the state that the measurement will be assigned to, + as per the CompositeMeasurementModel. + """ + model = CompositeMeasurementModel(y.model, state_id) + super().__init__(value=y.value, stamp=y.stamp, model=model, state_id=y.state_id) diff --git a/navlie/datagen.py b/navlie/datagen.py index da3eade9..72222d9a 100644 --- a/navlie/datagen.py +++ b/navlie/datagen.py @@ -51,7 +51,9 @@ def __init__( input_func: Callable[[float], np.ndarray], input_covariance: np.ndarray, input_freq: float, - meas_model_list: List[MeasurementModel] = [], # TODO: fix mutable default argument + meas_model_list: List[ + MeasurementModel + ] = [], # TODO: fix mutable default argument meas_freq_list: Union[float, List[float]] = None, meas_offset_list: Union[float, List[float]] = [], ): @@ -277,9 +279,7 @@ def generate_measurement( if noise: y = y.reshape((-1, 1)) + randvec(R) - meas_list.append( - Measurement(y.reshape(og_shape), x.stamp, model, state_id) - ) + meas_list.append(Measurement(y.reshape(og_shape), x.stamp, model, state_id)) if not received_list: meas_list = meas_list[0] diff --git a/navlie/filters.py b/navlie/filters.py index 2158aa1f..6a30f7ca 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -130,9 +130,7 @@ def predict( # usually only happens on estimator start-up if x.state.stamp is None: if u.stamp is None: - raise ValueError( - "Either state or input must have a time stamp" - ) + raise ValueError("Either state or input must have a time stamp") t_km1 = u.stamp else: t_km1 = x.state.stamp @@ -150,9 +148,7 @@ def predict( details_dict = {} if u is not None: Q = self.process_model.covariance(x_jac, u, dt) - x_new.state, A = self.process_model.evaluate_with_jacobian( - x.state, u, dt - ) + x_new.state, A = self.process_model.evaluate_with_jacobian(x.state, u, dt) x_new.covariance = A @ x.covariance @ A.T + Q x_new.symmetrize() x_new.state.stamp = t_km1 + dt @@ -218,9 +214,7 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < -1e10: - raise RuntimeError( - "Measurement stamp is earlier than state stamp" - ) + raise RuntimeError("Measurement stamp is earlier than state stamp") elif u is not None and dt > 1e-11: x = self.predict(x, u, dt) @@ -335,9 +329,7 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < 0: - raise RuntimeError( - "Measurement stamp is earlier than state stamp" - ) + raise RuntimeError("Measurement stamp is earlier than state stamp") elif dt > 0 and u is not None: x = self.predict(x, u, dt) @@ -362,9 +354,7 @@ def correct( P_inv = np.linalg.inv(x.covariance) R_inv = np.linalg.inv(R) - cost_old = np.ndarray.item( - 0.5 * (e.T @ P_inv @ e + z.T @ R_inv @ z) - ) + cost_old = np.ndarray.item(0.5 * (e.T @ P_inv @ e + z.T @ R_inv @ z)) P = J @ x.covariance @ J.T # Compute covariance of innovation @@ -397,8 +387,7 @@ def correct( z_new = y.minus(y_check) e_new = x_new.minus(x.state).reshape((-1, 1)) cost_new = np.ndarray.item( - 0.5 - * (e_new.T @ P_inv @ e_new + z_new.T @ R_inv @ z_new) + 0.5 * (e_new.T @ P_inv @ e_new + z_new.T @ R_inv @ z_new) ) if cost_new < cost_old: step_accepted = True @@ -439,18 +428,14 @@ def correct( S = 0.5 * (S + S.T) K = np.linalg.solve(S.T, (P @ G.T).T).T x.state = x_op - x.covariance = (np.identity(x.state.dof) - K @ G) @ ( - J @ x.covariance @ J.T - ) + x.covariance = (np.identity(x.state.dof) - K @ G) @ (J @ x.covariance @ J.T) x.symmetrize() return x -def generate_sigmapoints( - dof: int, method: str -) -> Tuple[np.ndarray, np.ndarray]: +def generate_sigmapoints(dof: int, method: str) -> Tuple[np.ndarray, np.ndarray]: """Generates unit sigma points from three available methods. @@ -605,9 +590,7 @@ def predict( if u.covariance is not None: input_covariance = u.covariance else: - raise ValueError( - "Input covariance information must be provided." - ) + raise ValueError("Input covariance information must be provided.") if dt is None: dt = u.stamp - x.state.stamp @@ -707,9 +690,7 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < 0: - raise RuntimeError( - "Measurement stamp is earlier than state stamp" - ) + raise RuntimeError("Measurement stamp is earlier than state stamp") elif u is not None: x = self.predict(x, u, dt) @@ -731,10 +712,7 @@ def correct( if y_check is not None: y_propagated = np.array( - [ - y.model.evaluate(x.state.plus(sp)).ravel() - for sp in sigmapoints.T - ] + [y.model.evaluate(x.state.plus(sp)).ravel() for sp in sigmapoints.T] ) # predicted measurement mean @@ -744,10 +722,7 @@ def correct( err = y_propagated - y_mean sigmapoints = sigmapoints.T Pyy = ( - np.sum( - w[:, None, None] * err[:, :, None] * err[:, None, :], axis=0 - ) - + R + np.sum(w[:, None, None] * err[:, :, None] * err[:, None, :], axis=0) + R ) Pxy = np.sum( w[:, None, None] * sigmapoints[:, :, None] * err[:, None, :], @@ -871,9 +846,7 @@ def run_filter( 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.correct(x, y, u) meas_idx += 1 if meas_idx < len(meas_data): diff --git a/navlie/imm.py b/navlie/imm.py index 9bb07ea3..3a468b2f 100644 --- a/navlie/imm.py +++ b/navlie/imm.py @@ -312,9 +312,7 @@ def interaction( mu_mix = np.zeros((n_modes, n_modes)) for i in range(n_modes): for j in range(n_modes): - mu_mix[i, j] = ( - 1.0 / c[j] * self.transition_matrix[i, j] * mu_models[i] - ) + mu_mix[i, j] = 1.0 / c[j] * self.transition_matrix[i, j] * mu_models[i] x_mix = [] for j in range(n_modes): @@ -382,23 +380,17 @@ def correct( c_bar = np.zeros(n_modes) for i in range(n_modes): for j in range(n_modes): - c_bar[j] = ( - c_bar[j] + self.transition_matrix[i, j] * mu_km_models[i] - ) + c_bar[j] = c_bar[j] + self.transition_matrix[i, j] * mu_km_models[i] # 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 @@ -473,9 +465,7 @@ def run_imm_filter( 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 dbffb169..6fd77577 100644 --- a/navlie/lib/__init__.py +++ b/navlie/lib/__init__.py @@ -36,6 +36,8 @@ GlobalPosition, InvariantMeasurement, PointRelativePosition, + AbsoluteVelocity, + AbsolutePosition, ) from .preintegration import ( diff --git a/navlie/lib/camera.py b/navlie/lib/camera.py index e43c5534..a8cafbb9 100644 --- a/navlie/lib/camera.py +++ b/navlie/lib/camera.py @@ -162,9 +162,7 @@ def is_measurement_valid(self, uv: np.ndarray) -> bool: and (uv[0] < self.image_width) ) - def is_landmark_in_front_of_cam( - self, pose: SE3State, r_pw_a: np.ndarray - ) -> bool: + def is_landmark_in_front_of_cam(self, pose: SE3State, r_pw_a: np.ndarray) -> bool: """Checks if a given landmark is in front of the camera.""" r_pc_c: np.ndarray = self.resolve_landmark_in_cam_frame(pose, r_pw_a) r_pc_c = r_pc_c.ravel() diff --git a/navlie/lib/datasets.py b/navlie/lib/datasets.py index fd53c6b1..d82c24bc 100644 --- a/navlie/lib/datasets.py +++ b/navlie/lib/datasets.py @@ -143,9 +143,7 @@ def __init__( Q_c = np.eye(12) Q_c[0:3, 0:3] *= 0.01**2 # Gyro continuous-time covariance Q_c[3:6, 3:6] *= 0.01**2 # Accel continuous-time covariance - Q_c[6:9, 6:9] *= ( - 0.0001**2 - ) # Gyro random-walk continuous-time covariance + Q_c[6:9, 6:9] *= 0.0001**2 # Gyro random-walk continuous-time covariance Q_c[9:12, 9:12] *= ( 0.0001**2 ) # Accel random-walk continuous-time covariance diff --git a/navlie/lib/imu.py b/navlie/lib/imu.py index 8c935262..d2798d6b 100644 --- a/navlie/lib/imu.py +++ b/navlie/lib/imu.py @@ -12,7 +12,8 @@ import numpy as np from navlie.types import ProcessModel, Input from typing import Any, List -from navlie.lib.states import CompositeState, VectorState, SE23State +from navlie.lib.states import VectorState, SE23State +from navlie.composite import CompositeState from math import factorial @@ -31,7 +32,7 @@ def __init__( state_id: Any = None, covariance: np.ndarray = None, ): - """ + """ Parameters ---------- gyro : np.ndarray with size 3 @@ -47,13 +48,11 @@ def __init__( state_id : Any, optional State ID associated with the reading, by default None covariance : np.ndarray with size 12x12, optional - Covariance matrix describing the IMU noise, by default None. + Covariance matrix describing the IMU noise, by default None. """ super().__init__(dof=12, stamp=stamp, covariance=covariance) self.gyro = np.array(gyro).ravel() #:np.ndarray: Gyro reading - self.accel = np.array( - accel - ).ravel() #:np.ndarray: Accelerometer reading + self.accel = np.array(accel).ravel() #:np.ndarray: Accelerometer reading if bias_accel_walk is None: bias_accel_walk = np.zeros((3, 1)) @@ -137,10 +136,11 @@ def random(): class IMUState(CompositeState): """ The IMU state is a composite state that contains the navigation state - (attitude, velocity, position), and the gyro and accelerometer biases. The + (attitude, velocity, position), and the gyro and accelerometer biases. The navigation state is stored as an element of :math:`SE_2(3)`, and the biases are stored as vectors. """ + def __init__( self, nav_state: np.ndarray, @@ -279,7 +279,6 @@ def jacobian_from_blocks( for you. """ - for jac in [attitude, position, velocity, bias_gyro, bias_accel]: if jac is not None: dim = jac.shape[0] @@ -344,13 +343,14 @@ def M_matrix(phi_vec): The M matrix from `derivation document `_ """ - phi_mat = SO3.wedge(phi_vec) + phi_mat = SO3.wedge(phi_vec) M = np.sum( [ - (2 / factorial(n + 2)) * np.linalg.matrix_power(phi_mat, n) for n in - range(100) - ], axis=0, - ) + (2 / factorial(n + 2)) * np.linalg.matrix_power(phi_mat, n) + for n in range(100) + ], + axis=0, + ) return M diff --git a/navlie/lib/models.py b/navlie/lib/models.py index 049f3cde..34ef96b7 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -2,18 +2,21 @@ Measurement, ProcessModel, MeasurementModel, - Input, ) from navlie.lib.states import ( - CompositeState, MatrixLieGroupState, VectorState, VectorInput, ) +from navlie.composite import CompositeState, CompositeMeasurementModel from pymlg import SO2, SO3 import numpy as np from typing import List, Any -from scipy.linalg import block_diag + +from navlie.composite import ( + CompositeInput, + CompositeProcessModel, +) # For backwards compatibility, since these was moved class SingleIntegrator(ProcessModel): @@ -106,7 +109,6 @@ def jacobian(self, x, u, dt) -> np.ndarray: return Ad def covariance(self, x, u, dt) -> np.ndarray: - Ld = self.input_jacobian(dt) return Ld @ self._Q @ Ld.T @@ -152,9 +154,7 @@ def __init__(self, Q: np.ndarray): self._Q = Q self.dim = int(Q.shape[0] / 2) - def evaluate( - self, x: VectorState, u: VectorInput, dt: float - ) -> VectorState: + def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState: x = x.copy() u = u.copy() Ad = super().jacobian(x, u, dt) @@ -189,7 +189,6 @@ def jacobian(self, x, u, dt) -> np.ndarray: return A def covariance(self, x, u, dt) -> np.ndarray: - L = self.input_jacobian(dt) return L @ self._Q @ L.T @@ -228,7 +227,7 @@ class BodyFrameVelocity(ProcessModel): .. math:: \mathbf{T}_k = \mathbf{T}_{k-1} \exp(\Delta t \mathbf{u}_{k-1}^\wedge) - This is commonly the process model associated with SE(n). + This is commonly the process model associated with SE(n). This class is comptabile with ``SO2State, SO3State, SE2State, SE3State, SE23State``. """ @@ -243,9 +242,7 @@ def evaluate( x.value = x.value @ x.group.Exp(u.value * dt) return x - def jacobian( - self, x: MatrixLieGroupState, u: VectorInput, dt: float - ) -> np.ndarray: + def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray: if x.direction == "right": return x.group.adjoint(x.group.Exp(-u.value * dt)) elif x.direction == "left": @@ -265,7 +262,7 @@ def covariance( class RelativeBodyFrameVelocity(ProcessModel): """ - The relative body-frame velocity process model is of the form + The relative body-frame velocity process model is of the form .. math:: \mathbf{T}_k = \exp(\Delta t \mathbf{u}_{1,k-1}^\wedge) @@ -283,7 +280,7 @@ class RelativeBodyFrameVelocity(ProcessModel): """ def __init__(self, Q1: np.ndarray, Q2: np.ndarray): - """ + """ Parameters ---------- Q1 : np.ndarray @@ -320,16 +317,12 @@ def evaluate( x.value = x.group.Exp(-u[0] * dt) @ x.value @ x.group.Exp(u[1] * dt) return x - def jacobian( - self, x: MatrixLieGroupState, u: VectorInput, dt: float - ) -> np.ndarray: + def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray: u = u.value.reshape((2, round(u.value.size / 2))) if x.direction == "right": return x.group.adjoint(x.group.Exp(-u[1] * dt)) else: - raise NotImplementedError( - "TODO: left jacobian not yet implemented." - ) + raise NotImplementedError("TODO: left jacobian not yet implemented.") def covariance( self, x: MatrixLieGroupState, u: VectorInput, dt: float @@ -346,9 +339,7 @@ def covariance( L2 = dt * x.group.left_jacobian(-dt * u2) return L1 @ self._Q1 @ L1.T + L2 @ self._Q2 @ L2.T else: - raise NotImplementedError( - "TODO: left covariance not yet implemented." - ) + raise NotImplementedError("TODO: left covariance not yet implemented.") class LinearMeasurement(MeasurementModel): @@ -363,6 +354,7 @@ class LinearMeasurement(MeasurementModel): This class is comptabile with ``VectorState``. """ + def __init__(self, C: np.ndarray, R: np.ndarray): """ Parameters @@ -385,238 +377,11 @@ def covariance(self, x: VectorState) -> np.ndarray: return self._R -class CompositeInput(Input): - """ - - """ - # TODO: add tests to new methods - def __init__(self, input_list: List[Input]) -> None: - self.input_list = input_list - - @property - def dof(self) -> int: - return sum([input.dof for input in self.input_list]) - - @property - def stamp(self) -> float: - return self.input_list[0].stamp - - def get_index_by_id(self, state_id): - """ - Get index of a particular state_id in the list of inputs. - """ - return [x.state_id for x in self.input_list].index(state_id) - - def add_input(self, input: Input): - """ - Adds an input to the composite input. - """ - self.input_list.append(input) - - def remove_input_by_id(self, state_id): - """ - Removes a given input by ID. - """ - idx = self.get_index_by_id(state_id) - self.input_list.pop(idx) - - def get_input_by_id(self, state_id) -> Input: - """ - Get input object by id. - """ - idx = self.get_index_by_id(state_id) - return self.input_list[idx] - - def get_dof_by_id(self, state_id) -> int: - """ - Get degrees of freedom of sub-input by id. - """ - idx = self.get_index_by_id(state_id) - return self.input_list[idx].dof - - def get_stamp_by_id(self, state_id) -> float: - """ - Get timestamp of sub-input by id. - """ - idx = self.get_index_by_id(state_id) - return self.input_list[idx].stamp - - def set_stamp_by_id(self, stamp: float, state_id): - """ - Set the timestamp of a sub-input by id. - """ - idx = self.get_index_by_id(state_id) - self.input_list[idx].stamp = stamp - - def set_input_by_id(self, input: Input, state_id): - """ - Set the whole sub-input by id. - """ - idx = self.get_index_by_id(state_id) - self.input_list[idx] = input - - def set_stamp_for_all(self, stamp: float): - """ - Set the timestamp of all subinputs. - """ - for input in self.input_list: - input.stamp = stamp - - def to_list(self): - """ - Converts the CompositeInput object back into a list of inputs. - """ - return self.input_list - - def copy(self) -> "CompositeInput": - return CompositeInput([input.copy() for input in self.input_list]) - - def plus(self, w: np.ndarray): - new = self.copy() - temp = w - for i, input in enumerate(self.input_list): - new.input_list[i] = input.plus(temp[: input.dof]) - temp = temp[input.dof :] - - return new - - -class CompositeProcessModel(ProcessModel): - """ - - Should this be called a StackedProcessModel? - # TODO: Add documentation and tests - """ - - # TODO: This needs to be expanded and/or changed. We have come across the - # following use cases: - # 1. Applying a completely seperate process model to each sub-state. - # 2. Applying a single process model to each sub-state (seperately). - # 3. Applying a single process model to one sub-state, and leaving the rest - # unchanged. - # 4. Applying process model A to some sub-states, and process model B to - # other sub-states - # 5. Receiving a CompositeInput, which is a list of synchronously-received - # inputs, and applying each input to the corresponding sub-state. - # 6. Receiving the state-specific input asynchronously, applying to the - # corresponding sub-state, and leaving the rest unchanged. Typically happens - # with case 3. - - # What they all have in common: list of decoupled process models, one per - # substate. For coupled process models, the user will have to define their - # own process model from scratch. - - def __init__( - self, - model_list: List[ProcessModel], - shared_input: bool = False, - ): - self._model_list = model_list - self._shared_input = shared_input - - def evaluate( - self, - x: CompositeState, - u: CompositeInput, - dt: float, - ) -> CompositeState: - x = x.copy() - for i, x_sub in enumerate(x.value): - if self._shared_input: - u_sub = u - else: - u_sub = u.input_list[i] - x.value[i] = self._model_list[i].evaluate(x_sub, u_sub, dt) - - return x - - def jacobian( - self, - x: CompositeState, - u: CompositeInput, - dt: float, - ) -> np.ndarray: - jac = [] - for i, x_sub in enumerate(x.value): - if self._shared_input: - u_sub = u - else: - u_sub = u.input_list[i] - jac.append(self._model_list[i].jacobian(x_sub, u_sub, dt)) - - return block_diag(*jac) - - def covariance( - self, - x: CompositeState, - u: CompositeInput, - dt: float, - ) -> np.ndarray: - cov = [] - for i, x_sub in enumerate(x.value): - if self._shared_input: - u_sub = u - else: - u_sub = u.input_list[i] - cov.append(self._model_list[i].covariance(x_sub, u_sub, dt)) - - return block_diag(*cov) - - -class CompositeMeasurementModel(MeasurementModel): - """ - Wrapper for a standard measurement model that assigns the model to a specific - substate (referenced by `state_id`) inside a CompositeState. - """ - - def __init__(self, model: MeasurementModel, state_id): - self.model = model - self.state_id = state_id - - def __repr__(self): - return f"{self.model}(of substate {self.state_id})" - - def evaluate(self, x: CompositeState) -> np.ndarray: - return self.model.evaluate(x.get_state_by_id(self.state_id)) - - def jacobian(self, x: CompositeState) -> np.ndarray: - x_sub = x.get_state_by_id(self.state_id) - jac_sub = self.model.jacobian(x_sub) - jac = np.zeros((jac_sub.shape[0], x.dof)) - slc = x.get_slice_by_id(self.state_id) - jac[:, slc] = jac_sub - return jac - - def covariance(self, x: CompositeState) -> np.ndarray: - x_sub = x.get_state_by_id(self.state_id) - return self.model.covariance(x_sub) - - -class CompositeMeasurement(Measurement): - def __init__(self, y: Measurement, state_id: Any): - """ - Converts a standard Measurement into a CompositeMeasurement, which - replaces the model with a CompositeMeasurementModel. - - Parameters - ---------- - y : Measurement - Measurement to be converted. - state_id : Any - ID of the state that the measurement will be assigned to, - as per the CompositeMeasurementModel. - """ - model = CompositeMeasurementModel(y.model, state_id) - super().__init__( - value=y.value, stamp=y.stamp, model=model, state_id=y.state_id - ) - - class RangePointToAnchor(MeasurementModel): """ Range measurement from a point state to an anchor (which is also another point). I.e., the state is a vector with the first ``dim`` elements being - the position of the point. This model is of the form + the position of the point. This model is of the form .. math:: y = ||\mathbf{r}_a - \mathbf{r}|| @@ -626,10 +391,10 @@ class RangePointToAnchor(MeasurementModel): """ def __init__(self, anchor_position: List[float], R: float): - """ + """ Parameters ---------- - + anchor_position : np.ndarray or List[float] Position of anchor. R : float @@ -847,9 +612,7 @@ class RangePoseToPose(MeasurementModel): # TODO. tag_body_positions should be optional. argh but this will be # a breaking change since the argument order needs to be different. - def __init__( - self, tag_body_position1, tag_body_position2, state_id1, state_id2, R - ): + def __init__(self, tag_body_position1, tag_body_position2, state_id1, state_id2, R): self.tag_body_position1 = np.array(tag_body_position1).flatten() self.tag_body_position2 = np.array(tag_body_position2).flatten() self.state_id1 = state_id1 @@ -865,9 +628,7 @@ def evaluate(self, x: CompositeState) -> np.ndarray: C_a2 = x2.attitude r_t1_1 = self.tag_body_position1.reshape((-1, 1)) r_t2_2 = self.tag_body_position2.reshape((-1, 1)) - r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - ( - C_a2 @ r_t2_2 + r_2w_a - ) + r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a) return np.array(np.linalg.norm(r_t1t2_a.flatten())) def jacobian(self, x: CompositeState) -> np.ndarray: @@ -879,18 +640,16 @@ def jacobian(self, x: CompositeState) -> np.ndarray: C_a2 = x2.attitude r_t1_1 = self.tag_body_position1.reshape((-1, 1)) r_t2_2 = self.tag_body_position2.reshape((-1, 1)) - r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - ( - C_a2 @ r_t2_2 + r_2w_a - ) + r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a) if C_a1.shape == (2, 2): att_group = SO2 elif C_a1.shape == (3, 3): att_group = SO3 - rho: np.ndarray = ( - r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten()) - ).reshape((-1, 1)) + rho: np.ndarray = (r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten())).reshape( + (-1, 1) + ) if x1.direction == "right": jac1 = x1.jacobian_from_blocks( @@ -914,9 +673,7 @@ def jacobian(self, x: CompositeState) -> np.ndarray: position=-rho.T @ np.identity(r_t2_2.size), ) - return x.jacobian_from_blocks( - {self.state_id1: jac1, self.state_id2: jac2} - ) + return x.jacobian_from_blocks({self.state_id1: jac1, self.state_id2: jac2}) def covariance(self, x: CompositeState) -> np.ndarray: return self._R @@ -997,11 +754,14 @@ def covariance(self, x: MatrixLieGroupState) -> np.ndarray: else: return self.R + class GlobalPosition(AbsolutePosition): - """ + """ This class is deprecated. Use ``AbsolutePosition`` instead. """ - pass # alias + + pass # alias + class AbsoluteVelocity(MeasurementModel): """ @@ -1081,9 +841,7 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks( - position=x.attitude[2, :].reshape((1, -1)) - ) + return x.jacobian_from_blocks(position=x.attitude[2, :].reshape((1, -1))) elif x.direction == "left": return x.jacobian_from_blocks( attitude=SO3.odot(x.position)[2, :].reshape((1, -1)), @@ -1127,13 +885,9 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks( - attitude=-SO3.odot(x.attitude.T @ self._g_a) - ) + return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._g_a)) elif x.direction == "left": - return x.jacobian_from_blocks( - attitude=x.attitude.T @ -SO3.odot(self._g_a) - ) + return x.jacobian_from_blocks(attitude=x.attitude.T @ -SO3.odot(self._g_a)) def covariance(self, x: MatrixLieGroupState) -> np.ndarray: if np.isscalar(self.R): @@ -1176,13 +930,9 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks( - attitude=-SO3.odot(x.attitude.T @ self._m_a) - ) + return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._m_a)) elif x.direction == "left": - return x.jacobian_from_blocks( - attitude=-x.attitude.T @ SO3.odot(self._m_a) - ) + return x.jacobian_from_blocks(attitude=-x.attitude.T @ SO3.odot(self._m_a)) def covariance(self, x: MatrixLieGroupState) -> np.ndarray: if np.isscalar(self.R): @@ -1192,9 +942,7 @@ def covariance(self, x: MatrixLieGroupState) -> np.ndarray: class _InvariantInnovation(MeasurementModel): - def __init__( - self, y: np.ndarray, model: MeasurementModel, direction="right" - ): + def __init__(self, y: np.ndarray, model: MeasurementModel, direction="right"): self.measurement_model = model self.y = y.ravel() self.direction = direction @@ -1245,9 +993,7 @@ def _compute_direction(self, x: MatrixLieGroupState) -> str: elif x.direction == "right": direction = "left" else: - raise ValueError( - "Invalid direction. Must be 'left', 'right' or 'auto'" - ) + raise ValueError("Invalid direction. Must be 'left', 'right' or 'auto'") return direction diff --git a/navlie/lib/preintegration.py b/navlie/lib/preintegration.py index 3afe5c88..ce601ce7 100644 --- a/navlie/lib/preintegration.py +++ b/navlie/lib/preintegration.py @@ -135,9 +135,7 @@ def accel_bias(self): @property def value(self): - db = self.new_bias.reshape((-1, 1)) - self.original_bias.reshape( - (-1, 1) - ) + db = self.new_bias.reshape((-1, 1)) - self.original_bias.reshape((-1, 1)) return self.original_value @ SE23.Exp(self.bias_jacobian @ db) def increment(self, u: IMU, dt: float): @@ -172,9 +170,7 @@ def increment(self, u: IMU, dt: float): A_full = A_full[0:9, 9:9] L_full = L_full[0:9, 0:6] - self.covariance = ( - A_full @ self.covariance @ A_full.T + L_full @ Q @ L_full.T - ) + self.covariance = A_full @ self.covariance @ A_full.T + L_full @ Q @ L_full.T self.bias_jacobian = A @ self.bias_jacobian - L self.symmetrize() @@ -299,9 +295,7 @@ def jacobian(self, x: IMUState, rmi: IMUIncrement, dt=None) -> np.ndarray: dt = rmi.stamps[1] - rmi.stamps[0] DG = G_matrix(self.gravity, dt) DU = rmi.value - J = SE23.right_jacobian( - rmi.bias_jacobian @ (x.bias - rmi.original_bias) - ) + J = SE23.right_jacobian(rmi.bias_jacobian @ (x.bias - rmi.original_bias)) A = np.identity(15) if x.direction == "right": A[0:9, 0:9] = adjoint_IE3(inverse_IE3(DU)) @@ -395,9 +389,7 @@ def increment(self, u: VectorInput, dt): # Increment the covariance A = self.group.adjoint(self.group.inverse(U)) L = dt * self.group.left_jacobian(-unbiased_velocity * dt) - self.covariance = ( - A @ self.covariance @ A.T + L @ self.input_covariance @ L.T - ) + self.covariance = A @ self.covariance @ A.T + L @ self.input_covariance @ L.T # Increment the bias jacobian self.bias_jacobian = A @ self.bias_jacobian + L @@ -864,9 +856,7 @@ class PreintegratedLinearModel(ProcessModel): def __init__(self): pass - def evaluate( - self, x: VectorState, rmi: LinearIncrement, dt=None - ) -> VectorState: + def evaluate(self, x: VectorState, rmi: LinearIncrement, dt=None) -> VectorState: x = x.copy() A_ij = rmi.value[0] Du_ij = rmi.value[1] @@ -879,16 +869,12 @@ def evaluate( x_j = A_ij @ x_i + Du_ij if rmi.original_bias is not None: - x_j = np.vstack( - (x_j, x.value[-rmi.original_bias.size :].reshape((-1, 1))) - ) + x_j = np.vstack((x_j, x.value[-rmi.original_bias.size :].reshape((-1, 1)))) x.value = x_j.ravel() return x - def jacobian( - self, x: VectorState, rmi: LinearIncrement, dt=None - ) -> np.ndarray: + def jacobian(self, x: VectorState, rmi: LinearIncrement, dt=None) -> np.ndarray: if rmi.original_bias is not None: A_ij = rmi.value[0] B_ij = rmi.bias_jacobian @@ -902,7 +888,5 @@ def jacobian( return A - def covariance( - self, x: VectorState, rmi: LinearIncrement, dt=None - ) -> np.ndarray: + def covariance(self, x: VectorState, rmi: LinearIncrement, dt=None) -> np.ndarray: return rmi.covariance diff --git a/navlie/lib/states.py b/navlie/lib/states.py index 57cf31b9..392910db 100644 --- a/navlie/lib/states.py +++ b/navlie/lib/states.py @@ -2,7 +2,7 @@ from pymlg.numpy.base import MatrixLieGroup import numpy as np from navlie.types import State, Input -from typing import Any, List +from typing import Any try: # We do not want to make ROS a hard dependency, so we import it only if @@ -15,6 +15,9 @@ raise +from navlie.composite import CompositeState # For backwards compatibility + + class VectorState(State): """ A standard vector-based state, with value represented by a 1D numpy array. @@ -185,25 +188,19 @@ def dot(self, other: "MatrixLieGroupState") -> "MatrixLieGroupState": @property def attitude(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have attitude property".format( - self.__class__.__name__ - ) + "{0} does not have attitude property".format(self.__class__.__name__) ) @property def position(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have position property".format( - self.__class__.__name__ - ) + "{0} does not have position property".format(self.__class__.__name__) ) @property def velocity(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have velocity property".format( - self.__class__.__name__ - ) + "{0} does not have velocity property".format(self.__class__.__name__) ) def jacobian_from_blocks(self, **kwargs) -> np.ndarray: @@ -211,15 +208,16 @@ def jacobian_from_blocks(self, **kwargs) -> np.ndarray: class SO2State(MatrixLieGroupState): - """ + """ A state object for rotations in 2D. The value of this state is stored as a 2x2 numpy array representing a direct element of the SO2 group. I.e., - + .. math:: - \mathbf{C} \in \mathbb{R}^{2 \times 2}, \quad + \mathbf{C} \in \mathbb{R}^{2 \times 2}, \quad \mathbf{C}^T \mathbf{C} = \mathbf{I}, \quad \det(\mathbf{C}) = \mathbf{1} - + """ + group = SO2 def __init__( @@ -250,16 +248,17 @@ def random(stamp: float = None, state_id=None, direction="right"): class SO3State(MatrixLieGroupState): - """ + """ A state object for rotations in 3D. The value of this state is stored as a 3x3 numpy array representing a direct element of the SO3 group. I.e., - + .. math:: - + \mathbf{C} \in \mathbb{R}^{3 \times 3}, \quad \mathbf{C}^T \mathbf{C} = \mathbf{I}, \quad \det(\mathbf{C}) = \mathbf{1} - + """ + group = SO3 def __init__( @@ -351,6 +350,7 @@ class SE2State(MatrixLieGroupState): """ + group = SE2 def __init__( @@ -387,9 +387,7 @@ def pose(self, T): self.value = T @staticmethod - def jacobian_from_blocks( - attitude: np.ndarray = None, position: np.ndarray = None - ): + def jacobian_from_blocks(attitude: np.ndarray = None, position: np.ndarray = None): for jac in [attitude, position]: if jac is not None: dim = jac.shape[0] @@ -422,7 +420,8 @@ class SE3State(MatrixLieGroupState): \end{bmatrix}, \quad \mathbf{C} \in SO(3), \quad \mathbf{r} \in \mathbb{R}^3. - """ + """ + group = SE3 def __init__( @@ -451,9 +450,7 @@ def position(self, r): self.value[0:3, 3] = r @staticmethod - def jacobian_from_blocks( - attitude: np.ndarray = None, position: np.ndarray = None - ): + def jacobian_from_blocks(attitude: np.ndarray = None, position: np.ndarray = None): for jac in [attitude, position]: if jac is not None: dim = jac.shape[0] @@ -629,10 +626,11 @@ class SL3State(MatrixLieGroupState): the SL3 group. I.e., .. math:: - + \mathbf{C} \in SL(3), \quad \mathbf{C} \in \mathbb{R}^{3 \times 3}, \quad \det(\mathbf{C}) = \mathbf{1}. - """ + """ + group = SL3 def __init__( @@ -645,347 +643,6 @@ def __init__( super().__init__(value, self.group, stamp, state_id, direction) -class CompositeState(State): - """ - A "composite" state object intended to hold a list of State objects as a - single conceptual "state". The intended use is to hold a list of states - as a single state at a specific time, of potentially different types, - and this class will take care of defining the appropriate operations on - the composite state such as the ``plus`` and ``minus`` methods, as well - as the ``plus_jacobian`` and ``minus_jacobian`` methods. - - Each state in the provided list has an index (the index in the list), as - well as a state_id, which is found as an attribute in the corresponding State - object. - - It is possible to access sub-states in the composite states both by index - and by ID. - """ - - def __init__( - self, state_list: List[State], stamp: float = None, state_id=None - ): - """ - Parameters - ---------- - state_list: List[State] - List of State that forms this composite state - stamp: float, optional - Timestamp of the composite state. This can technically be different - from the timestamps of the substates. - state_id: Any, optional - State ID of the composite state. This can be different from the - state IDs of the substates. - """ - if isinstance(state_list, tuple): - state_list = list(state_list) - - #:List[State]: The substates are the CompositeState's value. - self.value = state_list - - self.stamp = stamp - self.state_id = state_id - - def __getstate__(self): - """ - Get the state of the object for pickling. - """ - # When using __slots__ the pickle module expects a tuple from __getstate__. - # See https://stackoverflow.com/questions/1939058/simple-example-of-use-of-setstate-and-getstate/41754104#41754104 - return ( - None, - { - "value": self.value, - "stamp": self.stamp, - "state_id": self.state_id, - }, - ) - - def __setstate__(self, attributes): - """ - Set the state of the object for unpickling. - """ - # When using __slots__ the pickle module sends a tuple for __setstate__. - # See https://stackoverflow.com/questions/1939058/simple-example-of-use-of-setstate-and-getstate/41754104#41754104 - - attributes = attributes[1] - self.value = attributes["value"] - self.stamp = attributes["stamp"] - self.state_id = attributes["state_id"] - - @property - def dof(self): - return sum([x.dof for x in self.value]) - - def get_index_by_id(self, state_id): - """ - Get index of a particular state_id in the list of states. - """ - return [x.state_id for x in self.value].index(state_id) - - def get_slices(self) -> List[slice]: - """ - Get slices for each state in the list of states. - """ - slices = [] - counter = 0 - for state in self.value: - slices.append(slice(counter, counter + state.dof)) - counter += state.dof - - return slices - - def add_state(self, state: State, stamp: float = None, state_id=None): - """Adds a state and it's corresponding slice to the composite state.""" - self.value.append(state) - - def remove_state_by_id(self, state_id): - """Removes a given state by ID.""" - idx = self.get_index_by_id(state_id) - self.value.pop(idx) - - def get_slice_by_id(self, state_id, slices=None): - """ - Get slice of a particular state_id in the list of states. - """ - - if slices is None: - slices = self.get_slices() - - idx = self.get_index_by_id(state_id) - return slices[idx] - - def get_matrix_block_by_ids( - self, mat: np.ndarray, state_id_1: Any, state_id_2: Any = None - ) -> np.ndarray: - """Gets the portion of a matrix corresponding to two states. - - This function is useful when extract specific blocks of a covariance - matrix, for example. - - Parameters - ---------- - mat : np.ndarray - N x N matrix - state_id_1 : Any - State ID of state 1. - state_id_2 : Any, optional - State ID of state 2. If None, state_id_2 is set to state_id_1. - - Returns - ------- - np.ndarray - Subblock of mat corrsponding to - slices of state_id_1 and state_id_2. - """ - - if state_id_2 is None: - state_id_2 = state_id_1 - - slice_1 = self.get_slice_by_id(state_id_1) - slice_2 = self.get_slice_by_id(state_id_2) - - return mat[slice_1, slice_2] - - def set_matrix_block_by_ids( - self, - new_mat_block: np.ndarray, - mat: np.ndarray, - state_id_1: Any, - state_id_2: Any = None, - ) -> np.ndarray: - """Sets the portion of the covariance block corresponding to two states. - - Parameters - ---------- - new_mat_block : np.ndarray - A subblock to be entered into mat. - mat : np.ndarray - Full matrix. - state_id_1 : Any - State ID of state 1. - state_id_2 : Any, optional - State ID of state 2. If None, state_id_2 is set to state_id_1. - - Returns - ------- - np.ndarray - mat with updated subblock. - """ - - if state_id_2 is None: - state_id_2 = state_id_1 - - slice_1 = self.get_slice_by_id(state_id_1) - slice_2 = self.get_slice_by_id(state_id_2) - - mat[slice_1, slice_2] = new_mat_block - return mat - - def get_value_by_id(self, state_id) -> Any: - """ - Get state value by id. - """ - idx = self.get_index_by_id(state_id) - return self.value[idx].value - - def get_state_by_id(self, state_id) -> State: - """ - Get state object by id. - """ - idx = self.get_index_by_id(state_id) - return self.value[idx] - - def get_dof_by_id(self, state_id) -> int: - """ - Get degrees of freedom of sub-state by id. - """ - idx = self.get_index_by_id(state_id) - return self.value[idx].dof - - def get_stamp_by_id(self, state_id) -> float: - """ - Get timestamp of sub-state by id. - """ - idx = self.get_index_by_id(state_id) - return self.value[idx].stamp - - def set_stamp_by_id(self, stamp: float, state_id): - """ - Set the timestamp of a sub-state by id. - """ - idx = self.get_index_by_id(state_id) - self.value[idx].stamp = stamp - - def set_state_by_id(self, state: State, state_id): - """ - Set the whole sub-state by id. - """ - idx = self.get_index_by_id(state_id) - self.value[idx] = state - - def set_value_by_id(self, value: Any, state_id: Any): - """ - Set the value of a sub-state by id. - """ - idx = self.get_index_by_id(state_id) - self.value[idx].value = value - - def set_stamp_for_all(self, stamp: float): - """ - Set the timestamp of all substates. - """ - for state in self.value: - state.stamp = stamp - - def to_list(self): - """ - Converts the CompositeState object back into a list of states. - """ - return self.value - - def copy(self) -> "CompositeState": - """ - Returns a new composite state object where the state values have also - been copied. - """ - return self.__class__( - [state.copy() for state in self.value], self.stamp, self.state_id - ) - - def plus(self, dx, new_stamp: float = None) -> "CompositeState": - """ - Updates the value of each sub-state given a dx. Interally parses - the dx vector. - """ - new = self.copy() - for i, state in enumerate(new.value): - new.value[i] = state.plus(dx[: state.dof]) - dx = dx[state.dof :] - - if new_stamp is not None: - new.set_stamp_for_all(new_stamp) - - return new - - def minus(self, x: "CompositeState") -> np.ndarray: - dx = [] - for i, v in enumerate(x.value): - dx.append( - self.value[i].minus(x.value[i]).reshape((self.value[i].dof,)) - ) - - return np.concatenate(dx).reshape((-1, 1)) - - def plus_by_id( - self, dx, state_id: int, new_stamp: float = None - ) -> "CompositeState": - """ - Updates a specific sub-state. - """ - new = self.copy() - idx = new.get_index_by_id(state_id) - new.value[idx].plus(dx) - if new_stamp is not None: - new.set_stamp_by_id(new_stamp, state_id) - - return new - - def jacobian_from_blocks(self, block_dict: dict): - """ - Returns the jacobian of the entire composite state given jacobians - associated with some of the substates. These are provided as a dictionary - with the the keys being the substate IDs. - """ - block: np.ndarray = list(block_dict.values())[0] - m = block.shape[0] # Dimension of "y" value - jac = np.zeros((m, self.dof)) - slices = self.get_slices() - for state_id, block in block_dict.items(): - slc = self.get_slice_by_id(state_id, slices) - jac[:, slc] = block - - return jac - - def plus_jacobian(self, dx: np.ndarray) -> np.ndarray: - dof = self.dof - jac = np.zeros((dof, dof)) - counter = 0 - for state in self.value: - jac[ - counter : counter + state.dof, - counter : counter + state.dof, - ] = state.plus_jacobian(dx[: state.dof]) - dx = dx[state.dof :] - counter += state.dof - - return jac - - def minus_jacobian(self, x: "CompositeState") -> np.ndarray: - dof = self.dof - jac = np.zeros((dof, dof)) - counter = 0 - for i, state in enumerate(self.value): - jac[ - counter : counter + state.dof, - counter : counter + state.dof, - ] = state.minus_jacobian(x.value[i]) - counter += state.dof - - return jac - - def __repr__(self): - substate_line_list = [] - for v in self.value: - substate_line_list.extend(v.__repr__().split("\n")) - substates_str = "\n".join([" " + s for s in substate_line_list]) - s = [ - f"{self.__class__.__name__}(stamp={self.stamp}, state_id={self.state_id}) with substates:", - substates_str, - ] - return "\n".join(s) - - class VectorInput(Input): """ A standard vector-based input, with value represented by a 1D numpy array. @@ -1000,7 +657,7 @@ def __init__( state_id: Any = None, covariance=None, ): - """ + """ Parameters ---------- value : np.ndarray diff --git a/navlie/types.py b/navlie/types.py index 9774f3ff..5139fefb 100644 --- a/navlie/types.py +++ b/navlie/types.py @@ -51,8 +51,8 @@ class State(ABC): - a value of some sort; - a certain number of degrees of freedom (dof); - - ``plus`` and ``minus`` methods that generalize addition and subtracting to - to this object. + - ``plus`` and ``minus`` methods that generalize addition and subtracting to + to this object. Optionally, it is often useful to assign a timestamp (``stamp``) and a label (``state_id``) to differentiate state instances from others. @@ -82,15 +82,11 @@ class State(ABC): __slots__ = ["value", "dof", "stamp", "state_id"] - def __init__( - self, value: Any, dof: int, stamp: float = None, state_id=None - ): + def __init__(self, value: Any, dof: int, stamp: float = None, state_id=None): self.value = value #:Any: State value self.dof = dof #:int: Degree of freedom of the state self.stamp = stamp #:float: Timestamp - self.state_id = ( - state_id #:Any: Some identifier associated with the state - ) + self.state_id = state_id #:Any: Some identifier associated with the state @abstractmethod def plus(self, dx: np.ndarray) -> "State": @@ -122,8 +118,8 @@ def plus_jacobian(self, dx: np.ndarray) -> np.ndarray: .. math:: \mathbf{J} = \\frac{D (\mathcal{X} \oplus \delta \mathbf{x})}{D \delta \mathbf{x}} - - + + For Lie groups, this is known as the *group Jacobian*. """ return self.plus_jacobian_fd(dx) @@ -146,11 +142,11 @@ def plus_jacobian_fd(self, dx, step_size=1e-8) -> np.ndarray: def minus_jacobian(self, x: "State") -> np.ndarray: """ Jacobian of the ``minus`` operator with respect to self. - + .. math:: \mathbf{J} = \\frac{D (\mathcal{Y} \ominus \mathcal{X})}{D \mathcal{Y}} - + That is, if ``dx = y.minus(x)`` then this is the Jacobian of ``dx`` with respect to ``y``. For Lie groups, this is the inverse of the *group Jacobian* evaluated at ``dx = x1.minus(x2)``. @@ -274,7 +270,7 @@ class ProcessModel(ABC): of either two ways. **1. Specifying the covariance matrix directly:** - + The first way is to specify the :math:`\mathbf{Q}_k` covariance matrix directly by overriding the ``covariance`` method. This covariance matrix represents the distribution of process model errors directly. @@ -429,7 +425,7 @@ def sqrt_information(self, x: State, u: Input, dt: float) -> np.ndarray: def input_covariance(self, x: State, u: Input, dt: float) -> np.ndarray: """ - Covariance matrix of additive noise *on the input*. + Covariance matrix of additive noise *on the input*. Parameters ---------- @@ -511,7 +507,7 @@ def minus(self, y_check: np.ndarray) -> np.ndarray: class StateWithCovariance: """ - A data container containing a ``State`` object and a covariance array. + A data container containing a ``State`` object and a covariance array. This class can be used as-is without inheritance. """ @@ -539,9 +535,7 @@ def __init__(self, state: State, covariance: np.ndarray): raise ValueError("covariance must be an n x n array.") if covariance.shape[0] != state.dof: - raise ValueError( - "Covariance matrix does not correspond with state DOF." - ) + raise ValueError("Covariance matrix does not correspond with state DOF.") #:navlie.types.State: state object self.state = state diff --git a/navlie/utils.py b/navlie/utils.py index 4e8b1308..8ffd9161 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -94,12 +94,12 @@ class GaussianResultList: results[:, 3:] # returns all degrees of freedom except the first three This can be very useful if you want to examine the nees or rmse of just some - states, or if you want to plot the error of just some states. For example, + states, or if you want to plot the error of just some states. For example, if you have an SE3State, where the first 3 degrees of freedom are attitude, and the last 3 are position, you can plot only the attitude error with .. code-block:: python - + nav.plot_error(results[:, 0:3]) and likewise get only the attitude NEES with ``results[:, 0:3].nees``. @@ -138,13 +138,9 @@ def __init__(self, result_list: List[GaussianResult]): #:numpy.ndarray with shape (N,): numpy array of State objects self.state: List[State] = np.array([r.state for r in result_list]) #:numpy.ndarray with shape (N,): numpy array of true State objects - self.state_true: List[State] = np.array( - [r.state_true for r in result_list] - ) + self.state_true: List[State] = np.array([r.state_true for r in result_list]) #:numpy.ndarray with shape (N,dof,dof): covariance - self.covariance: np.ndarray = np.array( - [r.covariance for r in result_list] - ) + self.covariance: np.ndarray = np.array([r.covariance for r in result_list]) #:numpy.ndarray with shape (N, dof): error throughout trajectory self.error = np.array([r.error for r in result_list]) #:numpy.ndarray with shape (N,): EES throughout trajectory @@ -207,9 +203,7 @@ def __getitem__(self, key): out.dof = out.error.shape[1] * np.ones_like(out.stamp) out.md = np.sqrt(out.nees) - out.three_sigma = 3 * np.sqrt( - np.diagonal(out.covariance, axis1=1, axis2=2) - ) + out.three_sigma = 3 * np.sqrt(np.diagonal(out.covariance, axis1=1, axis2=2)) out.rmse = np.sqrt(out.ees / out.dof) out.value = self.value[key1] out.value_true = self.value_true[key1] @@ -356,9 +350,7 @@ def __init__(self, trial_results: List[GaussianResultList]): self.ees = self.average_ees #:numpy.ndarray with shape (N,dof): root-mean-squared error of each component self.rmse: np.ndarray = np.sqrt( - np.average( - np.power(np.array([t.error for t in trial_results]), 2), axis=0 - ) + np.average(np.power(np.array([t.error for t in trial_results]), 2), axis=0) ) #:numpy.ndarray with shape (N,): Total RMSE, this can be meaningless if units differ in a state self.total_rmse: np.ndarray = np.sqrt(self.average_ees) @@ -780,9 +772,7 @@ def plot_meas( axs[i].scatter( y_stamps, y_meas[:, i], color="b", alpha=0.7, s=2, label="Measured" ) - axs[i].plot( - y_stamps, y_true[:, i], color="r", alpha=1, label="Modelled" - ) + axs[i].plot(y_stamps, y_true[:, i], color="r", alpha=1, label="Modelled") axs[i].fill_between( y_stamps, y_true[:, i] + three_sigma[:, i], @@ -885,7 +875,7 @@ def plot_poses( # TODO. handle 2D case if isinstance(poses, GaussianResultList): poses = poses.state - + if isinstance(poses, StateWithCovariance): poses = [poses.state] @@ -1360,8 +1350,7 @@ def fun(T: SE23State): else: raise ValueError( - f"Unknown method '{method}'. " - "Must be 'forward', 'central' or 'cs" + f"Unknown method '{method}'. " "Must be 'forward', 'central' or 'cs" ) return jac_fd diff --git a/tests/integration/test_batch_noiseless.py b/tests/integration/test_batch_noiseless.py index 2b4003b1..81f90bdd 100644 --- a/tests/integration/test_batch_noiseless.py +++ b/tests/integration/test_batch_noiseless.py @@ -110,9 +110,7 @@ def test_noiseless_batch( # Run batch estimator = BatchEstimator(max_iters=20) - estimate_list = estimator.solve( - x0, P0, input_list, meas_list, process_model - ) + estimate_list = estimator.solve(x0, P0, input_list, meas_list, process_model) results = GaussianResultList( [ GaussianResult(estimate_list[i], state_true[i]) diff --git a/tests/integration/test_filter_ekf_noiseless.py b/tests/integration/test_filter_ekf_noiseless.py index 60cfa1c0..c0706d33 100644 --- a/tests/integration/test_filter_ekf_noiseless.py +++ b/tests/integration/test_filter_ekf_noiseless.py @@ -58,9 +58,7 @@ def make_range_models_iterated_ekf(R): def make_filter_trial_prediction_noiseless(dg, x0_true, P0, t_max, ekf): def ekf_trial(trial_number: int) -> List[GaussianResult]: np.random.seed(trial_number) - state_true, input_data, meas_data = dg.generate( - x0_true, 0, t_max, noise=False - ) + state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=False) x0_check = x0_true.copy() estimate_list = run_filter(ekf, x0_check, P0, input_data, meas_data) diff --git a/tests/integration/test_interacting_multiple_models.py b/tests/integration/test_interacting_multiple_models.py index 34a7bfc7..899c3caf 100644 --- a/tests/integration/test_interacting_multiple_models.py +++ b/tests/integration/test_interacting_multiple_models.py @@ -154,8 +154,7 @@ def c_profile(t): np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) ), BodyFrameVelocity( - 9 - * np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + 9 * np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) ), ], np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]), diff --git a/tests/integration/test_iterated_ekf.py b/tests/integration/test_iterated_ekf.py index 44e6bc02..c168235e 100644 --- a/tests/integration/test_iterated_ekf.py +++ b/tests/integration/test_iterated_ekf.py @@ -31,9 +31,7 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [ - GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) - ], + "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], "measurement_freq": 10, "filter": IteratedKalmanFilter, } @@ -48,9 +46,7 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [ - GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) - ], + "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], "measurement_freq": 10, "filter": IteratedKalmanFilter, } @@ -65,9 +61,7 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [ - GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) - ], + "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], "measurement_freq": 10, "filter": ExtendedKalmanFilter, } diff --git a/tests/unit/test_composite.py b/tests/unit/test_composite.py index 8c654ebb..5b5353df 100644 --- a/tests/unit/test_composite.py +++ b/tests/unit/test_composite.py @@ -6,12 +6,12 @@ VectorInput, SE2State, VectorState, - CompositeState, ) from pymlg import SE2 import numpy as np import pickle import os +from navlie import CompositeState def test_composite_process_model(): @@ -19,9 +19,7 @@ def test_composite_process_model(): T13 = SE2State(SE2.Exp([-0.5, 1, 1]), stamp=0.0, state_id=3) Q = np.diag([0.1**2, 0.1**2, 0.001**2]) x0 = CompositeState([T12, T13]) - process_model = CompositeProcessModel( - [BodyFrameVelocity(Q), BodyFrameVelocity(Q)] - ) + process_model = CompositeProcessModel([BodyFrameVelocity(Q), BodyFrameVelocity(Q)]) u = CompositeInput( [ VectorInput(np.array([0.3, 1, 0]), 1), diff --git a/tests/unit/test_meas_models.py b/tests/unit/test_meas_models.py index c53f9e99..b321cd70 100644 --- a/tests/unit/test_meas_models.py +++ b/tests/unit/test_meas_models.py @@ -1,13 +1,10 @@ -from navlie.lib.states import ( +from navlie.lib import ( MatrixLieGroupState, SO3State, SE2State, SE3State, SE23State, - CompositeState, -) -from navlie.lib.imu import IMUState -from navlie.lib.models import ( + IMUState, Altitude, GlobalPosition, InvariantMeasurement, @@ -18,7 +15,7 @@ Gravitometer, AbsoluteVelocity, ) -from navlie.types import Measurement, MeasurementModel +from navlie import Measurement, MeasurementModel, CompositeState from pymlg import SO3, SE3, SE2, SE3, SE23 import numpy as np import pytest @@ -111,9 +108,7 @@ def test_range_pose_to_pose_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_global_position_se2(direction): - x = SE2State( - SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction - ) + x = SE2State(SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction) model = GlobalPosition(np.identity(3)) _jacobian_test(x, model, atol=1e-5) @@ -188,9 +183,7 @@ def test_altitude_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_gravity_so3(direction): - x = SO3State( - SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction - ) + x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction) model = Gravitometer(np.identity(3)) _jacobian_test(x, model) @@ -218,9 +211,7 @@ def test_gravity_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_magnetometer_so3(direction): - x = SO3State( - SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction - ) + x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction) model = Magnetometer(np.identity(3)) _jacobian_test(x, model) @@ -247,9 +238,7 @@ def test_magnetometer_se23(direction): def test_invariant_magnetometer_so3(): - x = SO3State( - SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left" - ) + x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left") b = [1, 0, 0] y = np.array(b) @@ -261,9 +250,7 @@ def test_invariant_magnetometer_so3(): def test_invariant_magnetometer_se3(): - x = SE3State( - SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left" - ) + x = SE3State(SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left") b = [1, 0, 0] y = np.array(b) diff --git a/tests/unit/test_states.py b/tests/unit/test_states.py index 1177bc12..cee4cf77 100644 --- a/tests/unit/test_states.py +++ b/tests/unit/test_states.py @@ -64,9 +64,7 @@ def test_minus_jacobian(s: str): assert np.allclose(jac, jac_test, atol=1e-5) -@pytest.mark.parametrize( - "s", ["so2", "so3", "se2", "se3", "se23", "sl3", "mlg"] -) +@pytest.mark.parametrize("s", ["so2", "so3", "se2", "se3", "se23", "sl3", "mlg"]) def test_mlg_dot(s: str): x = sample_states[s] dx = np.random.randn(x.dof) diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 81781709..690ac139 100644 --- a/tests/unit/test_utils.py +++ b/tests/unit/test_utils.py @@ -60,6 +60,7 @@ def test_gaussian_result_slicing(): assert np.all(grl_test.error == grl.error[:, slc]) assert np.allclose(grl_test.nees, nees_test) + def test_gaussian_result_list_slicing_equivalency(): # Construct a dummy GaussianResultList x_true = [SE23State(SE23.random(), stamp=i) for i in range(10)] @@ -67,16 +68,18 @@ def test_gaussian_result_list_slicing_equivalency(): cov = [(i + 1) * np.eye(9) for i in range(10)] x_cov = [StateWithCovariance(x, c) for x, c in zip(x_hat, cov)] gr = [GaussianResult(x, t) for x, t in zip(x_cov, x_true)] - results = GaussianResultList(gr) - - results[0:10] # returns the first 10 time steps - results[:, 0] # returns the first degree of freedom - results[0:10, 0] # returns the first degree of freedom for the first 10 time steps - results[0:10, [0, 1]] # returns the first two degrees of freedom for the first 10 time steps - results[:, 3:] # returns the all but the first three degrees of freedom - + results = GaussianResultList(gr) + + results[0:10] # returns the first 10 time steps + results[:, 0] # returns the first degree of freedom + results[0:10, 0] # returns the first degree of freedom for the first 10 time steps + results[ + 0:10, [0, 1] + ] # returns the first two degrees of freedom for the first 10 time steps + results[:, 3:] # returns the all but the first three degrees of freedom + assert np.all(results[0:10].error == results[0:10, :].error) - assert np.all(results[:, [0,1,2]].error == results[:, 0:3].error) + assert np.all(results[:, [0, 1, 2]].error == results[:, 0:3].error) assert np.all(results[:, 3:].error == results[:, 3:9].error) diff --git a/tests/unit/test_van_loan.py b/tests/unit/test_van_loan.py index eec3e7f8..84ac9a0b 100644 --- a/tests/unit/test_van_loan.py +++ b/tests/unit/test_van_loan.py @@ -30,9 +30,7 @@ def test_van_loan_double_integrator(): # Compare to analytical solution A_d_test = np.array([[1, dt], [0, 1]]) - Q_d_test = np.array( - [[1 / 3 * dt**3, 1 / 2 * dt**2], [1 / 2 * dt**2, dt]] - ) + Q_d_test = np.array([[1 / 3 * dt**3, 1 / 2 * dt**2], [1 / 2 * dt**2, dt]]) assert np.allclose(A_d, A_d_test) assert np.allclose(Q_d, Q_d_test) From f679054b833877ba1fccb209456ab7c70fa8bcac Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Mon, 4 Dec 2023 20:59:01 -0800 Subject: [PATCH 2/4] fix links in some docs --- navlie/lib/imu.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navlie/lib/imu.py b/navlie/lib/imu.py index d2798d6b..ec11fdd6 100644 --- a/navlie/lib/imu.py +++ b/navlie/lib/imu.py @@ -4,7 +4,7 @@ Note that the treatment of IMU kinematics in this module is different from typical treatments in the sense that it is done directly on :math:`SE_2(3)`. A PDF describing the detailed derivations can be found at the -`following link. `_ +`following link. `_ """ @@ -341,7 +341,7 @@ def N_matrix(phi_vec: np.ndarray): def M_matrix(phi_vec): """ The M matrix from `derivation document - `_ + `_ """ phi_mat = SO3.wedge(phi_vec) M = np.sum( From 372b2f4418d0263091581f3ff03bc72dd503c033 Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Tue, 5 Dec 2023 20:22:40 -0800 Subject: [PATCH 3/4] black with line-length 80 --- docs/source/fun_figs.py | 21 ++++-- examples/ex_ekf_vector.py | 4 +- examples/ex_imm_se3.py | 16 +++-- examples/ex_imm_vector.py | 6 +- examples/ex_inertial_gps.py | 4 +- examples/ex_inertial_nav.py | 8 ++- examples/ex_invariant_so3.py | 8 ++- examples/ex_monte_carlo.py | 8 ++- examples/ex_ukf_se3.py | 4 +- examples/ex_ukf_vector.py | 8 ++- examples/ex_varying_noise.py | 4 +- navlie/batch/problem.py | 20 ++++-- navlie/batch/residuals.py | 4 +- navlie/composite.py | 12 +++- navlie/datagen.py | 4 +- navlie/filters.py | 49 +++++++++---- navlie/imm.py | 20 ++++-- navlie/lib/camera.py | 4 +- navlie/lib/datasets.py | 4 +- navlie/lib/imu.py | 4 +- navlie/lib/models.py | 70 ++++++++++++++----- navlie/lib/preintegration.py | 32 ++++++--- navlie/lib/states.py | 20 ++++-- navlie/types.py | 12 +++- navlie/utils.py | 23 ++++-- tests/integration/test_batch_noiseless.py | 4 +- .../integration/test_filter_ekf_noiseless.py | 4 +- .../test_interacting_multiple_models.py | 3 +- tests/integration/test_iterated_ekf.py | 12 +++- tests/unit/test_composite.py | 4 +- tests/unit/test_meas_models.py | 20 ++++-- tests/unit/test_states.py | 4 +- tests/unit/test_utils.py | 4 +- tests/unit/test_van_loan.py | 4 +- 34 files changed, 319 insertions(+), 109 deletions(-) diff --git a/docs/source/fun_figs.py b/docs/source/fun_figs.py index 9bf9e490..55099f02 100644 --- a/docs/source/fun_figs.py +++ b/docs/source/fun_figs.py @@ -39,13 +39,20 @@ def banana_plot(ax=None): # random greyscale color color = np.random.uniform(0.3, 0.9) - ax.plot(traj_pos[:, 0], traj_pos[:, 1], color=(color, color, color), zorder=1) + ax.plot( + traj_pos[:, 0], + traj_pos[:, 1], + color=(color, color, color), + zorder=1, + ) # save the final state final_states.append(x_traj[-1]) final_positions = np.array([x.position for x in final_states]) - ax.scatter(final_positions[:, 0], final_positions[:, 1], color="C0", zorder=2) + ax.scatter( + final_positions[:, 0], final_positions[:, 1], color="C0", zorder=2 + ) # Propagate the mean with EKF kf = nav.ExtendedKalmanFilter(process_model) @@ -150,14 +157,20 @@ def three_sigma_plot(axs=None): # Set spacing to the above values fig.subplots_adjust( - top=0.975, bottom=0.097, left=0.025, right=0.992, hspace=0.2, wspace=0.117 + top=0.975, + bottom=0.097, + left=0.025, + right=0.992, + hspace=0.2, + wspace=0.117, ) # Save the figure with transparent background, next to this file import os fig.savefig( - os.path.join(os.path.dirname(__file__), "fun_figs.png"), transparent=True + os.path.join(os.path.dirname(__file__), "fun_figs.png"), + transparent=True, ) plt.show() diff --git a/examples/ex_ekf_vector.py b/examples/ex_ekf_vector.py index e28d7d17..a15bc2c4 100644 --- a/examples/ex_ekf_vector.py +++ b/examples/ex_ekf_vector.py @@ -93,7 +93,9 @@ def main(): sns.set_style("whitegrid") 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.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)") diff --git a/examples/ex_imm_se3.py b/examples/ex_imm_se3.py index baca262d..15a87cd2 100644 --- a/examples/ex_imm_se3.py +++ b/examples/ex_imm_se3.py @@ -102,7 +102,9 @@ def imm_trial(trial_number: int) -> List[nav.GaussianResult]: x0_check = x0.plus(nav.randvec(P0)) - estimate_list = nav.imm.run_imm_filter(imm, x0_check, P0, input_list, meas_list) + 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]) @@ -142,8 +144,12 @@ def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: 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_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), @@ -193,7 +199,9 @@ def ekf_trial(trial_number: int) -> List[nav.GaussianResult]: 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] + 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( diff --git a/examples/ex_imm_vector.py b/examples/ex_imm_vector.py index 5607c67d..0d09c42c 100644 --- a/examples/ex_imm_vector.py +++ b/examples/ex_imm_vector.py @@ -58,9 +58,9 @@ def main(): # 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) - ) + 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 diff --git a/examples/ex_inertial_gps.py b/examples/ex_inertial_gps.py index 24505602..8d0faae9 100644 --- a/examples/ex_inertial_gps.py +++ b/examples/ex_inertial_gps.py @@ -43,7 +43,9 @@ def main(): 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, ax, line_color="tab:blue", step=20, label="Estimate" + ) nav.plot_poses( results.state_true, ax, diff --git a/examples/ex_inertial_nav.py b/examples/ex_inertial_nav.py index ec6755f2..daf4ac74 100644 --- a/examples/ex_inertial_nav.py +++ b/examples/ex_inertial_nav.py @@ -168,7 +168,9 @@ 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] @@ -196,7 +198,9 @@ def input_profile(stamp: float, x: IMUState) -> np.ndarray: 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, ax, line_color="tab:blue", step=500, label="Estimate" + ) nav.plot_poses( results.state_true, ax, diff --git a/examples/ex_invariant_so3.py b/examples/ex_invariant_so3.py index 7b240225..c94a72ce 100644 --- a/examples/ex_invariant_so3.py +++ b/examples/ex_invariant_so3.py @@ -45,7 +45,9 @@ def main(): # 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) + results_ekf = nav.GaussianResultList.from_estimates( + estimate_list, state_true + ) # ########################################################################## # Run the invariant filter @@ -57,7 +59,9 @@ def main(): ekf = nav.ExtendedKalmanFilter(process_model=process_model) estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants) - results_invariant = nav.GaussianResultList.from_estimates(estimate_list, state_true) + results_invariant = nav.GaussianResultList.from_estimates( + estimate_list, state_true + ) return results_ekf, results_invariant diff --git a/examples/ex_monte_carlo.py b/examples/ex_monte_carlo.py index dd230d0d..cf7e1610 100644 --- a/examples/ex_monte_carlo.py +++ b/examples/ex_monte_carlo.py @@ -29,7 +29,9 @@ def main(): 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]) + 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), @@ -56,7 +58,9 @@ def ekf_trial(trial_number: int) -> List[GaussianResult]: # 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) + 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) diff --git a/examples/ex_ukf_se3.py b/examples/ex_ukf_se3.py index c3dd2928..557f0739 100644 --- a/examples/ex_ukf_se3.py +++ b/examples/ex_ukf_se3.py @@ -31,7 +31,9 @@ def main(): # %% ####################################################################### # Run Filter - ukf = SigmaPointKalmanFilter(process_model, method="unscented", iterate_mean=False) + ukf = SigmaPointKalmanFilter( + process_model, method="unscented", iterate_mean=False + ) # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! start_time = time.time() diff --git a/examples/ex_ukf_vector.py b/examples/ex_ukf_vector.py index ce6d510d..86dedae7 100644 --- a/examples/ex_ukf_vector.py +++ b/examples/ex_ukf_vector.py @@ -56,7 +56,9 @@ def main(): if noise_active: x0 = x0.plus(randvec(P0)) - ukf = SigmaPointKalmanFilter(process_model, method="cubature", iterate_mean=False) + ukf = SigmaPointKalmanFilter( + process_model, method="cubature", iterate_mean=False + ) # ukf = UnscentedKalmanFilter(process_model, iterate_mean=False) # Equivalent syntax! start_time = time.time() @@ -73,7 +75,9 @@ def main(): 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.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)") diff --git a/examples/ex_varying_noise.py b/examples/ex_varying_noise.py index a7971587..147eb3de 100644 --- a/examples/ex_varying_noise.py +++ b/examples/ex_varying_noise.py @@ -79,7 +79,9 @@ def ekf_trial(trial_number: int) -> List[GaussianResult]: """ np.random.seed(trial_number) - state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=True) + state_true, input_data, meas_data = dg.generate( + x0_true, 0, t_max, noise=True + ) x0_check = x0_true.plus(randvec(P0)) diff --git a/navlie/batch/problem.py b/navlie/batch/problem.py index 4d8ba88f..9f0c7034 100644 --- a/navlie/batch/problem.py +++ b/navlie/batch/problem.py @@ -262,7 +262,9 @@ def _solve_LM(self) -> Dict[Hashable, State]: # Compute the new value of the cost function after the update e, H, cost = self.compute_error_jac_cost(variables=variables_test) - gain_ratio = (prev_cost - cost) / (0.5 * delta_x.T @ (mu * delta_x - b)) + gain_ratio = (prev_cost - cost) / ( + 0.5 * delta_x.T @ (mu * delta_x - b) + ) gain_ratio = gain_ratio.item(0) # If the gain ratio is above zero, accept the step @@ -331,7 +333,9 @@ def compute_error_jac_cost( cost_list = [] # For each factor, evaluate error and Jacobian - for i, (residual, loss) in enumerate(zip(self.residual_list, self.loss_list)): + for i, (residual, loss) in enumerate( + zip(self.residual_list, self.loss_list) + ): variables_list = [variables[key] for key in residual.keys] # Do not compute Jacobian for variables that are held fixed @@ -341,7 +345,9 @@ def compute_error_jac_cost( ] # Evaluate current factor at states - error, jacobians = residual.evaluate(variables_list, compute_jacobians) + error, jacobians = residual.evaluate( + variables_list, compute_jacobians + ) # Compute the robust loss weight and then weight the error u = np.linalg.norm(error) @@ -362,7 +368,9 @@ def compute_error_jac_cost( # Correctly weight the Jacobian jacobian = sqrt_loss_weight * jacobian - H[self.residual_slices[i], self.variable_slices[key]] = jacobian + H[ + self.residual_slices[i], self.variable_slices[key] + ] = jacobian # Sum up costs from each residual cost = np.sum(np.array(cost_list)) @@ -428,7 +436,9 @@ def _correct_states( delta_xi_current = delta_x[slc, [0]] variables[key] = var.plus(delta_xi_current) - def get_covariance_block(self, key_1: Hashable, key_2: Hashable) -> np.ndarray: + def get_covariance_block( + self, key_1: Hashable, key_2: Hashable + ) -> np.ndarray: """Retrieve the covariance block corresponding to two variables. Parameters diff --git a/navlie/batch/residuals.py b/navlie/batch/residuals.py index 229f12c1..ba2540a7 100644 --- a/navlie/batch/residuals.py +++ b/navlie/batch/residuals.py @@ -160,7 +160,9 @@ def evaluate( if compute_jacobians: jac_list = [None] * len(states) if compute_jacobians[0]: - jac_list[0] = -L.T @ self._process_model.jacobian(x_km1, self._u, dt) + jac_list[0] = -L.T @ self._process_model.jacobian( + x_km1, self._u, dt + ) if compute_jacobians[1]: jac_list[1] = L.T @ x_k.minus_jacobian(x_k_hat) diff --git a/navlie/composite.py b/navlie/composite.py index e3c7b0b7..e96c4b47 100644 --- a/navlie/composite.py +++ b/navlie/composite.py @@ -21,7 +21,9 @@ class CompositeState(State): and by ID. """ - def __init__(self, state_list: List[State], stamp: float = None, state_id=None): + def __init__( + self, state_list: List[State], stamp: float = None, state_id=None + ): """ Parameters ---------- @@ -268,7 +270,9 @@ def plus(self, dx, new_stamp: float = None) -> "CompositeState": def minus(self, x: "CompositeState") -> np.ndarray: dx = [] for i, v in enumerate(x.value): - dx.append(self.value[i].minus(x.value[i]).reshape((self.value[i].dof,))) + dx.append( + self.value[i].minus(x.value[i]).reshape((self.value[i].dof,)) + ) return np.concatenate(dx).reshape((-1, 1)) @@ -564,4 +568,6 @@ def __init__(self, y: Measurement, state_id: Any): as per the CompositeMeasurementModel. """ model = CompositeMeasurementModel(y.model, state_id) - super().__init__(value=y.value, stamp=y.stamp, model=model, state_id=y.state_id) + super().__init__( + value=y.value, stamp=y.stamp, model=model, state_id=y.state_id + ) diff --git a/navlie/datagen.py b/navlie/datagen.py index 72222d9a..a1242869 100644 --- a/navlie/datagen.py +++ b/navlie/datagen.py @@ -279,7 +279,9 @@ def generate_measurement( if noise: y = y.reshape((-1, 1)) + randvec(R) - meas_list.append(Measurement(y.reshape(og_shape), x.stamp, model, state_id)) + meas_list.append( + Measurement(y.reshape(og_shape), x.stamp, model, state_id) + ) if not received_list: meas_list = meas_list[0] diff --git a/navlie/filters.py b/navlie/filters.py index 6a30f7ca..4f72ee0c 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -148,7 +148,9 @@ def predict( details_dict = {} if u is not None: Q = self.process_model.covariance(x_jac, u, dt) - x_new.state, A = self.process_model.evaluate_with_jacobian(x.state, u, dt) + x_new.state, A = self.process_model.evaluate_with_jacobian( + x.state, u, dt + ) x_new.covariance = A @ x.covariance @ A.T + Q x_new.symmetrize() x_new.state.stamp = t_km1 + dt @@ -214,7 +216,9 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < -1e10: - raise RuntimeError("Measurement stamp is earlier than state stamp") + raise RuntimeError( + "Measurement stamp is earlier than state stamp" + ) elif u is not None and dt > 1e-11: x = self.predict(x, u, dt) @@ -329,7 +333,9 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < 0: - raise RuntimeError("Measurement stamp is earlier than state stamp") + raise RuntimeError( + "Measurement stamp is earlier than state stamp" + ) elif dt > 0 and u is not None: x = self.predict(x, u, dt) @@ -354,7 +360,9 @@ def correct( P_inv = np.linalg.inv(x.covariance) R_inv = np.linalg.inv(R) - cost_old = np.ndarray.item(0.5 * (e.T @ P_inv @ e + z.T @ R_inv @ z)) + cost_old = np.ndarray.item( + 0.5 * (e.T @ P_inv @ e + z.T @ R_inv @ z) + ) P = J @ x.covariance @ J.T # Compute covariance of innovation @@ -387,7 +395,8 @@ def correct( z_new = y.minus(y_check) e_new = x_new.minus(x.state).reshape((-1, 1)) cost_new = np.ndarray.item( - 0.5 * (e_new.T @ P_inv @ e_new + z_new.T @ R_inv @ z_new) + 0.5 + * (e_new.T @ P_inv @ e_new + z_new.T @ R_inv @ z_new) ) if cost_new < cost_old: step_accepted = True @@ -428,14 +437,18 @@ def correct( S = 0.5 * (S + S.T) K = np.linalg.solve(S.T, (P @ G.T).T).T x.state = x_op - x.covariance = (np.identity(x.state.dof) - K @ G) @ (J @ x.covariance @ J.T) + x.covariance = (np.identity(x.state.dof) - K @ G) @ ( + J @ x.covariance @ J.T + ) x.symmetrize() return x -def generate_sigmapoints(dof: int, method: str) -> Tuple[np.ndarray, np.ndarray]: +def generate_sigmapoints( + dof: int, method: str +) -> Tuple[np.ndarray, np.ndarray]: """Generates unit sigma points from three available methods. @@ -590,7 +603,9 @@ def predict( if u.covariance is not None: input_covariance = u.covariance else: - raise ValueError("Input covariance information must be provided.") + raise ValueError( + "Input covariance information must be provided." + ) if dt is None: dt = u.stamp - x.state.stamp @@ -690,7 +705,9 @@ def correct( if y.stamp is not None: dt = y.stamp - x.state.stamp if dt < 0: - raise RuntimeError("Measurement stamp is earlier than state stamp") + raise RuntimeError( + "Measurement stamp is earlier than state stamp" + ) elif u is not None: x = self.predict(x, u, dt) @@ -712,7 +729,10 @@ def correct( if y_check is not None: y_propagated = np.array( - [y.model.evaluate(x.state.plus(sp)).ravel() for sp in sigmapoints.T] + [ + y.model.evaluate(x.state.plus(sp)).ravel() + for sp in sigmapoints.T + ] ) # predicted measurement mean @@ -722,7 +742,10 @@ def correct( err = y_propagated - y_mean sigmapoints = sigmapoints.T Pyy = ( - np.sum(w[:, None, None] * err[:, :, None] * err[:, None, :], axis=0) + R + np.sum( + w[:, None, None] * err[:, :, None] * err[:, None, :], axis=0 + ) + + R ) Pxy = np.sum( w[:, None, None] * sigmapoints[:, :, None] * err[:, None, :], @@ -846,7 +869,9 @@ def run_filter( 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.correct(x, y, u) meas_idx += 1 if meas_idx < len(meas_data): diff --git a/navlie/imm.py b/navlie/imm.py index 3a468b2f..9bb07ea3 100644 --- a/navlie/imm.py +++ b/navlie/imm.py @@ -312,7 +312,9 @@ def interaction( mu_mix = np.zeros((n_modes, n_modes)) for i in range(n_modes): for j in range(n_modes): - mu_mix[i, j] = 1.0 / c[j] * self.transition_matrix[i, j] * mu_models[i] + mu_mix[i, j] = ( + 1.0 / c[j] * self.transition_matrix[i, j] * mu_models[i] + ) x_mix = [] for j in range(n_modes): @@ -380,17 +382,23 @@ def correct( c_bar = np.zeros(n_modes) for i in range(n_modes): for j in range(n_modes): - c_bar[j] = c_bar[j] + self.transition_matrix[i, j] * mu_km_models[i] + c_bar[j] = ( + c_bar[j] + self.transition_matrix[i, j] * mu_km_models[i] + ) # 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 @@ -465,7 +473,9 @@ def run_imm_filter( 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/camera.py b/navlie/lib/camera.py index a8cafbb9..e43c5534 100644 --- a/navlie/lib/camera.py +++ b/navlie/lib/camera.py @@ -162,7 +162,9 @@ def is_measurement_valid(self, uv: np.ndarray) -> bool: and (uv[0] < self.image_width) ) - def is_landmark_in_front_of_cam(self, pose: SE3State, r_pw_a: np.ndarray) -> bool: + def is_landmark_in_front_of_cam( + self, pose: SE3State, r_pw_a: np.ndarray + ) -> bool: """Checks if a given landmark is in front of the camera.""" r_pc_c: np.ndarray = self.resolve_landmark_in_cam_frame(pose, r_pw_a) r_pc_c = r_pc_c.ravel() diff --git a/navlie/lib/datasets.py b/navlie/lib/datasets.py index d82c24bc..fd53c6b1 100644 --- a/navlie/lib/datasets.py +++ b/navlie/lib/datasets.py @@ -143,7 +143,9 @@ def __init__( Q_c = np.eye(12) Q_c[0:3, 0:3] *= 0.01**2 # Gyro continuous-time covariance Q_c[3:6, 3:6] *= 0.01**2 # Accel continuous-time covariance - Q_c[6:9, 6:9] *= 0.0001**2 # Gyro random-walk continuous-time covariance + Q_c[6:9, 6:9] *= ( + 0.0001**2 + ) # Gyro random-walk continuous-time covariance Q_c[9:12, 9:12] *= ( 0.0001**2 ) # Accel random-walk continuous-time covariance diff --git a/navlie/lib/imu.py b/navlie/lib/imu.py index ec11fdd6..d8a362f3 100644 --- a/navlie/lib/imu.py +++ b/navlie/lib/imu.py @@ -52,7 +52,9 @@ def __init__( """ super().__init__(dof=12, stamp=stamp, covariance=covariance) self.gyro = np.array(gyro).ravel() #:np.ndarray: Gyro reading - self.accel = np.array(accel).ravel() #:np.ndarray: Accelerometer reading + self.accel = np.array( + accel + ).ravel() #:np.ndarray: Accelerometer reading if bias_accel_walk is None: bias_accel_walk = np.zeros((3, 1)) diff --git a/navlie/lib/models.py b/navlie/lib/models.py index 34ef96b7..f97aefdc 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -154,7 +154,9 @@ def __init__(self, Q: np.ndarray): self._Q = Q self.dim = int(Q.shape[0] / 2) - def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState: + def evaluate( + self, x: VectorState, u: VectorInput, dt: float + ) -> VectorState: x = x.copy() u = u.copy() Ad = super().jacobian(x, u, dt) @@ -242,7 +244,9 @@ def evaluate( x.value = x.value @ x.group.Exp(u.value * dt) return x - def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray: + def jacobian( + self, x: MatrixLieGroupState, u: VectorInput, dt: float + ) -> np.ndarray: if x.direction == "right": return x.group.adjoint(x.group.Exp(-u.value * dt)) elif x.direction == "left": @@ -317,12 +321,16 @@ def evaluate( x.value = x.group.Exp(-u[0] * dt) @ x.value @ x.group.Exp(u[1] * dt) return x - def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray: + def jacobian( + self, x: MatrixLieGroupState, u: VectorInput, dt: float + ) -> np.ndarray: u = u.value.reshape((2, round(u.value.size / 2))) if x.direction == "right": return x.group.adjoint(x.group.Exp(-u[1] * dt)) else: - raise NotImplementedError("TODO: left jacobian not yet implemented.") + raise NotImplementedError( + "TODO: left jacobian not yet implemented." + ) def covariance( self, x: MatrixLieGroupState, u: VectorInput, dt: float @@ -339,7 +347,9 @@ def covariance( L2 = dt * x.group.left_jacobian(-dt * u2) return L1 @ self._Q1 @ L1.T + L2 @ self._Q2 @ L2.T else: - raise NotImplementedError("TODO: left covariance not yet implemented.") + raise NotImplementedError( + "TODO: left covariance not yet implemented." + ) class LinearMeasurement(MeasurementModel): @@ -612,7 +622,9 @@ class RangePoseToPose(MeasurementModel): # TODO. tag_body_positions should be optional. argh but this will be # a breaking change since the argument order needs to be different. - def __init__(self, tag_body_position1, tag_body_position2, state_id1, state_id2, R): + def __init__( + self, tag_body_position1, tag_body_position2, state_id1, state_id2, R + ): self.tag_body_position1 = np.array(tag_body_position1).flatten() self.tag_body_position2 = np.array(tag_body_position2).flatten() self.state_id1 = state_id1 @@ -628,7 +640,9 @@ def evaluate(self, x: CompositeState) -> np.ndarray: C_a2 = x2.attitude r_t1_1 = self.tag_body_position1.reshape((-1, 1)) r_t2_2 = self.tag_body_position2.reshape((-1, 1)) - r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a) + r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - ( + C_a2 @ r_t2_2 + r_2w_a + ) return np.array(np.linalg.norm(r_t1t2_a.flatten())) def jacobian(self, x: CompositeState) -> np.ndarray: @@ -640,16 +654,18 @@ def jacobian(self, x: CompositeState) -> np.ndarray: C_a2 = x2.attitude r_t1_1 = self.tag_body_position1.reshape((-1, 1)) r_t2_2 = self.tag_body_position2.reshape((-1, 1)) - r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a) + r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - ( + C_a2 @ r_t2_2 + r_2w_a + ) if C_a1.shape == (2, 2): att_group = SO2 elif C_a1.shape == (3, 3): att_group = SO3 - rho: np.ndarray = (r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten())).reshape( - (-1, 1) - ) + rho: np.ndarray = ( + r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten()) + ).reshape((-1, 1)) if x1.direction == "right": jac1 = x1.jacobian_from_blocks( @@ -673,7 +689,9 @@ def jacobian(self, x: CompositeState) -> np.ndarray: position=-rho.T @ np.identity(r_t2_2.size), ) - return x.jacobian_from_blocks({self.state_id1: jac1, self.state_id2: jac2}) + return x.jacobian_from_blocks( + {self.state_id1: jac1, self.state_id2: jac2} + ) def covariance(self, x: CompositeState) -> np.ndarray: return self._R @@ -841,7 +859,9 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks(position=x.attitude[2, :].reshape((1, -1))) + return x.jacobian_from_blocks( + position=x.attitude[2, :].reshape((1, -1)) + ) elif x.direction == "left": return x.jacobian_from_blocks( attitude=SO3.odot(x.position)[2, :].reshape((1, -1)), @@ -885,9 +905,13 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._g_a)) + return x.jacobian_from_blocks( + attitude=-SO3.odot(x.attitude.T @ self._g_a) + ) elif x.direction == "left": - return x.jacobian_from_blocks(attitude=x.attitude.T @ -SO3.odot(self._g_a)) + return x.jacobian_from_blocks( + attitude=x.attitude.T @ -SO3.odot(self._g_a) + ) def covariance(self, x: MatrixLieGroupState) -> np.ndarray: if np.isscalar(self.R): @@ -930,9 +954,13 @@ def evaluate(self, x: MatrixLieGroupState): def jacobian(self, x: MatrixLieGroupState): if x.direction == "right": - return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._m_a)) + return x.jacobian_from_blocks( + attitude=-SO3.odot(x.attitude.T @ self._m_a) + ) elif x.direction == "left": - return x.jacobian_from_blocks(attitude=-x.attitude.T @ SO3.odot(self._m_a)) + return x.jacobian_from_blocks( + attitude=-x.attitude.T @ SO3.odot(self._m_a) + ) def covariance(self, x: MatrixLieGroupState) -> np.ndarray: if np.isscalar(self.R): @@ -942,7 +970,9 @@ def covariance(self, x: MatrixLieGroupState) -> np.ndarray: class _InvariantInnovation(MeasurementModel): - def __init__(self, y: np.ndarray, model: MeasurementModel, direction="right"): + def __init__( + self, y: np.ndarray, model: MeasurementModel, direction="right" + ): self.measurement_model = model self.y = y.ravel() self.direction = direction @@ -993,7 +1023,9 @@ def _compute_direction(self, x: MatrixLieGroupState) -> str: elif x.direction == "right": direction = "left" else: - raise ValueError("Invalid direction. Must be 'left', 'right' or 'auto'") + raise ValueError( + "Invalid direction. Must be 'left', 'right' or 'auto'" + ) return direction diff --git a/navlie/lib/preintegration.py b/navlie/lib/preintegration.py index ce601ce7..3afe5c88 100644 --- a/navlie/lib/preintegration.py +++ b/navlie/lib/preintegration.py @@ -135,7 +135,9 @@ def accel_bias(self): @property def value(self): - db = self.new_bias.reshape((-1, 1)) - self.original_bias.reshape((-1, 1)) + db = self.new_bias.reshape((-1, 1)) - self.original_bias.reshape( + (-1, 1) + ) return self.original_value @ SE23.Exp(self.bias_jacobian @ db) def increment(self, u: IMU, dt: float): @@ -170,7 +172,9 @@ def increment(self, u: IMU, dt: float): A_full = A_full[0:9, 9:9] L_full = L_full[0:9, 0:6] - self.covariance = A_full @ self.covariance @ A_full.T + L_full @ Q @ L_full.T + self.covariance = ( + A_full @ self.covariance @ A_full.T + L_full @ Q @ L_full.T + ) self.bias_jacobian = A @ self.bias_jacobian - L self.symmetrize() @@ -295,7 +299,9 @@ def jacobian(self, x: IMUState, rmi: IMUIncrement, dt=None) -> np.ndarray: dt = rmi.stamps[1] - rmi.stamps[0] DG = G_matrix(self.gravity, dt) DU = rmi.value - J = SE23.right_jacobian(rmi.bias_jacobian @ (x.bias - rmi.original_bias)) + J = SE23.right_jacobian( + rmi.bias_jacobian @ (x.bias - rmi.original_bias) + ) A = np.identity(15) if x.direction == "right": A[0:9, 0:9] = adjoint_IE3(inverse_IE3(DU)) @@ -389,7 +395,9 @@ def increment(self, u: VectorInput, dt): # Increment the covariance A = self.group.adjoint(self.group.inverse(U)) L = dt * self.group.left_jacobian(-unbiased_velocity * dt) - self.covariance = A @ self.covariance @ A.T + L @ self.input_covariance @ L.T + self.covariance = ( + A @ self.covariance @ A.T + L @ self.input_covariance @ L.T + ) # Increment the bias jacobian self.bias_jacobian = A @ self.bias_jacobian + L @@ -856,7 +864,9 @@ class PreintegratedLinearModel(ProcessModel): def __init__(self): pass - def evaluate(self, x: VectorState, rmi: LinearIncrement, dt=None) -> VectorState: + def evaluate( + self, x: VectorState, rmi: LinearIncrement, dt=None + ) -> VectorState: x = x.copy() A_ij = rmi.value[0] Du_ij = rmi.value[1] @@ -869,12 +879,16 @@ def evaluate(self, x: VectorState, rmi: LinearIncrement, dt=None) -> VectorState x_j = A_ij @ x_i + Du_ij if rmi.original_bias is not None: - x_j = np.vstack((x_j, x.value[-rmi.original_bias.size :].reshape((-1, 1)))) + x_j = np.vstack( + (x_j, x.value[-rmi.original_bias.size :].reshape((-1, 1))) + ) x.value = x_j.ravel() return x - def jacobian(self, x: VectorState, rmi: LinearIncrement, dt=None) -> np.ndarray: + def jacobian( + self, x: VectorState, rmi: LinearIncrement, dt=None + ) -> np.ndarray: if rmi.original_bias is not None: A_ij = rmi.value[0] B_ij = rmi.bias_jacobian @@ -888,5 +902,7 @@ def jacobian(self, x: VectorState, rmi: LinearIncrement, dt=None) -> np.ndarray: return A - def covariance(self, x: VectorState, rmi: LinearIncrement, dt=None) -> np.ndarray: + def covariance( + self, x: VectorState, rmi: LinearIncrement, dt=None + ) -> np.ndarray: return rmi.covariance diff --git a/navlie/lib/states.py b/navlie/lib/states.py index 392910db..96e38b87 100644 --- a/navlie/lib/states.py +++ b/navlie/lib/states.py @@ -188,19 +188,25 @@ def dot(self, other: "MatrixLieGroupState") -> "MatrixLieGroupState": @property def attitude(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have attitude property".format(self.__class__.__name__) + "{0} does not have attitude property".format( + self.__class__.__name__ + ) ) @property def position(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have position property".format(self.__class__.__name__) + "{0} does not have position property".format( + self.__class__.__name__ + ) ) @property def velocity(self) -> np.ndarray: raise NotImplementedError( - "{0} does not have velocity property".format(self.__class__.__name__) + "{0} does not have velocity property".format( + self.__class__.__name__ + ) ) def jacobian_from_blocks(self, **kwargs) -> np.ndarray: @@ -387,7 +393,9 @@ def pose(self, T): self.value = T @staticmethod - def jacobian_from_blocks(attitude: np.ndarray = None, position: np.ndarray = None): + def jacobian_from_blocks( + attitude: np.ndarray = None, position: np.ndarray = None + ): for jac in [attitude, position]: if jac is not None: dim = jac.shape[0] @@ -450,7 +458,9 @@ def position(self, r): self.value[0:3, 3] = r @staticmethod - def jacobian_from_blocks(attitude: np.ndarray = None, position: np.ndarray = None): + def jacobian_from_blocks( + attitude: np.ndarray = None, position: np.ndarray = None + ): for jac in [attitude, position]: if jac is not None: dim = jac.shape[0] diff --git a/navlie/types.py b/navlie/types.py index 5139fefb..b5b79d7f 100644 --- a/navlie/types.py +++ b/navlie/types.py @@ -82,11 +82,15 @@ class State(ABC): __slots__ = ["value", "dof", "stamp", "state_id"] - def __init__(self, value: Any, dof: int, stamp: float = None, state_id=None): + def __init__( + self, value: Any, dof: int, stamp: float = None, state_id=None + ): self.value = value #:Any: State value self.dof = dof #:int: Degree of freedom of the state self.stamp = stamp #:float: Timestamp - self.state_id = state_id #:Any: Some identifier associated with the state + self.state_id = ( + state_id #:Any: Some identifier associated with the state + ) @abstractmethod def plus(self, dx: np.ndarray) -> "State": @@ -535,7 +539,9 @@ def __init__(self, state: State, covariance: np.ndarray): raise ValueError("covariance must be an n x n array.") if covariance.shape[0] != state.dof: - raise ValueError("Covariance matrix does not correspond with state DOF.") + raise ValueError( + "Covariance matrix does not correspond with state DOF." + ) #:navlie.types.State: state object self.state = state diff --git a/navlie/utils.py b/navlie/utils.py index 8ffd9161..08760795 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -138,9 +138,13 @@ def __init__(self, result_list: List[GaussianResult]): #:numpy.ndarray with shape (N,): numpy array of State objects self.state: List[State] = np.array([r.state for r in result_list]) #:numpy.ndarray with shape (N,): numpy array of true State objects - self.state_true: List[State] = np.array([r.state_true for r in result_list]) + self.state_true: List[State] = np.array( + [r.state_true for r in result_list] + ) #:numpy.ndarray with shape (N,dof,dof): covariance - self.covariance: np.ndarray = np.array([r.covariance for r in result_list]) + self.covariance: np.ndarray = np.array( + [r.covariance for r in result_list] + ) #:numpy.ndarray with shape (N, dof): error throughout trajectory self.error = np.array([r.error for r in result_list]) #:numpy.ndarray with shape (N,): EES throughout trajectory @@ -203,7 +207,9 @@ def __getitem__(self, key): out.dof = out.error.shape[1] * np.ones_like(out.stamp) out.md = np.sqrt(out.nees) - out.three_sigma = 3 * np.sqrt(np.diagonal(out.covariance, axis1=1, axis2=2)) + out.three_sigma = 3 * np.sqrt( + np.diagonal(out.covariance, axis1=1, axis2=2) + ) out.rmse = np.sqrt(out.ees / out.dof) out.value = self.value[key1] out.value_true = self.value_true[key1] @@ -350,7 +356,9 @@ def __init__(self, trial_results: List[GaussianResultList]): self.ees = self.average_ees #:numpy.ndarray with shape (N,dof): root-mean-squared error of each component self.rmse: np.ndarray = np.sqrt( - np.average(np.power(np.array([t.error for t in trial_results]), 2), axis=0) + np.average( + np.power(np.array([t.error for t in trial_results]), 2), axis=0 + ) ) #:numpy.ndarray with shape (N,): Total RMSE, this can be meaningless if units differ in a state self.total_rmse: np.ndarray = np.sqrt(self.average_ees) @@ -772,7 +780,9 @@ def plot_meas( axs[i].scatter( y_stamps, y_meas[:, i], color="b", alpha=0.7, s=2, label="Measured" ) - axs[i].plot(y_stamps, y_true[:, i], color="r", alpha=1, label="Modelled") + axs[i].plot( + y_stamps, y_true[:, i], color="r", alpha=1, label="Modelled" + ) axs[i].fill_between( y_stamps, y_true[:, i] + three_sigma[:, i], @@ -1350,7 +1360,8 @@ def fun(T: SE23State): else: raise ValueError( - f"Unknown method '{method}'. " "Must be 'forward', 'central' or 'cs" + f"Unknown method '{method}'. " + "Must be 'forward', 'central' or 'cs" ) return jac_fd diff --git a/tests/integration/test_batch_noiseless.py b/tests/integration/test_batch_noiseless.py index 81f90bdd..2b4003b1 100644 --- a/tests/integration/test_batch_noiseless.py +++ b/tests/integration/test_batch_noiseless.py @@ -110,7 +110,9 @@ def test_noiseless_batch( # Run batch estimator = BatchEstimator(max_iters=20) - estimate_list = estimator.solve(x0, P0, input_list, meas_list, process_model) + estimate_list = estimator.solve( + x0, P0, input_list, meas_list, process_model + ) results = GaussianResultList( [ GaussianResult(estimate_list[i], state_true[i]) diff --git a/tests/integration/test_filter_ekf_noiseless.py b/tests/integration/test_filter_ekf_noiseless.py index c0706d33..60cfa1c0 100644 --- a/tests/integration/test_filter_ekf_noiseless.py +++ b/tests/integration/test_filter_ekf_noiseless.py @@ -58,7 +58,9 @@ def make_range_models_iterated_ekf(R): def make_filter_trial_prediction_noiseless(dg, x0_true, P0, t_max, ekf): def ekf_trial(trial_number: int) -> List[GaussianResult]: np.random.seed(trial_number) - state_true, input_data, meas_data = dg.generate(x0_true, 0, t_max, noise=False) + state_true, input_data, meas_data = dg.generate( + x0_true, 0, t_max, noise=False + ) x0_check = x0_true.copy() estimate_list = run_filter(ekf, x0_check, P0, input_data, meas_data) diff --git a/tests/integration/test_interacting_multiple_models.py b/tests/integration/test_interacting_multiple_models.py index 899c3caf..34a7bfc7 100644 --- a/tests/integration/test_interacting_multiple_models.py +++ b/tests/integration/test_interacting_multiple_models.py @@ -154,7 +154,8 @@ def c_profile(t): np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) ), BodyFrameVelocity( - 9 * np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) + 9 + * np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]) ), ], np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1]), diff --git a/tests/integration/test_iterated_ekf.py b/tests/integration/test_iterated_ekf.py index c168235e..44e6bc02 100644 --- a/tests/integration/test_iterated_ekf.py +++ b/tests/integration/test_iterated_ekf.py @@ -31,7 +31,9 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], + "measurement_models": [ + GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) + ], "measurement_freq": 10, "filter": IteratedKalmanFilter, } @@ -46,7 +48,9 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], + "measurement_models": [ + GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) + ], "measurement_freq": 10, "filter": IteratedKalmanFilter, } @@ -61,7 +65,9 @@ ), "input_freq": 50, "input_covariance": Q, - "measurement_models": [GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2]))], + "measurement_models": [ + GlobalPosition(np.diag([0.1**2, 0.1**2, 0.1**2])) + ], "measurement_freq": 10, "filter": ExtendedKalmanFilter, } diff --git a/tests/unit/test_composite.py b/tests/unit/test_composite.py index 5b5353df..d852c583 100644 --- a/tests/unit/test_composite.py +++ b/tests/unit/test_composite.py @@ -19,7 +19,9 @@ def test_composite_process_model(): T13 = SE2State(SE2.Exp([-0.5, 1, 1]), stamp=0.0, state_id=3) Q = np.diag([0.1**2, 0.1**2, 0.001**2]) x0 = CompositeState([T12, T13]) - process_model = CompositeProcessModel([BodyFrameVelocity(Q), BodyFrameVelocity(Q)]) + process_model = CompositeProcessModel( + [BodyFrameVelocity(Q), BodyFrameVelocity(Q)] + ) u = CompositeInput( [ VectorInput(np.array([0.3, 1, 0]), 1), diff --git a/tests/unit/test_meas_models.py b/tests/unit/test_meas_models.py index b321cd70..f0d2e7fe 100644 --- a/tests/unit/test_meas_models.py +++ b/tests/unit/test_meas_models.py @@ -108,7 +108,9 @@ def test_range_pose_to_pose_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_global_position_se2(direction): - x = SE2State(SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction) + x = SE2State( + SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction + ) model = GlobalPosition(np.identity(3)) _jacobian_test(x, model, atol=1e-5) @@ -183,7 +185,9 @@ def test_altitude_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_gravity_so3(direction): - x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction) + x = SO3State( + SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction + ) model = Gravitometer(np.identity(3)) _jacobian_test(x, model) @@ -211,7 +215,9 @@ def test_gravity_se23(direction): @pytest.mark.parametrize("direction", ["right", "left"]) def test_magnetometer_so3(direction): - x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction) + x = SO3State( + SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction + ) model = Magnetometer(np.identity(3)) _jacobian_test(x, model) @@ -238,7 +244,9 @@ def test_magnetometer_se23(direction): def test_invariant_magnetometer_so3(): - x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left") + x = SO3State( + SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left" + ) b = [1, 0, 0] y = np.array(b) @@ -250,7 +258,9 @@ def test_invariant_magnetometer_so3(): def test_invariant_magnetometer_se3(): - x = SE3State(SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left") + x = SE3State( + SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left" + ) b = [1, 0, 0] y = np.array(b) diff --git a/tests/unit/test_states.py b/tests/unit/test_states.py index cee4cf77..1177bc12 100644 --- a/tests/unit/test_states.py +++ b/tests/unit/test_states.py @@ -64,7 +64,9 @@ def test_minus_jacobian(s: str): assert np.allclose(jac, jac_test, atol=1e-5) -@pytest.mark.parametrize("s", ["so2", "so3", "se2", "se3", "se23", "sl3", "mlg"]) +@pytest.mark.parametrize( + "s", ["so2", "so3", "se2", "se3", "se23", "sl3", "mlg"] +) def test_mlg_dot(s: str): x = sample_states[s] dx = np.random.randn(x.dof) diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 690ac139..65781ba8 100644 --- a/tests/unit/test_utils.py +++ b/tests/unit/test_utils.py @@ -72,7 +72,9 @@ def test_gaussian_result_list_slicing_equivalency(): results[0:10] # returns the first 10 time steps results[:, 0] # returns the first degree of freedom - results[0:10, 0] # returns the first degree of freedom for the first 10 time steps + results[ + 0:10, 0 + ] # returns the first degree of freedom for the first 10 time steps results[ 0:10, [0, 1] ] # returns the first two degrees of freedom for the first 10 time steps diff --git a/tests/unit/test_van_loan.py b/tests/unit/test_van_loan.py index 84ac9a0b..eec3e7f8 100644 --- a/tests/unit/test_van_loan.py +++ b/tests/unit/test_van_loan.py @@ -30,7 +30,9 @@ def test_van_loan_double_integrator(): # Compare to analytical solution A_d_test = np.array([[1, dt], [0, 1]]) - Q_d_test = np.array([[1 / 3 * dt**3, 1 / 2 * dt**2], [1 / 2 * dt**2, dt]]) + Q_d_test = np.array( + [[1 / 3 * dt**3, 1 / 2 * dt**2], [1 / 2 * dt**2, dt]] + ) assert np.allclose(A_d, A_d_test) assert np.allclose(Q_d, Q_d_test) From 2d31ef2c7710872ebb273609af557d557804f7c0 Mon Sep 17 00:00:00 2001 From: Charles Cossette Date: Tue, 5 Dec 2023 20:36:55 -0800 Subject: [PATCH 4/4] more docs. fix minor todo --- navlie/datagen.py | 13 ++++++++++--- navlie/lib/models.py | 25 ++++++++++++++++++++++--- navlie/utils.py | 1 - 3 files changed, 32 insertions(+), 7 deletions(-) diff --git a/navlie/datagen.py b/navlie/datagen.py index a1242869..58369a9c 100644 --- a/navlie/datagen.py +++ b/navlie/datagen.py @@ -53,9 +53,9 @@ def __init__( input_freq: float, meas_model_list: List[ MeasurementModel - ] = [], # TODO: fix mutable default argument + ] = None, meas_freq_list: Union[float, List[float]] = None, - meas_offset_list: Union[float, List[float]] = [], + meas_offset_list: Union[float, List[float]] = None, ): # Make input covariance a callable if it isnt if callable(input_covariance): @@ -65,6 +65,12 @@ def __init__( else: raise ValueError("Input covariance must be a function or a matrix.") + if meas_model_list is None: + meas_model_list = [] + + if meas_offset_list is None: + meas_offset_list = [] + if isinstance(meas_model_list, MeasurementModel): meas_model_list = [meas_model_list] @@ -113,7 +119,8 @@ def __init__( def add_measurement_model( self, model: MeasurementModel, freq: float, offset: float = 0.0 ): - """Adds a new measurement model to the existing list of measurement models. + """ + Adds a new measurement model to the existing list of measurement models. Parameters ---------- diff --git a/navlie/lib/models.py b/navlie/lib/models.py index f97aefdc..5dc95c05 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -618,13 +618,32 @@ class RangePoseToPose(MeasurementModel): """ Range model given two absolute poses of rigid bodies, each containing a ranging tag. - """ - # TODO. tag_body_positions should be optional. argh but this will be - # a breaking change since the argument order needs to be different. + Compatible with ``SE2State, SE3State, SE23State, IMUState``. + """ def __init__( self, tag_body_position1, tag_body_position2, state_id1, state_id2, R ): + """ + Parameters + ---------- + tag_body_position1 : np.ndarray + Position of tag in body frame of Robot 1. + tag_body_position2 : np.ndarray + Position of tag in body frame of Robot 2. + state_id1 : Any + State ID of Robot 1. + state_id2 : Any + State ID of Robot 2. + R : float or np.ndarray with size 1 + Covariance associated with range measurement error. + """ + # TODO. Make tag_body_position1 and tag_body_position2 optional, with a + # default value of either [0,0] or [0,0,0] (depending on the dimension + # of the passed state). Unfortunately, changing argument order is a + # breaking change. + + self.tag_body_position1 = np.array(tag_body_position1).flatten() self.tag_body_position2 = np.array(tag_body_position2).flatten() self.state_id1 = state_id1 diff --git a/navlie/utils.py b/navlie/utils.py index 08760795..398538b0 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -165,7 +165,6 @@ def __init__(self, result_list: List[GaussianResult]): self.value_true = np.array([r.state_true.value for r in result_list]) def __getitem__(self, key): - # TODO need more tests for all cases! if isinstance(key, tuple): if not len(key) == 2: raise IndexError("Only two dimensional indexing is supported")