From a0a955e5a5d4967802474a68f65a7313822cf5d6 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Thu, 17 Apr 2014 16:14:32 -0400
Subject: [PATCH] fixing imu simulator for non-identical rotations

---
 .../covarianceAnalysisCreateFactorGraph.m          |  3 +--
 .../covarianceAnalysisCreateTrajectory.m           | 14 +++++---------
 .../+imuSimulator/getPoseFromGtScenario.m          |  2 +-
 3 files changed, 7 insertions(+), 12 deletions(-)

diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
index cb10d14cd..d19619474 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
@@ -84,8 +84,7 @@ for i=0:length(measurements)
         graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
           noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
       end
-      
-      
+          
       if options.imuFactorType == 2
         %         % Initialize preintegration
         %         imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
index 105fe3e80..5f225f4df 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
@@ -9,8 +9,7 @@ import gtsam.*;
 
 values = Values;
 
-warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
-warning('using identity rotation')
+warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - using identity rotation')
 
 if options.useRealData == 1
   %% Create a ground truth trajectory from Real data (if available)
@@ -55,11 +54,8 @@ if options.useRealData == 1
           scenarioIndIMU2 = previousScenarioInd+j;
           IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
           IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
-          
-%           scenarioIndIMU1
-%           IMUPose1.print('IMUPose1');
-%           IMUPose2.print('IMUPose2');
-          
+          IMURot2 = IMUPose2.rotation.matrix;
+                    
           IMUdeltaPose = IMUPose1.between(IMUPose2);
           IMUdeltaPoseVector     = Pose3.Logmap(IMUdeltaPose);
           IMUdeltaRotVector      = IMUdeltaPoseVector(1:3);
@@ -72,10 +68,10 @@ if options.useRealData == 1
           
           % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
           % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
-          measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
+          measurements(i).imu(j).accel = IMURot2' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
           
           % Update velocity
-          currentVel = currentVel + measurements(i).imu(j).accel * deltaT;
+          currentVel = currentVel + IMURot2 * measurements(i).imu(j).accel * deltaT;
         end
       end
       
diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
index 73c60efc3..bf8da3343 100644
--- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
+++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
@@ -19,7 +19,7 @@ dZ = gtECEF(3) - initialPositionECEF(3);
 [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
 
 gtPosition = [xlt, ylt, zlt]';
-gtRotation = Rot3; %Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
+gtRotation = Rot3; % Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
 currentPose = Pose3(gtRotation, Point3(gtPosition));
 
 end
\ No newline at end of file