factorIMU

Convert IMU readings to factor

Since R2022a

Description

The factorIMU object converts raw inertial measurement unit (IMU) readings into constraints between poses, velocities, and IMU biases for a factorGraph object.

You can connect the factorIMU object to a gravity rotation node to enable pose representation in the sensor frame, then transform the pose from the sensor frame to the local navigation reference frame of the IMU by performing left multiplication of the inverse of the gravity rotation value with the sensor pose. To estimate the gravity rotation value, use the estimateGravityRotation function.

To refine the gravity rotation estimate during factor graph optimization, free the gravity rotation node by using the fixNode function with the flag argument specified as false.

You can also connect the factorIMU object to a sensor transform node to enable estimation and refinement of the sensor transform. This is useful when calibrating the extrinsic transform between an IMU and another sensor. For details about these processes, see Multi-Sensor Extrinsic Calibration using Factor Graph.

Creation

Syntax

F = factorIMU(nodeID,GyroscopeReadings,AccelerometerReadings)

F = factorIMU(nodeID,GyroscopeReadings,AccelerometerReadings,imuparams)

F = factorIMU(nodeID,SampleRate,GyroscopeBiasNoise,AccelerometerBiasNoise,GyroscopeNoise,AccelerometerNoise,GyroscopeReadings,AccelerometerReadings)

F = factorIMU(___,Name=Value)

Description

F = factorIMU(nodeID,GyroscopeReadings,AccelerometerReadings) creates a factorIMU object, F, with the specified node identification numbers property NodeID set to nodeID, and with the gyroscope readings and accelerometer readings properties set to the values of their corresponding arguments.

F = factorIMU(nodeID,GyroscopeReadings,AccelerometerReadings,imuparams) specifies IMU parameters, such as sampling rate, gyroscope bias noise, and accelerometer bias noise, as a factorIMUParameters object.

F = factorIMU(nodeID,SampleRate,GyroscopeBiasNoise,AccelerometerBiasNoise,GyroscopeNoise,AccelerometerNoise,GyroscopeReadings,AccelerometerReadings) creates a factorIMU object, F, with the specified node identification numbers property NodeID set to nodeID, and with a sample rate, gyroscope bias noise, accelerometer bias noise, gyroscope noise, accelerometer noise, gyroscope readings, and accelerometer readings set to their corresponding values, respectively.

F = factorIMU(___,Name=Value) sets writable properties using one or more name-value arguments. For example, ReferenceFrame="NED" sets the ReferenceFrame property of the factorIMU object to "NED".

Input Arguments

nodeID — Node ID numbers

row vector of nonnegative integers

Node ID numbers, specified as a row vector of nonnegative integers.

• Six element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2] — The factorIMU object expects the nodes of the corresponding node IDs to have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS].

• Seven element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2 GravityRotationID] — The factorIMU object expects the nodes of the corresponding node IDs to have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS, GRAVITY_ROTATION].

• Eight element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2 GravityRotationID SensorTransformID] — The factorIMU object expects the nodes of the corresponding node IDs to have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS, GRAVITY_ROTATION, TRANSFORM_SE3]. In this case, factorIMU ignores any specified value for the SensorTransform property, and sets it to an SE(3) transformation representing an identity rotation with no translation.

Adding the factorIMU object to a factor graph also adds any nodes specified to nodeID that do not already exist in the factor graph. Each added node has a type corresponding to its position in the nodeID argument.

This argument sets the NodeID property.

For more information about the expected node types of all supported factors, see Expected Node Types of Factor Objects.

GyroscopeReadings — Gyroscope readings

N-by-3 matrix

Gyroscope readings, specified as an N-by-3 matrix, where N is the number of readings. The specified gyroscope readings are preintegrated into the factor. The GyroscopeReadings and AccelerometerReadings arguments must have the same size.

This argument sets the GyroscopeReadings property.

AccelerometerReadings — Accelerometer readings

N-by-3 matrix

Accelerometer readings, specified as an N-by-3 matrix, where N is the number of readings. The specified accelerometer readings are preintegrated into the factor. The GyroscopeReadings and AccelerometerReadings arguments must have the same size.

This argument sets the AccelerometerReadings property.

imuparams — Factor IMU parameters

factorIMUParameters (default) | factorIMUParameters object

Factor IMU parameters, specified as a factorIMUParameters object.

This argument sets the SampleRate, GyroscopeBiasNoise, AccelerometerBiasNoise, GyroscopeNoise, and AccelerometerNoise properties.

SampleRate — IMU sampling rate

100 (default) | numeric scalar greater than or equal to 100

IMU sampling rate, in Hz, specified as a numeric scalar greater than or equal to 100.

This argument sets the SampleRate property.

GyroscopeBiasNoise — Gyroscope bias noise covariance

eye(3) (default) | 3-by-3 matrix

Gyroscope bias noise covariance, specified as a 3-by-3 matrix.

This argument sets the GyroscopeBiasNoise property.

AccelerometerBiasNoise — Accelerometer bias noise covariance

