12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed velocity in rotating frame

release/4.3a0
Luca 2014-02-13 17:32:16 -05:00
parent a1e6d84e22
commit a01fe12ee6
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -200,20 +200,27 @@ for i = 1:length(times)
currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
% Rotate pose in fixed frame to get pose in rotating frame % Rotate pose in fixed frame to get pose in rotating frame
previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
% Get velocity in rotating frame by treating it like a position and using compose % Get velocity in rotating frame by treating it like a position and using compose
% Maybe Luca knows a better way to do this within GTSAM. % Maybe Luca knows a better way to do this within GTSAM.
currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); %currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame));
currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); %currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT);
%currentRot3RotatingGT = currentRotatingFrame.rotation();
%currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT));
% TODO: check if initial velocity in rotating frame is correct
currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT;
% Store GT (ground truth) poses % Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
velocityInFixedGT(:,i) = currentVelocityFixedGT; velocityInFixedGT(:,i) = currentVelocityFixedGT;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
velocityInRotatingGT(:,i) = currentVelocityPoseRotatingGT.translation.vector; velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
@ -265,6 +272,7 @@ for i = 1:length(times)
% Add values to the graph. Use the current pose and velocity % Add values to the graph. Use the current pose and velocity
% estimates as to values when interpreting the next pose and % estimates as to values when interpreting the next pose and
% velocity estimates % velocity estimates
%ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU);
newValues.insert(currentPoseKey, currentPoseEstimate); newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);