predict

Predict pose and velocity of factor

Since R2022a

Syntax

[predictedpose,predictedvel] = predict(factor,prevpose,prevvel,prevbias)

Description

[predictedpose,predictedvel] = predict(factor,prevpose,prevvel,prevbias) predicts the pose predictpose and velocity predictedvel of the factor factor based on IMU readings and the initial pose, prevpose, velocity prevvel, and biasprevbias.

Examples

Predict Pose and Velocity of IMU Factor

Predict the next pose and velocity of an IMU factor based on the previous pose, velocity, and biases.

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

Predict Pose and Velocity

Set up previous pose, velocity and biases measurements to use to predict the next pose and velocity.

prevpose = rand(1,7);
prevvel = rand(1,3);
prevaccelbias = rand(1,3);
prevgyrobias = rand(1,3);
prevbiases = [prevgyrobias,prevaccelbias]

prevbiases = 1×6

    0.4854    0.8003    0.1419    0.1576    0.9706    0.9572

Use the predict function and the previous measurements to predict the next pose and velocity.

[predictedpose,predictedvel] = predict(f,prevpose,prevvel,prevbiases)

predictedpose = 1×7

    0.8220    0.9170    0.1383    0.6307    0.7048    0.1055    0.3071

predictedvel = 1×3

    0.6202    0.8395    0.8509

Input Arguments

factor — IMU factor

factorIMU object

IMU factor, specified as a factorIMU object.

prevpose — Previous pose of factor

seven-element vector

Previous pose of the factor, specified as a seven-element vector. The pose consists of the 3-D position and orientation quaternion of the factor of the form [x y z w qx qy qz].

prevvel — Previous 3-D velocity

three-element vector

Previous 3-D velocity, specified as a three-element vector of the form [vx vy vz].

prevbias — Previous 3-D biases of gyroscope and accelerometer

six-element vector

Previous 3-D biases of the gyroscope and accelerometer, specified as a six-element vector of the form [gx gy gz ax ay az].

Output Arguments

predictedpose — Predicted pose

seven-element vector

Predicted 3-D position and orientation quaternion, returned as a seven-element vector of the form [x y z w qx qy qz]..

predictedvel — Predicted velocity

three-element vector

predicted 3-D velocity, returned as a three-element vector of the form [vx vy vz].

Extended Capabilities

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

Version History

Introduced in R2022a