25 lines
1006 B
Matlab
25 lines
1006 B
Matlab
function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ...
|
|
initialPose, omega1Body, velocity1Body, velocity2Body, deltaT)
|
|
%INTEGRATETRAJECTORY Integrate one trajectory step
|
|
|
|
import gtsam.*;
|
|
% Rotation: R^1_2
|
|
body2in1 = Rot3.Expmap(omega1Body * deltaT);
|
|
% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
|
|
velocity2inertial = body2in1.rotate(Point3(velocity2Body));
|
|
% Acceleration: a^1 = (v^1_2 - v^1_1)/dt
|
|
accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
|
|
|
|
% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
|
|
initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body));
|
|
% Acceleration in frame W: a^W = R^W_1 a^1
|
|
accelGlobal = initialPose.rotation().rotate(Point3(accelBody1));
|
|
|
|
finalPosition = Point3(initialPose.translation + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
|
|
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
|
|
finalRotation = initialPose.rotation.compose(body2in1);
|
|
finalPose = Pose3(finalRotation, finalPosition);
|
|
|
|
end
|
|
|