predict
Description
[
predicts the pose predictedpose
,predictedvel
] = predict(factor
,prevpose
,prevvel
,prevbias
)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
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)