4.1. Inertial Estimation with Imu Preintegration¶

4.1.1. Background¶

IMUs are powerful sensors capable of providing us with measurements on angular velocity and linear acceleration of the body at high frequencies (~200Hz and above). To estimate the pose of the body at any time, one can simply integrate the angular velocity to get the rotation and double-integrate the linear acceleration to get the velocity and translation.

\[ R^w_b(t_j) = R^w_b(t_i)\int^{t_j}_{t_i} exp(\hat{\omega}(t)) dt \]

\[ v^w(t_j) = v^w(t_i) + \int^{t_j}_{t_i} \hat{a} dt \]

\[ t^w(t_j) = \int^{t_j}_{t_i} \left( v^w(t_i) \int^{t_j}_{t_i} \hat{a}\right) dt \]

where $\hat{\omega}$ is the measured angular velocity and $\hat{a}$ is the measured linear acceleration.

Alas, things are not as straightforward as this. Unless you are using tactical or navigation grade IMUs (which cost $1000s), the sensor measurements will be noisy and affected by sensor bias $(b_{\omega}, b_{a})$ that will cause the estimates to drift away from the true values. To correct for this, we need to optimize with consideration for the noise and bias values as part of our model.

However, performing optimization at 200 Hz is a bit unrealistic and can quickly overwhelm the system, especially when we only need estimates every, e.g. 50 Hz. Can we instead account for multiple measurements between two estimation timestamps as a single binary constraint? The answer is yes and the methodology for that is developed in the paper by Lupton and Sukkarieh and improved upon by Forster et. al. which is termed as IMU Preintegration.

Luckily for us, GTSAM has the ImuFactor which performs IMU preintegration for us in a neatly tied package, such that all we need to consider is the coordinate frame of the sensor with respect to the body, and providing the measurements for preintegration.

4.1.2. Objectives¶

In this example, we shall examine how to use IMU preintegration for inertial estimation with factor graphs. Given a sequence of measurements, we will construct the factor graph and optimize it in order to get the desired pose estimates.

# Install the pre-requisites
%pip -q install gtbook ipympl  # also installs latest gtsam pre-release

Note: you may need to restart the kernel to use updated packages.

# All the imports we need
import numpy as np
import gtsam
from gtsam.utils.plot import plot_pose3
from matplotlib import pyplot as plt
from gtsam.symbol_shorthand import B, V, X
from gtbook.display import show
from mpl_toolkits.mplot3d import Axes3D

try:
  # For Google Colab
  from google.colab import output
  output.enable_custom_widget_manager()
  print(" -- Custom widgets enabled")
except:
  pass

# For interactive plots
%matplotlib widget

4.1.3. Example Trajectory¶

Let’s first generate an example trajectory we wish to estimate, as this will give us a good sense of what we want. We’ll also visualize the trajectory with a little helper function.

T = 12  # The timespan of our trajectory.
dt = 1e-2  # 100 Hz frequency
velocity = np.array([2, 0, 0])  # The velocity we wish to move at.

scenarios = {
    "zero_twist": (np.zeros(3), np.zeros(3)),  # Zero motion, stationary trajectory.
    "forward_motion": (np.zeros(3), velocity),  # Move forward in the x axis at 2 m/s.
    "loop": (np.array([0, -np.radians(30), 0]), velocity),  # A loop-de-loop trajectory.
    "sick": (np.array([np.radians(30), -np.radians(30), 0]), velocity)  # A spiral trajectory, "sick" in surfer slang.
}


def plot_scenario(scenario,
                  T,
                  dt,
                  title="IMU trajectory scenario",
                  fignum=0,
                  maxDim=5):
    for t in np.arange(0, T, dt):
        actualPose = scenario.pose(t)
        plot_pose3(fignum, actualPose, axis_length=0.3)

        translation = actualPose.translation()
        maxDim = max([max(np.abs(translation)), maxDim])
        ax = plt.gca()
        ax.set_xlim3d(-maxDim, maxDim)
        ax.set_ylim3d(-maxDim, maxDim)
        ax.set_zlim3d(-maxDim, maxDim)
        ax.set_title(title)

    plt.show()

We can now plot the various scenarios, e.g. forward motion.

for idx, scenario_name in enumerate(scenarios.keys()):
    scenario = gtsam.ConstantTwistScenario(*scenarios[scenario_name])
    plot_scenario(scenario, T, dt, fignum=idx + 1, title=scenario_name)

Let’s start with a simple trajectory to make understanding easier, so we will pick the forward motion trajectory. As you will see later, the same approach works for all trajectories.

scenario = gtsam.ConstantTwistScenario(*scenarios["forward_motion"])

