diff --git a/README.rst b/README.rst index 25e60e46..6074cfb1 100644 --- a/README.rst +++ b/README.rst @@ -13,6 +13,12 @@ navlie :target: https://www.python.org/downloads/ :alt: Python Version +.. image:: ./docs/source/fun_figs.png + :alt: Demo Figures + :align: center + :width: 100% + + An on-manifold state estimation library for robotics. The core idea behind this project is to abstract-away the state definition such that a single estimator implementation can operate on a variety of state manifolds, such as the usual vector space, and any Lie group. At the moment, algorithms and features of this package include: diff --git a/docs/source/fun_figs.png b/docs/source/fun_figs.png new file mode 100644 index 00000000..b351c5aa Binary files /dev/null and b/docs/source/fun_figs.png differ diff --git a/docs/source/fun_figs.py b/docs/source/fun_figs.py new file mode 100644 index 00000000..46a311d7 --- /dev/null +++ b/docs/source/fun_figs.py @@ -0,0 +1,165 @@ +# %% +import navlie as nav +import numpy as np +import matplotlib.pyplot as plt +import seaborn as sns + +sns.set_style("whitegrid") + +# %% Banana distribution plot +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]) + process_model = nav.lib.BodyFrameVelocity(np.zeros(3)) + + dx_samples = nav.randvec(covariance, N).T + x0_samples = [x0.plus(dx) for dx in dx_samples] + + # Monte-carlo the trajectory forward in time + dt = 0.1 + T = 10 + stamps = np.arange(0, T, dt) + + if ax is None: + fig, ax = plt.subplots(figsize=(8, 8)) + + final_states = [] + for sample in x0_samples: + x_traj = [sample.copy()] + u = nav.lib.VectorInput([0.1, 0.3, 0]) + x = sample + for _ in stamps: + x = process_model.evaluate(x, u, dt) + x_traj.append(x.copy()) + + # plot the trajectory + traj_pos = np.array([x.position for x in x_traj]) + + # 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) + + # 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 + kf = nav.ExtendedKalmanFilter(process_model) + x0_hat = nav.StateWithCovariance(x0, covariance) + + x_hat_traj = [x0_hat] + for t in stamps: + u.stamp = t + x_hat_traj.append(kf.predict(x_hat_traj[-1], u, dt)) + + mean_traj = np.array([x.state.position for x in x_hat_traj]) + 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): + N = 500 + x0 = nav.lib.SE3State([0.3, 3, 4, 0, 0, 0], direction="right") + process_model = nav.lib.BodyFrameVelocity(np.zeros(6)) + + dt = 0.1 + T = 20 + stamps = np.arange(0, T, dt) + + x_traj = [x0.copy()] + u = nav.lib.VectorInput([0.1, 0.3, 0, 1, 0, 0]) + x = x0.copy() + for _ in stamps: + x = process_model.evaluate(x, u, dt) + x_traj.append(x.copy()) + + fig, ax = nav.plot_poses(x_traj, ax = ax) + + +# pose3d_plot() + +# %% +def three_sigma_plot(axs = None): + dataset = nav.lib.datasets.SimulatedPoseRangingDataset() + + estimates = nav.run_filter( + nav.ExtendedKalmanFilter(dataset.process_model), + 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()) + + fig, axs = nav.plot_error(results[:, :3], axs=axs) + axs[2].set_xlabel("Time (s)") + + +# three_sigma_plot() + + +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. + + # The following values where chosen by trial and error + # top=0.975, + # bottom=0.097, + # left=0.025, + # right=0.992, + # hspace=0.2, + # wspace=0.117 + + # 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') + + # The last plot is a 3x1 grid + gs2 = gs[2].subgridspec(3, 1, hspace=0.1) + ax3 = fig.add_subplot(gs2[0]) + ax4 = fig.add_subplot(gs2[1]) + ax5 = fig.add_subplot(gs2[2]) + + # Remove tick labels for ax3 and ax4 + ax3.set_xticklabels([]) + ax4.set_xticklabels([]) + + # Remove all tick labels for ax2 + ax2.set_xticklabels([]) + 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 + fig.subplots_adjust( + 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) + + plt.show() +# %% diff --git a/docs/source/index.rst b/docs/source/index.rst index 26cf5e18..fbb725ee 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -15,6 +15,11 @@ Welcome to navlie! ------------------ +.. make a row of three figures stacked side by side +.. image:: ./fun_figs.png + :width: 100% + :align: center + navlie is a state estimation package specifically designed for both traditional and Lie-group-based estimation problems! The core idea behind this project is to use abstraction in such a way that both traditional and Lie-group-based problems fall under the exact same interface. Using this, a single estimator implementation can operate on a variety of state definitions, such as the usual vector space, and any Lie group. We allow the user to define their custom state, process model, and measurement models, after which they will have a variety of algorithms available to them, including: diff --git a/examples/ex_ekf_vector.py b/examples/ex_ekf_vector.py index 31d46127..a15bc2c4 100644 --- a/examples/ex_ekf_vector.py +++ b/examples/ex_ekf_vector.py @@ -90,7 +90,7 @@ def main(): import matplotlib.pyplot as plt import seaborn as sns - sns.set_theme() + sns.set_style("whitegrid") fig, ax = plt.subplots(1, 1) ax.plot(results.value[:, 0], results.value[:, 1], label="Estimate") ax.plot( @@ -113,4 +113,6 @@ def main(): axs[i].plot(results.stamp, results.error[:, i]) axs[0].set_title("Estimation error") axs[1].set_xlabel("Time (s)") + axs[0].set_ylabel("x error (m)") + axs[1].set_ylabel("y error (m)") plt.show() diff --git a/examples/ex_inertial_gps.py b/examples/ex_inertial_gps.py index 9e141f2f..8d0faae9 100644 --- a/examples/ex_inertial_gps.py +++ b/examples/ex_inertial_gps.py @@ -13,7 +13,7 @@ def main(): data = SimulatedInertialGPSDataset(t_start=0, t_end=20) gt_states = data.get_ground_truth() input_data = data.get_input_data() - meas_data = data.get_meas_data() + meas_data = data.get_measurement_data() # Filter initialization P0 = np.eye(15) diff --git a/examples/ex_iterated_ekf_se3.py b/examples/ex_iterated_ekf_se3.py index e94df17d..f5932a30 100644 --- a/examples/ex_iterated_ekf_se3.py +++ b/examples/ex_iterated_ekf_se3.py @@ -11,7 +11,7 @@ def main(): data = nav.lib.SimulatedPoseRangingDataset() gt_states = data.get_ground_truth() input_data = data.get_input_data() - meas_data = data.get_meas_data() + meas_data = data.get_measurement_data() # %% ########################################################################### # Perturb initial groundtruth state to initialize filter diff --git a/examples/ex_ukf_se2.py b/examples/ex_ukf_se3.py similarity index 97% rename from examples/ex_ukf_se2.py rename to examples/ex_ukf_se3.py index 32c8da6e..557f0739 100644 --- a/examples/ex_ukf_se2.py +++ b/examples/ex_ukf_se3.py @@ -25,7 +25,7 @@ def main(): data = SimulatedPoseRangingDataset(x0=x0, Q=Q, noise_active=noise_active) state_true = data.get_ground_truth() input_data = data.get_input_data() - meas_data = data.get_meas_data() + meas_data = data.get_measurement_data() if noise_active: x0 = x0.plus(randvec(P0)) # %% ####################################################################### diff --git a/examples/tutorial.py b/examples/tutorial.py deleted file mode 100644 index cb3557c9..00000000 --- a/examples/tutorial.py +++ /dev/null @@ -1,92 +0,0 @@ -import navlie as nav -from navlie.lib import VectorState, VectorInput -import numpy as np - -# Define the initial state -x0 = VectorState([0, 0, 0], stamp=0.0) -Q = np.eye(2) * 0.1**2 - - -# Define the process model -class BicycleModel(nav.ProcessModel): - def evaluate(self, x: VectorState, u: nav.lib.VectorInput, dt: float) -> VectorState: - x_next = x.copy() - x_next.value[0] += u.value[0] * dt * np.cos(x.value[2]) - x_next.value[1] += u.value[0] * dt * np.sin(x.value[2]) - x_next.value[2] += u.value[1] * dt - return x_next - - def input_covariance(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray: - return Q - - -# Define the measurement model -class RangeToLandmark(nav.MeasurementModel): - def __init__(self, landmark_position: np.ndarray): - self.landmark_position = landmark_position - - def evaluate(self, x: VectorState) -> np.ndarray: - return np.linalg.norm(x.value[:2] - self.landmark_position) - - def covariance(self, x: VectorState) -> np.ndarray: - return 0.1**2 - - -# Generate some simulated data -landmarks = np.array([[1, 1], [1, 2], [2, 2], [2, 1]]) -meas_models = [RangeToLandmark(landmark) for landmark in landmarks] -process_model = BicycleModel() -dg = nav.DataGenerator( - process_model=process_model, - input_func=lambda t, x: np.array([0.5, 0.3]), - input_covariance=lambda t: Q, - input_freq=50, - meas_model_list=meas_models, - meas_freq_list=[10, 10, 10, 10], -) - -state_data, input_data, meas_data = dg.generate(x0, start=0, stop=10, noise=True) - - -# Now lets run a filter! -# First, define the filter -kalman_filter = nav.ExtendedKalmanFilter(process_model) -P0 = np.diag([1, 1, 0.1**2]) # Initial covariance -x = nav.StateWithCovariance(x0, P0) # Estimate and covariance in one container -meas_idx = 0 -y = meas_data[meas_idx] -estimates = [] -for k in range(len(input_data) - 1): - u = input_data[k] - - # Fuse any measurements that have occurred. - while y.stamp < input_data[k + 1].stamp and meas_idx < len(meas_data): - x = kalman_filter.correct(x, y, u) - - # Load the next measurement - meas_idx += 1 - if meas_idx < len(meas_data): - y = meas_data[meas_idx] - - # Predict until the next input is available - dt = input_data[k + 1].stamp - x.state.stamp - x = kalman_filter.predict(x, u, dt) - - estimates.append(x.copy()) - - -# Plot the results -import matplotlib.pyplot as plt - -pos = np.array([state.value[:2] for state in state_data]) -plt.plot(pos[:, 0], pos[:, 1]) -plt.scatter(landmarks[:, 0], landmarks[:, 1]) -# add labels -for i, landmark in enumerate(landmarks): - plt.annotate(f"Landmark {i}", landmark) -plt.xlabel("x") -plt.ylabel("y") -plt.title("Simulated Trajectory") -plt.axis("equal") - -plt.show() diff --git a/navlie/filters.py b/navlie/filters.py index 063af35d..2158aa1f 100644 --- a/navlie/filters.py +++ b/navlie/filters.py @@ -129,6 +129,10 @@ def predict( # If state has no time stamp, load from measurement. # 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" + ) t_km1 = u.stamp else: t_km1 = x.state.stamp diff --git a/navlie/lib/datasets.py b/navlie/lib/datasets.py index 686caabd..fd53c6b1 100644 --- a/navlie/lib/datasets.py +++ b/navlie/lib/datasets.py @@ -105,7 +105,7 @@ def get_ground_truth(self) -> List[SE3State]: def get_input_data(self) -> List[nav.Input]: return self.input_data - def get_meas_data(self) -> List[nav.Measurement]: + def get_measurement_data(self) -> List[nav.Measurement]: return self.meas_data @@ -213,5 +213,5 @@ def get_ground_truth(self) -> List[IMUState]: def get_input_data(self) -> List[IMU]: return self.input_data - def get_meas_data(self) -> List[nav.Measurement]: + def get_measurement_data(self) -> List[nav.Measurement]: return self.meas_data diff --git a/navlie/types.py b/navlie/types.py index 90180fc7..9774f3ff 100644 --- a/navlie/types.py +++ b/navlie/types.py @@ -588,6 +588,6 @@ def get_input_data(self) -> List[Input]: pass @abstractmethod - def get_meas_data(self) -> List[Measurement]: + def get_measurement_data(self) -> List[Measurement]: """Returns a list of measurements.""" pass diff --git a/navlie/utils.py b/navlie/utils.py index a8bef1c0..473c83bb 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -77,7 +77,33 @@ class GaussianResultList: """ A data container that accepts a list of ``GaussianResult`` objects and stacks the attributes in numpy arrays. Convenient for plotting. This object - does nothing more than array-ifying the attributes of ``GaussianResult`` + does nothing more than array-ifying the attributes of ``GaussianResult``. + + This object also supports slicing, which will return a new ``GaussianResultList`` + object with the sliced attributes either through time or through the degrees + of freedom themselves. For example, + + .. code-block:: python + + results = GaussianResultList.from_estimates(estimates, state_true_list) + + 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 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, + 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``. + """ __slots__ = [ @@ -831,6 +857,7 @@ def plot_poses( arrow_length: float = 1, step: int = 5, label: str = None, + linewidth=None, ): """ Plots position trajectory in 3D @@ -858,6 +885,12 @@ def plot_poses( # TODO. handle 2D case if isinstance(poses, GaussianResultList): poses = poses.state + + if isinstance(poses, StateWithCovariance): + poses = [poses.state] + + if not isinstance(poses, list): + poses = [poses] if ax is None: fig = plt.figure() @@ -889,6 +922,7 @@ def plot_poses( color=colors[0], length=arrow_length, arrow_length_ratio=0.1, + linewidths=linewidth, ) ax.quiver( x, @@ -900,6 +934,7 @@ def plot_poses( color=colors[1], length=arrow_length, arrow_length_ratio=0.1, + linewidths=linewidth, ) ax.quiver( x, @@ -911,6 +946,7 @@ def plot_poses( color=colors[2], length=arrow_length, arrow_length_ratio=0.1, + linewidths=linewidth, ) set_axes_equal(ax) diff --git a/tests/test_examples.py b/tests/test_examples.py index 3399eb13..282e32c4 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -91,8 +91,8 @@ def test_ex_sequential_measurements(): main() -def test_ex_ukf_se2(): - from ex_ukf_se2 import main +def test_ex_ukf_se3(): + from ex_ukf_se3 import main main() diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 6ab52007..81781709 100644 --- a/tests/unit/test_utils.py +++ b/tests/unit/test_utils.py @@ -19,6 +19,7 @@ def test_gaussian_result_indexing(): + # Construct a dummy GaussianResultList x_true = [SE23State(SE23.random(), stamp=i) for i in range(10)] x_hat = [SE23State(SE23.random(), stamp=i) for i in range(10)] cov = [(i + 1) * np.eye(9) for i in range(10)] @@ -32,12 +33,13 @@ def test_gaussian_result_indexing(): cov_test = grl.covariance[:, slc, slc] nees_test = [e**2 / c for e, c in zip(e_test, cov_test)] nees_test = np.array(nees_test).squeeze() - assert np.alltrue(grl_test.covariance == cov_test) - assert np.alltrue(grl_test.error == grl.error[:, slc]) + assert np.all(grl_test.covariance == cov_test) + assert np.all(grl_test.error == grl.error[:, slc]) assert np.allclose(grl_test.nees, nees_test) def test_gaussian_result_slicing(): + # Construct a dummy GaussianResultList x_true = [SE23State(SE23.random(), stamp=i) for i in range(10)] x_hat = [SE23State(SE23.random(), stamp=i) for i in range(10)] cov = [(i + 1) * np.eye(9) for i in range(10)] @@ -54,10 +56,29 @@ def test_gaussian_result_slicing(): for e, c in zip(e_test, cov_test) ] nees_test = np.array(nees_test).squeeze() - assert np.alltrue(grl_test.covariance == cov_test) - assert np.alltrue(grl_test.error == grl.error[:, slc]) + assert np.all(grl_test.covariance == cov_test) + 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)] + x_hat = [SE23State(SE23.random(), stamp=i) for i in range(10)] + 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 + + 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[:, 3:].error == results[:, 3:9].error) + @pytest.mark.parametrize( "method, threshold", [("forward", 1e-6), ("central", 1e-10), ("cs", 1e-16)]