changed timestep to make IMU measurements more realistic. removed noise model from IMU type 2 constructor

matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m

@@ -2,7 +2,7 @@ import gtsam.*;
 
 % Test GTSAM covariances on a factor graph with:
 % Between Factors
-% IMU factors
+% IMU factors (type 1 and type 2)
 % Projection factors
 % Authors: Luca Carlone, David Jensen
 % Date: 2014/4/6
@@ -18,7 +18,7 @@ options.useRealData = 1;           % controls whether or not to use the real dat
 options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses
 
 options.includeIMUFactors = 1;     % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
-options.imuFactorType = 1;         % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
+options.imuFactorType = 2;         % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
 options.imuNonzeroBias = 1;        % if true, a nonzero bias is applied to IMU measurements
 
 options.includeCameraFactors = 0;  % not fully implemented yet
@@ -27,7 +27,7 @@ numberOfLandmarks = 10;            % Total number of visual landmarks, used for
 options.includeGPSFactors = 0;     % if true, GPS factors will be added as priors to poses
 options.gpsStartPose = 100;        % Pose number to start including GPS factors at
 
-options.trajectoryLength = 50; %209;    % length of the ground truth trajectory
+options.trajectoryLength = 209;%209;    % length of the ground truth trajectory
 options.subsampleStep = 20;        % number of poses to skip when using real data (to reduce computation on long trajectories)
 
 numMonteCarloRuns = 20;             % number of Monte Carlo runs to perform
@@ -51,8 +51,8 @@ end
 %% Imu metadata
 sigma_accel = 1e-3;         % std. deviation for accelerometer noise, typical 1e-3
 sigma_gyro = 1e-5;          % std. deviation for gyroscope noise, typical 1e-5
-sigma_accelBias = 1e-3;     % std. deviation for added accelerometer constant bias, typical 1e-3
-sigma_gyroBias = 1e-5;      % std. deviation for added gyroscope constant bias, typical 1e-5
+sigma_accelBias = 1e-4;     % std. deviation for added accelerometer constant bias, typical 1e-3
+sigma_gyroBias = 1e-6;      % std. deviation for added gyroscope constant bias, typical 1e-5
 
 metadata.imu.epsBias = 1e-10; % was 1e-7
 metadata.imu.g = [0;0;0];
@@ -132,6 +132,7 @@ figure(1)
 hold on;
 plot3DPoints(gtValues);
 plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
+%plot3DTrajectory(gtValues, '-r');
 axis equal
 
 % optimize
@@ -165,10 +166,10 @@ for k=1:numMonteCarloRuns
 
   % Create a random bias for each run
   if options.imuNonzeroBias == 1
-    %metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1);
-    %metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1);
-    metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1);
-    metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1);
+    metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1);
+    metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1);
+    %metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1);
+    %metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1);
   else
     metadata.imu.accelConstantBiasVector = zeros(3,1);
     metadata.imu.gyroConstantBiasVector = zeros(3,1);

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m

@@ -69,13 +69,28 @@ for i=0:length(measurements)
           use2ndOrderIntegration);
         % Generate IMU measurements with noise
         for j=1:length(measurements(i).imu) % all measurements to preintegrate
+          accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1));
           imuAccel = measurements(i).imu(j).accel ...
-            + (measurementNoise.imu.accelNoiseVector .* randn(3,1))...  % added noise
+            + accelNoise ...  % added noise
             + metadata.imu.accelConstantBiasVector;     % constant bias
+        
+          gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1));
           imuGyro = measurements(i).imu(j).gyro ...
-            + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))...   % added noise
+            + gyroNoise ...   % added noise
             + metadata.imu.gyroConstantBiasVector;      % constant bias
           
+          % Used for debugging
+          %fprintf('  A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
+          %    measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ...
+          %    accelNoise(1), accelNoise(2), accelNoise(3), ...
+          %    metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ...
+          %    imuAccel(1), imuAccel(2), imuAccel(3));
+          %fprintf('  G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
+          %    measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ...
+          %    gyroNoise(1), gyroNoise(2), gyroNoise(3), ...
+          %    metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ...
+          %    imuGyro(1), imuGyro(2), imuGyro(3));
+          
           % Preintegrate
           imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
         end
@@ -113,14 +128,14 @@ for i=0:length(measurements)
           % Preintegrate
           imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
         end
+        
         % 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)));
+          metadata.imu.g, metadata.imu.omegaCoriolis));
       end
       
     end % end of IMU factor creation

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m

@@ -43,7 +43,7 @@ if options.useRealData == 1
     if options.includeIMUFactors == 1
       currentVelKey = symbol('v', i);
       currentBiasKey = symbol('b', i);
-      deltaT = 0.01;   % amount of time between IMU measurements
+      deltaT = 1;   % amount of time between IMU measurements
       if i == 0
         currentVel = [0 0 0]';
       else