From 8f4c3fd02d1e82930f52fd69f3cc9e169916dee4 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 13 Feb 2014 16:01:35 -0500 Subject: [PATCH] Fixed issue with velocity errors --- matlab/unstable_examples/+imuSimulator/coriolisExample.m | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index f1e15581c..6a19243da 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -17,7 +17,7 @@ import gtsam.*; addpath(genpath('./Libraries')) %% General configuration -navFrameRotating = 1; % 0 = perform navigation in the fixed frame +navFrameRotating = 0; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 useRealisticValues = 1; % use reaslist values for initial position and earth rotation @@ -184,6 +184,7 @@ for i = 1:length(times) positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; else @@ -205,7 +206,7 @@ for i = 1:length(times) % 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. - currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT)); + currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); % Store GT (ground truth) poses