diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 335ce9f6a..35b2dd52d 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/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.trajectoryLength = 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 = 2; +numMonteCarloRuns = 2; % number of Monte Carlo runs to perform %% Camera metadata numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index f27dbd02f..ca0ce61b8 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -63,23 +63,47 @@ 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 - % Initialize preintegration - imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... - metadata.imu.zeroBias, ... - metadata.imu.AccelerometerSigma.^2 * eye(3), ... - metadata.imu.GyroscopeSigma.^2 * eye(3), ... - metadata.imu.IntegrationSigma.^2 * eye(3)); - % Preintegrate - imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); - % Add Imu factor - graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... - currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); - % Add between factor on biases - graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); - % Additional prior on zerobias - graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + + 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, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3)); + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + % 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 !!!! - diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index d19c5cf79..d0e58afdf 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/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