Main Content

predict

Predict pose and velocity of factor

Since R2022a

collapse all in page

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.

example

Examples

collapse all

Open Live Script

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

collapse all

IMU factor, specified as a factorIMU object.

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

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

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

collapse all

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

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

See Also

|

Topics