Main Content

factorIMU

Convert IMU readings to factor

Description

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

Creation

Description

example

F = factorIMU(nodeID,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.

F = factorIMU(___,ReferenceFrame="NED") specifies the reference frame for the local coordinate system as north-east-down (NED) instead of east-north-up (ENU), in addition to all input arguments from the previous syntax.

Properties

expand all

This property is read-only.

Node ID numbers, specified as a six-element row vector. This indicates which nodes in the factor graph this factor connects to.

You must specify this property at object creation.

This property is read-only.

IMU sampling rate, in Hz, specified as a positive 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 as "ENU" (east-north-up) or "NED" (north-east-down).

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

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.

Version History

Introduced in R2022a