Added option for type 2 IMU factors

matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m

@@ -12,12 +12,13 @@ close all
 %% Configuration
 options.useRealData = 1;           % controls whether or not to use the real data (if available) as the ground truth traj
 options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses
-options.includeIMUFactors = 1;     % if true, IMU type 1 Factors will be generated for the trajectory
+options.includeIMUFactors = 1;     % if true, IMU factors will be generated for the trajectory based on options.imuFactorType
+options.imuFactorType = 1;         % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
 options.includeCameraFactors = 0;  % not fully implemented yet
 options.trajectoryLength = 209;    % length of the ground truth trajectory
-options.subsampleStep = 20;
+options.subsampleStep = 20;        % number of poses to skip when using real data (to reduce computation on long trajectories)
 
-numMonteCarloRuns = 2;
+numMonteCarloRuns = 2;             % number of Monte Carlo runs to perform
 
 %% Camera metadata
 numberOfLandmarks = 10;    % Total number of visual landmarks, used for camera factors

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m

@@ -63,6 +63,28 @@ for i=0:size(measurements.deltaMatrix, 1);
           + (measurementNoise.imu.accelNoiseVector .* randn(3,1));  % added noise
       imuGyro = measurements.imu.gyro(i,:)' ...
           + (measurementNoise.imu.gyroNoiseVector .* randn(3,1));   % added noise
+
+      if options.imuFactorType == 2
+        % Initialize preintegration
+        imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
+          metadata.imu.zeroBias, ...
+          metadata.imu.AccelerometerSigma.^2 * eye(3), ...
+          metadata.imu.GyroscopeSigma.^2 * eye(3), ...
+          metadata.imu.IntegrationSigma.^2 * eye(3), ...
+          metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
+          metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
+          metadata.imu.BiasAccOmegaInit.^2 * eye(6));
+        % Preintegrate
+        imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
+        % Add Imu factor
+        graph.add(CombinedImuFactor( ...
+          currentPoseKey-1, currentVelKey-1, ...
+          currentPoseKey, currentVelKey, ...
+          currentBiasKey-1, currentBiasKey, ...
+          imuMeasurement, ...
+          metadata.imu.g, metadata.imu.omegaCoriolis, ...
+          noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
+      else % Assumed to be type 1 if type 2 is not selected
         % Initialize preintegration
         imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
           metadata.imu.zeroBias, ...
@@ -80,6 +102,8 @@ for i=0:size(measurements.deltaMatrix, 1);
         % Additional prior on zerobias
         graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
           noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); 
+      end
+     
     end % end of IMU factor creation
     
     %% Add Camera factors - UNDER CONSTRUCTION !!!! -

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m

@@ -21,13 +21,17 @@ if options.useRealData == 1
   
   % Limit the trajectory length
   options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
-  
+  fprintf('Scenario Ind: ');
   for i=1:options.trajectoryLength+1
     % Update the pose key
     currentPoseKey = symbol('x', i-1);
     
     % Generate the current pose 
-    scenarioInd = options.subsampleStep * (i-1) + 1
+    scenarioInd = options.subsampleStep * (i-1) + 1;
+    fprintf('%d, ', scenarioInd);
+    if mod(i,20) == 0
+      fprintf('\n');
+    end
     gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
     % truth in ENU
     dX = gtECEF(1) - initialPositionECEF(1);
@@ -50,6 +54,7 @@ if options.useRealData == 1
       measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
     end
   end
+  fprintf('\n');
 else
   %% Create a random trajectory as ground truth
   currentPose = Pose3; % initial pose  % initial pose