eye(3) (default) | 3-by-3 matrix

Accelerometer bias noise covariance, specified as a 3-by-3 matrix.

This argument sets the AccelerometerBiasNoise property.

GyroscopeNoise — Gyroscope measurement noise covariance

eye(3) (default) | 3-by-3 matrix

Gyroscope measurement noise covariance, specified as a 3-by-3 matrix.

This argument sets the GyroscopeNoise property.

AccelerometerNoise — Accelerometer measurement noise covariance

eye(3) (default) | 3-by-3 matrix

Accelerometer measurement noise covariance, specified as a 3-by-3 matrix.

This argument sets the AccelerometerNoise property.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: factorIMU(nodeID,gyroReadings,accelReadings,ReferenceFrame="NED") creates an IMU factor with a local coordinate system of north-east-down (NED).

ReferenceFrame — Reference frame for local coordinate system

"ENU" (default) | "NED"

Reference frame for the local coordinate system, specified as "ENU" (east-north-up) or "NED" (north-east-down).

This argument sets the ReferenceFrame property.

SensorTransform — Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to initial IMU frame

se3 (default) | se3 object

Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial IMU sensor reference frame, specified as an se3 object.

A sensor transform is unnecessary if the connecting pose nodes contain poses in the initial IMU sensor reference frame. Otherwise, you must specify the sensor transform.

You can use the estimateCameraIMUTransform function to estimate this transformation.

This argument sets the SensorTransform property.

If nodeID is an eight-element row vector of nonnegative integers, then factorIMU ignores any value specified by the SensorTransform argument and sets the SensorTransform property to an SE(3) transformation representing an identity rotation with no translation.

Output Arguments

F — Factor connecting poses, velocities, and IMU biases

factorIMU object

Factor connecting poses, velocities, IMU biases, returned as a factorIMU object. Optionally, this factor can include gravity rotation and sensor transform nodes, depending on how you specify the nodeID argument.

• If nodeID is a six-element row vector of nonnegative integers, then F is a factorIMU object that describes the constraints between poses, velocities, and IMU biases.

• If nodeID is a seven-element row vector of nonnegative integers, then F is a factorIMU object that describes the constraints between poses, velocities, IMU biases, and a gravity rotation node.

• If nodeID is a eight-element row vector of nonnegative integers, then F is a factorIMU object that describes the constraints between poses, velocities, IMU biases, a gravity rotation node, and a sensor transform node.

Properties

NodeID — Node ID numbers

row vector of nonnegative integers

This property is read-only after object creation. To set this property, use the nodeID argument when calling the factorIMU function.

Node ID numbers, represented as a row vector of nonnegative integers.

• Six element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2] — The nodes of the corresponding node IDs have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS].

• Seven element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2 GravityRotationID] — The nodes of the corresponding node IDs have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS, GRAVITY_ROTATION].

• Eight element row vector of the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2 GravityRotationID SensorTransformID] — The nodes of the corresponding node IDs have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS, GRAVITY_ROTATION, TRANSFORM_SE3]. In this case, factorIMU ignores any specified value for the SensorTransform property, and sets it to an SE(3) transformation representing an identity rotation with no translation.

Adding the factorIMU object to a factor graph also adds any nodes in NodeID property that do not already exist in the factor graph. Each added node has a type corresponding to its position in the NodeID property.

For more information about the expected node types of all supported factors, see Expected Node Types of Factor Objects.

SampleRate — IMU sampling rate

100 (default) | numeric scalar greater than or equal to 100

This property is read-only after object creation. To set this property, use the SampleRate or imuparams argument when calling the factorIMU function.

IMU sampling rate, in Hz, represented as a numeric scalar greater than or equal to 100.

GyroscopeBiasNoise — Gyroscope bias noise covariance

eye(3) (default) | 3-by-3 matrix

This property is read-only after object creation. To set this property, use the GyroscopeBiasNoise or imuparams argument when calling the factorIMU function.

Gyroscope bias noise covariance, represented as a 3-by-3 matrix.

AccelerometerBiasNoise — Accelerometer bias noise covariance

eye(3) (default) | 3-by-3 matrix

This property is read-only after object creation. To set this property, use the AccelerometerBiasNoise or imuparams argument when calling the factorIMU function.

Accelerometer bias noise covariance, represented as a 3-by-3 matrix.

GyroscopeNoise — Gyroscope measurement noise covariance

eye(3) (default) | 3-by-3 matrix

This property is read-only after object creation. To set this property, use the GyroscopeNoise or imuparams argument when calling the factorIMU function.

Gyroscope measurement noise covariance, represented as a 3-by-3 matrix.

AccelerometerNoise — Accelerometer measurement noise covariance

eye(3) (default) | 3-by-3 matrix

This property is read-only after object creation. To set this property, use the GyroscopeNoise or imuparams argument when calling the factorIMU function.

Accelerometer measurement noise covariance, represented as a 3-by-3 matrix.

GyroscopeReadings — Gyroscope readings

N-by-3 matrix

