Merge pull request #107 from decargroup/add_toy_slam_example

Add simple toy SLAM problem to examples

examples/ex_inertial_nav.py

@@ -2,202 +2,46 @@
 A slightly more complicated example of a robot localizing itself from relative
 position measurements to known landmarks.
 """
-from typing import List
 import numpy as np
-from pymlg import SE23
-from navlie.lib import (
-    IMU,
-    IMUState,
-    IMUKinematics,
-    InvariantMeasurement,
-    PointRelativePosition,
-)
 import navlie as nav
-
-# ##############################################################################
-# Problem Setup
-
-t_start = 0
-t_end = 15
-imu_freq = 100
-
-# IMU noise parameters
-sigma_gyro_ct = 0.01
-sigma_accel_ct = 0.01
-sigma_gyro_bias_ct = 0.0001
-sigma_accel_bias_ct = 0.0001
-init_gyro_bias = np.array([0.02, 0.03, -0.04]).reshape((-1, 1))
-init_accel_bias = np.array([0.01, 0.02, 0.05]).reshape((-1, 1))
-
-# Landmark parameters
-cylinder_radius = 7
-n_level = 1
-n_landmarks_per_level = 3
-max_height = 2.5
-
-# Landmark sensor parameters
-sigma_landmark_sensor = 0.1  # [m]
-landmark_sensor_freq = 10
-
-# Initialization parameters
-sigma_phi_init = 0.1
-sigma_v_init = 0.1
-sigma_r_init = 0.1
-sigma_bg_init = 0.01
-sigma_ba_init = 0.01
-nav_state_init = SE23.from_components(
-    np.identity(3),
-    np.array([3, 0, 0]).reshape((-1, 1)),
-    np.array([0, 3, 0]).reshape((-1, 1)),
-)
-
-################################################################################
-################################################################################
-
-# Continuous-time Power Spectral Density
-Q_c = np.eye(12)
-Q_c[0:3, 0:3] *= sigma_gyro_ct**2
-Q_c[3:6, 3:6] *= sigma_accel_ct**2
-Q_c[6:9, 6:9] *= sigma_gyro_bias_ct**2
-Q_c[9:12, 9:12] *= sigma_accel_bias_ct**2
-dt = 1 / imu_freq
-
-Q_noise = Q_c / dt
-
-
-def generate_landmark_positions(
-    cylinder_radius: float,
-    max_height: float,
-    n_levels: int,
-    n_landmarks_per_level: int,
-) -> List[np.ndarray]:
-    """Generates landmarks arranged in a cylinder.
-
-    Parameters
-    ----------
-    cylinder_radius : float
-        Radius of the cylinder that the landmarks are arranged in.
-    max_height : float
-        Top of cylinder.
-    n_levels : int
-        Number of discrete levels to place landmarks at vertically.
-    n_landmarks_per_level : int
-        Number of landmarks per level
-
-    Returns
-    -------
-    List[np.ndarray]
-        List of landmarks.
-    """
-
-    z = np.linspace(0, max_height, n_levels)
-
-    angles = np.linspace(0, 2 * np.pi, n_landmarks_per_level + 1)
-    angles = angles[0:-1]
-    x = cylinder_radius * np.cos(angles)
-    y = cylinder_radius * np.sin(angles)
-
-    # Generate landmarks
-    landmarks = []
-    for level_idx in range(n_levels):
-        for landmark_idx in range(n_landmarks_per_level):
-            cur_landmark = np.array(
-                [x[landmark_idx], y[landmark_idx], z[level_idx]]
-            ).reshape((3, -1))
-            landmarks.append(cur_landmark)
-
-    return landmarks
-
-
-def input_profile(stamp: float, x: IMUState) -> np.ndarray:
-    """Generates an IMU measurement for a circular trajectory,
-    where the robot only rotates about the z-axis and the acceleration
-    points towards the center of the circle.
-    """
-
-    # Add biases to true angular velocity and acceleration
-    bias_gyro = x.bias_gyro.reshape((-1, 1))
-    bias_accel = x.bias_accel.reshape((-1, 1))
-
-    C_ab = x.attitude
-    g_a = np.array([0, 0, -9.80665]).reshape((-1, 1))
-    omega = np.array([0, 0, -1.0]).reshape((-1, 1)) + bias_gyro
-    accel = np.array([0, -3.0, 0]).reshape((-1, 1)) + bias_accel - C_ab.T @ g_a
-
-    # Generate a random input to drive the bias random walk
-    Q_bias = Q_noise[6:, 6:]
-    bias_noise = nav.randvec(Q_bias)
-
-    u = IMU(omega, accel, stamp, bias_noise[0:3], bias_noise[3:6])
-    return u
-
-
-landmarks = generate_landmark_positions(
-    cylinder_radius, max_height, n_level, n_landmarks_per_level
-)
-
-process_model = IMUKinematics(Q_c / dt)
-meas_cov = np.identity(3) * sigma_landmark_sensor**2
-meas_model_list = [PointRelativePosition(pos, meas_cov) for pos in landmarks]
-
-# Create data generator
-data_gen = nav.DataGenerator(
-    process_model,
-    input_func=input_profile,
-    input_covariance=Q_noise,
-    input_freq=imu_freq,
-    meas_model_list=meas_model_list,
-    meas_freq_list=landmark_sensor_freq,
-)
-
-# Initial state and covariance
-x0 = IMUState(
-    nav_state_init,
-    init_gyro_bias,
-    init_accel_bias,
-    stamp=t_start,
-    state_id=0,
-    direction="left",
-)
-
-P0 = np.eye(15)
-P0[0:3, 0:3] *= sigma_phi_init**2
-P0[3:6, 3:6] *= sigma_v_init**2
-P0[6:9, 6:9] *= sigma_r_init**2
-P0[9:12, 9:12] *= sigma_bg_init**2
-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
-)
-
-# **************** Conversion to Invariant Measurements ! *********************
-invariants = [InvariantMeasurement(meas, "right") for meas in meas_list]
-# *****************************************************************************
-
-# Zero-out the random walk values
-for u in input_list:
-    u.bias_gyro_walk = np.array([0, 0, 0])
-    u.bias_accel_walk = np.array([0, 0, 0])
-
-
-ekf = nav.ExtendedKalmanFilter(process_model)
-
-estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants)
-
-# Postprocess the results and plot
-results = nav.GaussianResultList.from_estimates(estimate_list, states_true)
+from navlie.lib.datasets import SimulatedInertialLandmarkDataset
+
+def main():
+    np.set_printoptions(precision=3, suppress=True, linewidth=200)
+    np.random.seed(0)
+    # Load simulated dataset with default parameters
+    dataset = SimulatedInertialLandmarkDataset(t_start=0, t_end=20)
+    gt_states = dataset.get_ground_truth()
+    input_data = dataset.get_input_data()
+    meas_data = dataset.get_measurement_data()
+
+    # Filter initialization 
+    P0 = np.eye(15)
+    P0[0:3, 0:3] *= 0.1**2
+    P0[3:6, 3:6] *= 0.1**2
+    P0[6:9, 6:9] *= 0.1**2
+    P0[9:12, 9:12] *= 0.01**2
+    P0[12:15, 12:15] *= 0.01**2
+    x0 = gt_states[0].plus(nav.randvec(P0))
+
+    # ###########################################################################
+    # Run filter
+    ekf = nav.ExtendedKalmanFilter(dataset.process_model)
+    estimate_list = nav.run_filter(ekf, x0, P0, input_data, meas_data)
+
+    # Postprocess the results and plot
+    results = nav.GaussianResultList.from_estimates(estimate_list, gt_states)
+    return results, dataset
 
 if __name__ == "__main__":
+    results, dataset = main()
     import matplotlib.pyplot as plt
     import seaborn as sns
 
     fig = plt.figure()
     ax = plt.axes(projection="3d")
-    landmarks = np.array(landmarks)
+    landmarks = np.array(dataset.get_groundtruth_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"
     )

examples/ex_slam.py

@@ -0,0 +1,161 @@
+"""A toy SLAM example where we are interested in estimating robot poses and 
+3D landmark positions from IMU measurements and relative position measurements 
+to the landmarks.
+
+Here, the purpose is simply to show how the default EKF provided in navlie
+can be used in SLAM-type problem settings. The structure of the SLAM problem
+is not exploited by doing this. For a more efficient EKF implementation for
+SLAM, see the document: 
+    Simulataneous localization and mapping with the extended Kalman filter by Joan
+    Solà (2014).
+"""
+
+import typing
+import numpy as np
+import navlie as nav
+from navlie.lib.imu import IMUState
+from navlie.lib.datasets import SimulatedInertialLandmarkDataset
+from navlie.lib.states import VectorState, CompositeState
+from navlie.lib.models import PointRelativePositionSLAM, CompositeInput, CompositeProcessModel
+from scipy.linalg import block_diag
+
+class LandmarkProcessModel(nav.ProcessModel):
+    def evaluate(self, x: VectorState, t: float, u: np.ndarray):
+        return x.copy()
+
+    def jacobian(self, x: VectorState, t: float, u: np.ndarray):
+        return np.eye(3)
+
+    def covariance(self, x: VectorState, t: float, u: np.ndarray):
+        return np.zeros((3, 3))
+
+
+def main():
+    np.set_printoptions(precision=5, suppress=True, linewidth=1000)
+    np.random.seed(0)
+    # Load simulated dataset with default parameters
+    dataset = SimulatedInertialLandmarkDataset(t_start=0, t_end=10.0, R=0.1)
+    gt_states = dataset.get_ground_truth()
+    input_data = dataset.get_input_data()
+    meas_data = dataset.get_measurement_data()
+    landmarks = dataset.get_groundtruth_landmarks()
+
+    # Filter initialization - set small covariance on yaw and position
+    # as these are unobservable
+    P0_imu = np.eye(15)
+    P0_imu[0:2, 0:2] *= 0.1**2
+    P0_imu[2, 2] *= 1e-15
+    P0_imu[3:6, 3:6] *= 0.1**2
+    P0_imu[6:9, 6:9] *= 1e-15
+    P0_imu[9:12, 9:12] *= 0.01**2
+    P0_imu[12:15, 12:15] *= 0.01**2
+    init_imu_state = gt_states[0].plus(nav.randvec(P0_imu))
+
+    # Set the state ID to be "r" for robot state and "l" for landmark state
+    robot_state_id = "r"
+    landmark_state_id = "l"
+    init_imu_state.state_id = robot_state_id
+
+    landmark_state_ids: typing.List[str] = []
+
+    # Create a SLAM state that includes both the landmark states and the robot
+    # state
+    state_list = [init_imu_state]
+    P0_landmark = 0.1**2
+    P0_landmark_block = np.identity(3 * len(landmarks)) * P0_landmark
+    for i, pos in enumerate(landmarks):
+        # Perturb the initial landmark position
+        perturbed_pos = pos + nav.randvec(P0_landmark * np.eye(3))
+        cur_landmark_state_id = landmark_state_id + str(i)
+        state_list.append(VectorState(perturbed_pos, 0.0, cur_landmark_state_id))
+        landmark_state_ids.append(cur_landmark_state_id)
+
+    init_state = CompositeState(state_list, stamp=init_imu_state.stamp)
+    init_cov = block_diag(P0_imu, P0_landmark_block)
+
+    # Convert all measurments to SLAM measurements, where the landmark position
+    # is a state to be estimated
+    for meas in meas_data:
+        current_landmark_id = landmark_state_id + str(meas.model._landmark_id)
+        meas.model = PointRelativePositionSLAM(
+            robot_state_id, current_landmark_id, meas.model._R
+        )
+
+    # Create a composite process model that includes the robot process model and
+    # the (constant) landmark process model for each landmark
+    landmark_process_model = LandmarkProcessModel()
+    process_model_list = [dataset.process_model]
+    for i in range(len(landmarks)):
+        process_model_list.append(landmark_process_model)
+    process_model = CompositeProcessModel(process_model_list)
+
+    composite_inputs = []
+    for u in input_data:
+        # Create a composite input for each of the landmarks
+        input_list = [u]
+        for i in range(len(landmarks)):
+            input_list.append(None)
+        composite_inputs.append(CompositeInput(input_list))
+
+    # ###########################################################################
+    # Create and run filter    
+    ekf = nav.ExtendedKalmanFilter(process_model)
+    estimate_list = nav.run_filter(ekf, init_state, init_cov, composite_inputs, meas_data)
+
+    # Extract the IMU state estimates from the estimate list
+    imu_state_list: typing.List[IMUState] = []
+    imu_cov_list: typing.List[np.ndarray] = []
+    for estimate in estimate_list:
+        imu_state = estimate.state.get_state_by_id(robot_state_id)
+        imu_state.stamp = estimate.state.stamp
+        imu_state_list.append(imu_state)
+        imu_state_slice = estimate.state.get_slice_by_id(robot_state_id)
+        imu_cov_list.append(estimate.covariance[imu_state_slice, imu_state_slice])
+
+    imu_estimates_list: typing.List[nav.StateWithCovariance] = []
+    for state, cov in zip(imu_state_list, imu_cov_list):
+        imu_estimates_list.append(nav.StateWithCovariance(state, cov))
+
+    # Extract the final estimated landmark positions
+    final_estimate = estimate_list[-1]
+    landmark_est_list: typing.List[np.ndarray] = []
+    for landmark_id in landmark_state_ids:
+        landmark_state = final_estimate.state.get_state_by_id(landmark_id)
+        landmark_est_list.append(landmark_state.value)
+
+    # Postprocess the results and plot
+    imu_results = nav.GaussianResultList.from_estimates(imu_estimates_list, gt_states)
+    return imu_results, landmark_est_list, dataset
+
+
+if __name__ == "__main__":
+    imu_results, landmark_est_list, dataset = main()
+    import matplotlib.pyplot as plt
+    import seaborn as sns
+
+    fig = plt.figure()
+    ax = plt.axes(projection="3d")
+    landmarks = np.array(dataset.get_groundtruth_landmarks())
+    est_landmarks = np.array(landmark_est_list)
+    ax.scatter(est_landmarks[:, 0], est_landmarks[:, 1], est_landmarks[:, 2], marker="x", color="tab:blue")
+    ax.scatter(landmarks[:, 0], landmarks[:, 1], landmarks[:, 2], color="tab:red")
+    nav.plot_poses(imu_results.state, ax, line_color="tab:blue", step=500, label="Estimate")
+    nav.plot_poses(
+        imu_results.state_true,
+        ax,
+        line_color="tab:red",
+        step=500,
+        label="Groundtruth",
+    )
+    ax.legend()
+
+    sns.set_theme()
+    fig, axs = nav.plot_error(imu_results)
+    axs[0, 0].set_title("Attitude")
+    axs[0, 1].set_title("Velocity")
+    axs[0, 2].set_title("Position")
+    axs[0, 3].set_title("Gyro bias")
+    axs[0, 4].set_title("Accel bias")
+    axs[-1, 2]
+
+    plt.show()