# Let's visualize it for our understanding
plot_scenario(scenario, T, dt, title="Forward Motion", fignum=5)

As a final step in creating the example, we need to define the IMU biases.

accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)

4.1.4. IMU Preintegration¶

To perform preintegration, GTSAM conveniently provides us with an object called PreintegratedImuMeasurements. This object requires various parameters such as the sensor covariances, an initial estimate of the bias, and a potential tranform bodyPsensor is the IMU is not coincidental with the body frame.

We begin with specifying that the IMU has the Z axis pointing up. This is important since reaction to gravity is an acceleration that is measured by the IMU, and making a mistake here can throw our entire system into jeopardy easily. We also specify some nominal covariance values, though these would depend on the IMU itself.

Also, let’s create a ScenarioRunner which is a helper object to get us the measurements as well as other parameters we need during the main data loop.

pim_params = gtsam.PreintegrationParams.MakeSharedU(9.81)

# Some arbitrary noise sigmas
gyro_sigma = 1e-3
accel_sigma = 1e-3
I_3x3 = np.eye(3)
pim_params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
pim_params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
pim_params.setIntegrationCovariance(1e-7**2 * I_3x3)

# Define the PreintegratedImuMeasurements object here.
pim = gtsam.PreintegratedImuMeasurements(pim_params, actualBias)

runner = gtsam.ScenarioRunner(scenario, pim_params, dt, actualBias)
plot_scenario(scenario, T, dt, fignum=6)

We can define our main loop, where we accept the IMU measurements and create our ImuFactors by preintegrating the measurements.

def main_loop(runner, scenario, graph, initial, T):
    # The factor index for the estimation rate
    i = 0

    for k, t in enumerate(np.arange(0, T, dt)):
        # get measurements and add them to PIM
        measuredOmega = runner.measuredAngularVelocity(t)
        measuredAcc = runner.measuredSpecificForce(t)

        ### This is where all the magic happens!
        pim.integrateMeasurement(measuredAcc, measuredOmega, dt)

        if (k + 1) % int(1 / dt) == 0:
            # Create IMU factor every second.
            factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), B(0), pim)
            graph.push_back(factor)

            # We have created the binary constraint, so we clear out the preintegration values.
            pim.resetIntegration()

            # Get the true state which we will corrupt with some additive noise terms defined below
            actual_state_i = scenario.navState(t + dt)

            # These are additive noise terms.
            rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
            translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
            poseNoise = gtsam.Pose3(rotationNoise, translationNoise)

            noisy_state_i = gtsam.NavState(
                actual_state_i.pose().compose(poseNoise),
                actual_state_i.velocity() + np.random.randn(3) * 0.1)

            initial.insert(X(i + 1), noisy_state_i.pose())
            initial.insert(V(i + 1), noisy_state_i.velocity())
            i += 1

    return graph, initial

4.1.5. Factor Graph¶

We are now ready to generate our factor graph. The scenario object will provide us with the measurements, and we will add a prior on the pose and the velocity to ensure our graph is not singular.

graph = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()

Let’s add in the priors to our graph as well as some initial estimates.

def add_priors(scenario, graph, initial):
    # Noise models for
    priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
    velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)

    initial_state = scenario.navState(0)
    graph.push_back(
        gtsam.PriorFactorPose3(X(0), initial_state.pose(), priorNoise))
    graph.push_back(
        gtsam.PriorFactorVector(V(0), initial_state.velocity(), velNoise))

    initial.insert(B(0), actualBias)
    initial.insert(X(0), initial_state.pose())
    initial.insert(V(0), initial_state.velocity())

    return graph, initial


graph, initial = add_priors(scenario, graph, initial)

Now with everything set up, we can run our main loop.

graph, initial = main_loop(runner, scenario, graph, initial, T)

We want to visualize our initial estimates, so let’s create a small function to do so and plot out the initial trajectory based only on pure forward-integration. You will see that the initial estimates don’t look like anything close to the true trajectory!

def plot_trajectory(values: gtsam.Values,
                    title: str = "Estimated Trajectory",
                    fignum: int = 1,
                    show: bool = False):
    i = 0
    while values.exists(X(i)):
        pose_i = values.atPose3(X(i))
        plot_pose3(fignum, pose_i, 1)
        i += 1
    plt.title(title)

    gtsam.utils.plot.set_axes_equal(fignum)

    plt.ioff()

    if show:
        plt.show()


plot_trajectory(initial, title="Initial Trajectory", fignum=7, show=True)

4.1.6. Optimization¶

Now that we have a full factor graph and initial estimates, we can optimize for the correct estimates. This is as simple as just initializing an optimizer with the graph and initial values and calling optimize on it.