This property is read-only after object creation. To set this property, use the GyroscopeReadings argument when calling the factorIMU function.

Gyroscope readings, represented as an N-by-3 matrix, where N is the number of readings. The specified gyroscope readings are preintegrated into the factor. GyroscopeReadings and AccelerometerReadings properties must have the same size.

AccelerometerReadings — Accelerometer readings

N-by-3 matrix

This property is read-only after object creation. To set this property, use the AccelerometerReadings argument when calling the factorIMU function.

Accelerometer readings, represented as an N-by-3 matrix, where N is the number of readings. The specified accelerometer readings are preintegrated into the factor. GyroscopeReadings and AccelerometerReadings properties must have the same size.

ReferenceFrame — Reference frame for local coordinate system

"ENU" (default) | "NED"

Reference frame for the local coordinate system, specified as "ENU" (east-north-up) or "NED" (north-east-down).

Data Types: string | char

SensorTransform — Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to initial IMU frame

se3 (default) | se3 object

This property is read-only after object creation.

Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial IMU sensor reference frame, represented as an se3 object.

For example, if the connected pose nodes store camera poses in the initial camera sensor reference frame, the sensor transform rotates and translates a pose in the initial camera sensor reference frame to the initial IMU sensor reference frame. The initial sensor reference frame has the very first sensor pose at its origin.

To set this property, use the SensorTransform argument when calling the factorIMU function.

If the nodeID argument is an eight-element row vector of nonnegative integers, then factorIMU ignores any specified value for the SensorTransform argument and sets the SensorTransform property to an SE(3) transformation representing an identity rotation with no translation.

Object Functions

nodeType	Get node type of node in factor graph
predict	Predict pose and velocity of factor

Examples

Add factorIMU Object to Factor Graph

Set up parameters such as the connected node IDs, sample rate, noise, and readings. Then create an IMU factor with these parameters as arguments.

nodeID = [1,2,3,4,5,6];
sampleRate = 400; % Hz
gyroBiasNoise = 1.5e-9 * eye(3);
accelBiasNoise = diag([9.62e-9, 9.62e-9, 2.17e-8]);
gyroNoise = 6.93e-5 * eye(3);
accelNoise = 2.9e-6 * eye(3); 
gyroReadings = [ -0.0151    0.0299    0.0027
                -0.0079    0.0370   -0.0014
                -0.0320    0.0306    0.0035
                -0.0043    0.0340   -0.0066
                -0.0033    0.0331   -0.0011];
accelReadings = [   1.0666    0.0802    9.9586
                   1.1002    0.0199    9.6650
                   1.0287    0.3071   10.1864
                   0.9077   -0.2239   10.2989
                   1.2322    0.0174    9.8411];
  
f = factorIMU(nodeID, sampleRate, gyroBiasNoise, accelBiasNoise, ...
             gyroNoise, accelNoise, gyroReadings, accelReadings, ReferenceFrame="NED");

Create a default factor graph and add the factor to the graph using the addFactor function.

g = factorGraph;
addFactor(g,f);

More About

Expected Node Types of Factor Objects

The NodeID property of each factor object specifies and connects to these node types:

Factor Object	Expected Node Types of Specified Node IDs
factorGPS	["POSE_SE3"]
factorIMU	["POSE_SE3","VEL3","IMU_BIAS","POSE_SE3","VEL3","IMU_BIAS"]
factorCameraSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"] or ["POSE_SE3","POINT_XYZ","TRANSFORM_SE3"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorTwoPoseSIM3	["POSE_SE3","POSE_SE3_SCALE","POSE_SE3","POSE_SE3_SCALE"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]
factorTransformSE3Prior	["TRANSFORM_SE3"]

For example, factorPoseSE2AndPointXY([1 2]) creates a 2-D landmark factor connecting to node IDs 1 and 2. If you try to add that factor to a factor graph that already contains nodes with the IDs 1 and 2, the factor expects nodes 1 and 2 to be of types "POSE_SE2" and "POINT_XY", respectively.

References

[1] Forster, Christian, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.” IEEE Transactions on Robotics 33, no. 1 (February 2017): 1–21. https://doi.org/10.1109/TRO.2016.2597321.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a

R2026a: Connect to gravity rotation node to enable pose representation in sensor frame

You can now connect the factorIMU object to a gravity rotation node by specifying the node identification number for the gravity rotation node as the seventh element of a seven-element or eight-element nodeID argument. This enables pose representation in the sensor frame. You can transform the pose in the sensor frame to the local navigation reference frame of an IMU by left-multiplying the inverse of the gravity rotation value with the sensor pose. To estimate the gravity rotation, use the estimateGravityRotation function.

R2026a: Estimate or refine sensor transform

You can now estimate the SensorTransform property using factor graph optimization by connecting a sensor transform node to the factorIMU factor. To establish this connection, specify the node identification number for a sensor transform node as the eighth element of the nodeID argument.

You can also refine the sensor transform estimate using factor graph optimization by freeing the sensor transform node during optimization.