File imu_manifold_preintegration_update_auto_derivative.h¶
-
namespace sym
Functions
-
template<typename Scalar>
void ImuManifoldPreintegrationUpdateAutoDerivative(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)¶ Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output. Since this function is more expensive, there is no reason to use it instead.
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
IntegrateMeasurementmethod of thePreintegratedImuMeasurementsclass located insymforce/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: Preintegrated change in orientation Dv: Preintegrated change in velocity Dp: Preintegrated change in position covariance: Covariance [DR, Dv, Dp] in local coordinates of mean DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias accel_bias: Initial guess for the accelerometer measurement bias gyro_bias: Initial guess for the gyroscope measurement bias accel_cov_diagonal: The diagonal of the covariance of the accelerometer measurement gyro_cov_diagonal: The diagonal of the covariance of the gyroscope measurement accel_measurement: The accelerometer measurement gyro_measurement: The gyroscope measurement dt: The time between the new measurements and the last epsilon: 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,
-
template<typename Scalar>