File imu_preintegrator.h

namespace sym

Typedefs

using ImuPreintegratord = ImuPreintegrator<double>
using ImuPreintegratorf = ImuPreintegrator<float>
template<typename Scalar>
class ImuPreintegrator
#include <imu_preintegrator.h>

Class to on-manifold preintegrate IMU measurements for usage in a SymForce optimization problem.

For usage, see the doc-string of ImuFactor in imu_factor.h

Public Types

using Vector3 = typename PreintegratedImuMeasurements<Scalar>::Vector3
using Matrix33 = typename PreintegratedImuMeasurements<Scalar>::Matrix33
using Matrix99 = Eigen::Matrix<Scalar, 9, 9>

Public Functions

ImuPreintegrator(const Vector3 &accel_bias, const Vector3 &gyro_bias)

Initialize with given accel_bias and gyro_bias

void IntegrateMeasurement(const Vector3 &measured_accel, const Vector3 &measured_gyro, const Vector3 &accel_cov, const Vector3 &gyro_cov, Scalar dt, Scalar epsilon = kDefaultEpsilon<Scalar>)

Integrate a new measurement.

Postcondition:

If the orientation, velocity, and position were R0, v0, and p0 at the start of the first integrated IMU measurements, and Rf, vf, and pf are the corresponding values at the end of the measurement described by the arguments, DT is the total time covered by the integrated measurements, and g is the vector of acceleration due to gravity, then

  • pim.DR -> R0.inverse() * Rf

  • pim.Dv -> R0.inverse() * (vf - v0 - g * DT)

  • pim.Dp -> R0.inverse() * (pf - p0 - v0 * DT - 0.5 * g * DT^2)

  • pim.DX_D_accel_bias -> the derivative of DX wrt the accel bias linearized at pim.accel_bias

  • pim.DX_D_gyro_bias -> the derivative of DX wrt the gyro bias linearized at pim.gyro_bias

  • pim.accel_bias -> unchanged

  • pim.gyro_bias -> unchanged

  • pim.integrated_dt -> DT

  • covariance -> the covariance [DR, Dv, Dp] in local coordinates of mean

Parameters:

  • measured_accel – the next accelerometer measurement after the last integrated measurement

  • measured_gyro – the next gyroscope measurement after the last integrated measurement

  • accel_cov – the covariance of the accelerometer measurement (represented by its diagonal entries) [(meters / time^2)^2 * time]

  • gyro_cov – the covariance of the gyroscope measurement (represented by its diagonal entries) [(radians / time)^2 * time]

  • dt – the time span over which these measurements were made

const PreintegratedImuMeasurements<Scalar> &PreintegratedMeasurements() const
const Matrix99 &Covariance() const

Private Members

PreintegratedImuMeasurements<Scalar> preintegrated_measurements_
Matrix99 covariance_