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]

step_noise_covariance(DR, corrected_accel, accel_cov_diagonal, gyro_cov_diagonal, dt)[source]

Calculates the covariance of the error that the gyroscope and accelerometer noise of a single integration step adds to the new state (DR, Dv, Dp), in local coordinates of the new state.

The noise is modeled as continuous-time white noise with the given spectral densities, acting over the whole step, while the integrator holds the measurements and the rotation DR of the start of the step constant (see integrate_state()). The linearized error state (dR, dv, dp) then evolves within the step as:

d/dt [dR, dv, dp] = F [dR, dv, dp] + G [gyro_noise, accel_noise]

F = [[0, 0, 0],      G = [[I, 0],      A = -R [corrected_accel]_x
     [A, 0, 0],           [0, R],
     [0, I, 0]]           [0, 0]]

where R is the rotation matrix of DR. F is nilpotent (F^3 = 0), so the state transition matrix over a time s is exactly Phi(s) = I + F s + F^2 s^2 / 2, and the covariance of the noise integrated over the step is, exactly:

Q = int_0^dt Phi(dt - tau) G Sigma G^T Phi(dt - tau)^T dtau
  = sum_{i, j in 0..2} F^i (G Sigma G^T) (F^T)^j dt^(i + j + 1) / (i! j! (i + j + 1))

Written out by blocks, with Sg = diag(gyro_cov_diagonal) and Sa = R diag(accel_cov_diagonal) R^T:

Q_RR = Sg dt
Q_vR = A Sg dt^2 / 2
Q_pR = A Sg dt^3 / 6
Q_vv = Sa dt       + A Sg A^T dt^3 / 3
Q_pv = Sa dt^2 / 2 + A Sg A^T dt^4 / 8
Q_pp = Sa dt^3 / 3 + A Sg A^T dt^5 / 20

The Sa terms are the familiar covariance of doubly integrated white noise, which is positive definite for any dt > 0. The A Sg terms are the rotation noise accumulated during the step leaking into the velocity and position through the integrated acceleration.

This is exact for a non-rotating body. Holding DR fixed within the step drops the -[omega]_x dR term of the rotation-error dynamics (and the right Jacobian J_r(omega dt) the discrete model applied to the gyro noise), so with an angular rate omega the step covariance is accurate to first order in |omega| dt. Likewise, the covariance of a window does not depend on how it is divided into steps when the body does not rotate, and depends on it only to that order otherwise.

Parameters:

  • DR (Rot3) – Preintegrated DR at the start of the step

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

  • accel_cov_diagonal (Matrix31) – Diagonal of the accelerometer noise spectral density

  • gyro_cov_diagonal (Matrix31) – Diagonal of the gyroscope noise spectral density

  • dt (float) – Duration of the step

Returns:

sf.M99 – The noise covariance to add to the propagated covariance of the new state

Return type:

Matrix99

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.

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 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.

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 noise spectral density of the accelerometer measurement, i.e. the covariance of a measurement times the time it covers

  • gyro_cov_diagonal (Matrix31) – 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 (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]