File imu_manifold_preintegration_update.h

namespace sym

Functions

template<typename Scalar>
void ImuManifoldPreintegrationUpdate(const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &covariance, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &gyro_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &accel_measurement, const Eigen::Matrix<Scalar, 3, 1> &gyro_measurement, const Scalar dt, const Scalar epsilon, sym::Rot3<Scalar> *const new_DR = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dv = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dp = nullptr, Eigen::Matrix<Scalar, 9, 9> *const new_covariance = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_DR_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_gyro_bias = nullptr)

An internal helper function to update a set of preintegrated IMU measurements with a new pair of gyroscope and accelerometer measurements. Returns the updated preintegrated measurements.

NOTE: If you are interested in this function, you should likely be using the IntegrateMeasurement method of the PreintegratedImuMeasurements class located in symforce/slam/imu_preintegration/preintegrated_imu_measurements.h.

When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the derivatives w.r.t. to bias should all be 0.

Args: DR (sf.Rot3): Preintegrated change in orientation Dv (sf.V3): Preintegrated change in velocity Dp (sf.V3): Preintegrated change in position covariance (sf.M99): Covariance [DR, Dv, Dp] in local coordinates of mean DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias accel_bias (sf.V3): Initial guess for the accelerometer measurement bias gyro_bias (sf.V3): Initial guess for the gyroscope measurement bias accel_cov_diagonal (sf.M33): The diagonal of the covariance of the accelerometer measurement gyro_cov_diagonal (sf.M33): The diagonal of the covariance of the gyroscope measurement accel_measurement (sf.V3): The accelerometer measurement gyro_measurement (sf.V3): The gyroscope measurement dt (T.Scalar): The time between the new measurements and the last epsilon (T.Scalar): epsilon for numerical stability

Returns: T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: new_DR, new_Dv, new_Dp, new_covariance, new_DR_D_gyro_bias, new_Dv_D_accel_bias, new_Dv_D_gyro_bias, new_Dp_D_accel_bias new_Dp_D_gyro_bias,