diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1419f1773..feb840ad4 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,25 +11,26 @@ clc clear all close all -saveResults = 0; +saveResults = 1; %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses +options.includeBetweenFactors = 1; % 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.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements +options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors 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 = 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 = 50; % number of Monte Carlo runs to perform +numMonteCarloRuns = 100; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -53,8 +54,8 @@ metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); metadata.imu.AccelerometerSigma = 1e-3; metadata.imu.GyroscopeSigma = 1e-5; metadata.imu.IntegrationSigma = 1e-5; -metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; +metadata.imu.BiasAccelerometerSigma = 1e-4; %metadata.imu.epsBias; +metadata.imu.BiasGyroscopeSigma = 1e-4; %metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; @@ -154,8 +155,8 @@ for k=1:numMonteCarloRuns % Create a random bias for each run if options.imuNonzeroBias == 1 - metadata.imu.accelBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); - metadata.imu.gyroBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); + metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); + metadata.imu.gyroConstantBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); else metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 921d42679..b134a08d3 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -146,7 +146,7 @@ for i=0:length(measurements) end % end of Camera factor creation %% Add GPS factors - if options.includeGPSFactors == 1 + if options.includeGPSFactors == 1 && i >= options.gpsStartPose gpsPosition = measurements(i).gpsPositionVector ... + measurementNoise.gpsNoiseVector .* randn(3,1); graph.add(PriorFactorPose3(currentPoseKey, ...