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.
The measurement noise is modeled as continuous-time white noise acting over the whole step, and its contribution to the covariance is integrated in closed form over the step, holding the step’s starting rotation fixed (see :func:
step_noise_covariance). The covariance of a window of measurements is therefore full rank even when the window consists of a single step. It is exact, and does not depend on how the window is divided into steps, for a non-rotating body; with an angular rate it is accurate to first order in the rotation per step.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 noise spectral density of the accelerometer measurement, i.e. the covariance of a measurement times the time it covers gyro_cov_diagonal: The diagonal of the noise spectral density of the gyroscope measurement, i.e. the covariance of a measurement times the time it covers 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>