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 命令
您点击的链接对应于以下 MATLAB 命令:
请在 MATLAB 命令行窗口中直接输入以执行命令。Web 浏览器不支持 MATLAB 命令。
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)