factorIMU

Convert IMU readings to factor

Since R2022a

Description

The factorIMU object converts raw IMU readings into constraints between poses, velocities, and IMU biases for a factorGraph object.

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) specifies properties using one or more name-value arguments in addition to all input arguments from the previous syntax.

Input Arguments

imuparams — Factor IMU parameters

factorIMUParameters object

Factor IMU parameters, specified as a factorIMUParameters object.

Properties

NodeID — Node ID numbers

six-element row vector of integers

This property is read-only.

Node ID numbers, specified as a six-element row vector in the form [PoseID1 VelocityID1 IMUBiasID1 PoseID2 VelocityID2 IMUBiasID2]. The factorIMU object factor expects the nodes of the corresponding node IDs to have the node types [POSE_SE3, VEL3, IMU_BIAS, POSE_SE3, VEL3, IMU_BIAS] and connects those nodes in the factor graph using the specified node IDs.

If a factor in the factorIMU object specifies IDs that do not correspond to a nodes in the factor graph, the factor graph automatically creates POSE_SE3, VEL3, and IMU_BIAS type nodes with the corresponding IDs and adds the nodes to the factor graph when adding the factor to the factor graph.

You must specify this property at object creation.

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

SampleRate — IMU sampling rate

numeric scalar greater than 100

This property is read-only.

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

You must specify this property at object creation.

GyroscopeBiasNoise — Gyroscope bias noise covariance

3-by-3 matrix

This property is read-only.

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

You must specify this property at object creation.

AccelerometerBiasNoise — Accelerometer bias noise covariance

3-by-3 matrix

This property is read-only.

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

You must specify this property at object creation.

GyroscopeNoise — Gyroscope measurement noise covariance

3-by-3 matrix

This property is read-only.

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

You must specify this property at object creation.

AccelerometerNoise — Accelerometer measurement noise covariance

3-by-3 matrix

This property is read-only.

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

You must specify this property at object creation.

GyroscopeReadings — Gyroscope readings

N-by-3 matrix

This property is read-only.

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. GyroscopeReadings and AccelerometerReadings must have the same size.

You must specify this property at object creation.

AccelerometerReadings — Accelerometer readings

N-by-3 matrix

This property is read-only.

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. GyroscopeReadings and AccelerometerReadings must have the same size.

You must specify this property at object creation.

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

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.

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.

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.

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

These are the node types that the NodeID property of each factor object specifies and connects to:

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"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]

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