symforce.slam.imu_preintegration.manifold_symbolic module#

roll_forward_state(pose_i, vel_i, DR, Dv, Dp, gravity, dt)[source]#

Roll forward the given state by the given preintegrated measurements

This returns the pose_j and vel_j that give 0 residual

Parameters:

  • pose_i (Pose3) – Pose at time step i (world_T_body)

  • vel_i (Matrix31) – Velocity at time step i (world frame)

  • DR (Rot3) – Preintegrated estimate for pose_i.R.inverse() * pose_j.R

  • Dv (Matrix31) – Preintegrated estimate for vel_j - vel_i in the body frame at timestep i

  • Dp (Matrix31) – Preintegrated estimate for position change (before velocity and gravity

  • gravity (Matrix31) – Acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU.

  • dt (float) – The time between timestep i and j: t_j - t_i

Returns:

T.Tuple[sf.Pose3, sf.V3] – pose_j, vel_j

Return type:

Tuple[Pose3, Matrix31]

internal_imu_residual(pose_i, vel_i, pose_j, vel_j, accel_bias_i, gyro_bias_i, DR, Dv, Dp, sqrt_info, DR_D_gyro_bias, Dv_D_accel_bias, Dv_D_gyro_bias, Dp_D_accel_bias, Dp_D_gyro_bias, accel_bias_hat, gyro_bias_hat, gravity, dt, epsilon)[source]#

An internal helper function to linearize the difference between the orientation, velocity, and position at one time step and those of another time step. The expected difference is calculated from the preintegrated IMU measurements between those time steps.

NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use the one found in symforce/slam/imu_preintegration/imu_factor.h.

Time step i is the time of the first IMU measurement of the interval. Time step j is the time after the last IMU measurement of the interval.

Assumes sqrt_info is lower triangular and only reads the lower triangle.

Parameters:

  • pose_i (Pose3) – Pose at time step i (world_T_body)

  • vel_i (Matrix31) – Velocity at time step i (world frame)

  • pose_j (Pose3) – Pose at time step j (world_T_body)

  • vel_j (Matrix31) – Velocity at time step j (world frame)

  • accel_bias_i (Matrix31) – The accelerometer bias between timesteps i and j

  • gyro_bias_i (Matrix31) – The gyroscope bias between timesteps i and j

  • DR (Rot3) – Preintegrated estimate for pose_i.R.inverse() * pose_j.R

  • Dv (Matrix31) – Preintegrated estimate for vel_j - vel_i in the body frame at timestep i

  • Dp (Matrix31) –

    Preintegrated estimate for position change (before velocity and gravity corrections) in the body frame at timestep i:

    R_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2),
where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i

  • sqrt_info (Matrix99) – sqrt info matrix of DR(‘s tangent space), Dv, Dp

  • DR_D_gyro_bias (Matrix33) – Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat

  • Dv_D_accel_bias (Matrix33) – Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat

  • Dv_D_gyro_bias (Matrix33) – Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat

  • Dp_D_accel_bias (Matrix33) – Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat

  • Dp_D_gyro_bias (Matrix33) – Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat

  • accel_bias_hat (Matrix31) – The accelerometer bias used during preintegration

  • gyro_bias_hat (Matrix31) – The gyroscope bias used during preintegration

  • gravity (Matrix31) – Acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU.

  • dt (float) – The time between timestep i and j: t_j - t_i

  • epsilon (float) – epsilon used for numerical stability

Return type:

Matrix91

Outputs:

res: 9dof product residual between the orientations, then velocities, then positions.

handwritten_new_state_D_state_gyro_accel(DR, corrected_gyro, corrected_accel, dt, epsilon)[source]#

Calculates the derivatives of the new state (meaning the DR, Dv, and Dp) w.r.t. the previous state, gyroscope measurement, and the accelerometer measurement.

Parameters:

  • DR (Rot3) – Preintegrated DR of the previous state

  • corrected_gyro (Matrix31) – gyroscope measurment corrected for IMU bias

  • corrected_accel (Matrix31) – accelerometer measurement corrected for IMU bias

  • dt (float) – Time difference between the previous time step and the new time step

  • epsilon (float) – epsilon for numerical stability

Returns:

T.Tuple[sf.M99, sf.M93, sf.M93] – new_state_D_old_state, new_state_D_gyro_measurement, new_state_D_accel_measurement

Return type:

Tuple[Matrix99, Matrix93, Matrix93]

imu_manifold_preintegration_update(DR, Dv, Dp, covariance, DR_D_gyro_bias, Dv_D_accel_bias, Dv_D_gyro_bias, Dp_D_accel_bias, Dp_D_gyro_bias, accel_bias, gyro_bias, accel_cov_diagonal, gyro_cov_diagonal, accel_measurement, gyro_measurement, dt, epsilon, use_handwritten_derivatives=True)[source]#

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.

Parameters:

  • DR (Rot3) – Preintegrated change in orientation

  • Dv (Matrix31) – Preintegrated change in velocity

  • Dp (Matrix31) – Preintegrated change in position

  • covariance (Matrix99) – Covariance [DR, Dv, Dp] in local coordinates of mean

  • DR_D_gyro_bias (Matrix33) – Derivative of DR w.r.t. gyro_bias

  • Dv_D_accel_bias (Matrix33) – Derivative of Dv w.r.t. accel_bias

  • Dv_D_gyro_bias (Matrix33) – Derivative of Dv w.r.t. gyro_bias

  • Dp_D_accel_bias (Matrix33) – Derivative of Dp w.r.t. accel_bias

  • Dp_D_gyro_bias (Matrix33) – Derivative of Dp w.r.t. gyro_bias

  • accel_bias (Matrix31) – Initial guess for the accelerometer measurement bias

  • gyro_bias (Matrix31) – Initial guess for the gyroscope measurement bias

  • accel_cov_diagonal (Matrix31) – The diagonal of the covariance of the accelerometer measurement

  • gyro_cov_diagonal (Matrix31) – The diagonal of the covariance of the gyroscope measurement

  • accel_measurement (Matrix31) – The accelerometer measurement

  • gyro_measurement (Matrix31) – The gyroscope measurement

  • dt (float) – The time between the new measurements and the last

  • epsilon (float) – epsilon for numerical stability

  • use_handwritten_derivatives (bool) –

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,

Return type:

Tuple[Rot3, Matrix31, Matrix31, Matrix99, Matrix33, Matrix33, Matrix33, Matrix33, Matrix33]

integrate_state(DR, Dv, Dp, accel, gyro, dt, epsilon)[source]#

Given the old preintegrated state and the new IMU measurements, calculates the new preintegrated state.

Returns:

T.Tuple[sf.Rot3, sf.V3, sf.V3] – (new_DR, new_Dv, new_Dp)

Parameters:
Return type:

Tuple[Rot3, Matrix31, Matrix31]