lm_params = gtsam.LevenbergMarquardtParams()
lm_params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params)
result = optimizer.optimize()

Initial error: 2.55485e+08, values: 27
iter      cost      cost_change    lambda  success iter_time
3.635723e+06    2.52e+08    1.00e-05     1    3.92e-04
7.383290e+02    3.63e+06    1.00e-06     1    3.05e-04
3.746566e-05    7.38e+02    1.00e-07     1    2.97e-04
2.761648e-20    3.75e-05    1.00e-08     1    2.94e-04
3.304397e-23    2.76e-20    1.00e-09     1    3.10e-04

4.1.7. Final Result¶

Now that we have our final result values, we can visualize the result and see that our estimates are actually quite good.

plot_trajectory(result, fignum=8, show=True)

4.1.8. Other Scenarios¶

We can similarly run the same code above for other scenarios.

4.1.8.1. Standing Scenario¶

Let’s use the ImuFactorExample for all the scenarios, starting with Standing.

scenario = gtsam.ConstantTwistScenario(*scenarios["zero_twist"])
plot_scenario(scenario, T, dt, title="Zero Twist", fignum=9)

runner = gtsam.ScenarioRunner(scenario, pim_params, dt, actualBias)


graph = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()

graph, initial = add_priors(scenario, graph, initial)
graph, initial = main_loop(runner, scenario, graph, initial, T)

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params)
result = optimizer.optimize()

plot_trajectory(initial, title="Initial Trajectory", fignum=10, show=True)
plot_trajectory(result, fignum=11, show=True)

Initial error: 2.86533e+08, values: 27
iter      cost      cost_change    lambda  success iter_time
   0  3.040285e+06    2.83e+08    1.00e-05     1    3.64e-04
   1  3.019809e+02    3.04e+06    1.00e-06     1    3.00e-04
   2  1.592637e-06    3.02e+02    1.00e-07     1    2.94e-04
   3  1.473353e-22    1.59e-06    1.00e-08     1    2.94e-04

4.1.8.2. Loop Scenario¶

scenario = gtsam.ConstantTwistScenario(*scenarios["loop"])
plot_scenario(scenario, T, dt, title="Loop Scenario", fignum=12)

runner = gtsam.ScenarioRunner(scenario, pim_params, dt, actualBias)


graph = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()

graph, initial = add_priors(scenario, graph, initial)
graph, initial = main_loop(runner, scenario, graph, initial, T)

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params)
result = optimizer.optimize()

plot_trajectory(initial, title="Initial Trajectory", fignum=13, show=True)
plot_trajectory(result, fignum=14, show=True)

Initial error: 1.71512e+08, values: 27
iter      cost      cost_change    lambda  success iter_time
2.796515e+06    1.69e+08    1.00e-05     1    3.69e-04
1.345629e+03    2.80e+06    1.00e-06     1    2.90e-04
6.650640e-05    1.35e+03    1.00e-07     1    2.86e-04
5.243533e-20    6.65e-05    1.00e-08     1    2.85e-04
5.003348e-23    5.24e-20    1.00e-09     1    2.83e-04

4.1.8.3. Sick Scenario¶

scenario = gtsam.ConstantTwistScenario(*scenarios["sick"])
plot_scenario(scenario, T, dt, title="Sick Scenario", fignum=15)

runner = gtsam.ScenarioRunner(scenario, pim_params, dt, actualBias)


graph = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()

graph, initial = add_priors(scenario, graph, initial)
graph, initial = main_loop(runner, scenario, graph, initial, T)

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params)
result = optimizer.optimize()

plot_trajectory(initial, title="Initial Trajectory", fignum=16, show=True)
plot_trajectory(result, fignum=17, show=True)

Initial error: 2.40651e+08, values: 27
iter      cost      cost_change    lambda  success iter_time
2.449586e+06    2.38e+08    1.00e-05     1    3.89e-04
8.091429e+03    2.44e+06    1.00e-06     1    3.00e-04
9.830748e-04    8.09e+03    1.00e-07     1    2.95e-04
1.794730e-17    9.83e-04    1.00e-08     1    2.97e-04
7.715182e-23    1.79e-17    1.00e-09     1    2.91e-04

4.1.9. Conclusion¶

There you have it! Using the PreintegratedImuMeasurements object and combining it with the ImuFactor gives us a powerful and efficient mechanism for performing inertial estimation. GTSAM gives us useful abstractions for dealing with IMU measurements at much higher rates than the estimation rate, and allows us to optimize for the correct trajectory.

GTSAM by Example

Inertial Estimation with Imu Preintegration

Contents