Main Content

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

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.

example

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

expand all

Factor IMU parameters, specified as a factorIMUParameters object.

Properties

expand all

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.

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.

This property is read-only.

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

You must specify this property at object creation.

This property is read-only.

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

You must specify this property at object creation.

This property is read-only.

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

You must specify this property at object creation.

This property is read-only.

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

You must specify this property at object creation.

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.

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.

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

Data Types: string | char

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.

You can use the estimateCameraIMUTransform function to estimate this transformation.

Object Functions

nodeTypeGet node type of node in factor graph
predictPredict pose and velocity of factor

Examples

collapse all

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

expand all

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