diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 49a1c85ad..a2a7c3a7c 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -19,18 +19,18 @@ options.includeBetweenFactors = 0; % if true, BetweenFactors will be added betwe 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 +options.includeCameraFactors = 0; % not fully implemented 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.trajectoryLength = 50; %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; % number of Monte Carlo runs to perform +numMonteCarloRuns = 20; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -49,33 +49,43 @@ if options.includeCameraFactors == 1 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 + metadata.imu.epsBias = 1e-10; % was 1e-7 -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 = 1e-10; %metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = 1e-10; %metadata.imu.epsBias; -metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; -noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 -noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases -noisePriorBias = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1); ... - metadata.imu.BiasGyroscopeSigma * ones(3,1)]); +metadata.imu.IntegrationSigma = 1e-5; +metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +metadata.imu.AccelerometerSigma = sigma_accel; +metadata.imu.GyroscopeSigma = sigma_gyro; +metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; % noise on expected change in accelerometer bias over time +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; % noise on expected change in gyroscope bias over time +% noise on initial accelerometer and gyroscope biases +if options.imuNonzeroBias == 1 + metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)]; +else + metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1); +end -sigma_accel = metadata.imu.AccelerometerSigma; -sigma_gyro = metadata.imu.GyroscopeSigma; -noiseVectorAccel = sigma_accel * ones(3,1); -noiseVectorGyro = sigma_gyro * ones(3,1); +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 +noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);... + metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases +noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); + +noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); +noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); %% Between metadata -sigma_ang = 1e-2; sigma_cart = 1e-1; +sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 +sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); %% GPS metadata -sigma_gps = 1e-4; +sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); @@ -90,7 +100,7 @@ folderName = 'results/' % Set up noise models gtNoiseModels.noisePose = noisePose; gtNoiseModels.noiseVel = noiseVel; -gtNoiseModels.noiseBias = noiseBias; +gtNoiseModels.noiseBiasBetween = noiseBiasBetween; gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; gtNoiseModels.noiseGPS = noiseGPS; @@ -138,7 +148,7 @@ disp('Plotted ground truth') % Set up noise models monteCarloNoiseModels.noisePose = noisePose; monteCarloNoiseModels.noiseVel = noiseVel; -monteCarloNoiseModels.noiseBias = noiseBias; +monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; monteCarloNoiseModels.noisePriorPose = noisePose; monteCarloNoiseModels.noisePriorBias = noisePriorBias; monteCarloNoiseModels.noiseGPS = noiseGPS; @@ -151,12 +161,14 @@ monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; for k=1:numMonteCarloRuns - fprintf('Monte Carlo Run %d...', k'); + fprintf('Monte Carlo Run %d...\n', k'); % Create a random bias for each run if options.imuNonzeroBias == 1 - metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); - metadata.imu.gyroConstantBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(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); @@ -176,8 +188,6 @@ for k=1:numMonteCarloRuns % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); - fprintf('\n\tEstimate error: %g \n', graph.error(estimate) ); - fprintf('\tEstimate error (GT): %g \n', graph.error(gtValues) ); figure(1) plot3DTrajectory(estimate, '-b'); @@ -195,7 +205,6 @@ for k=1:numMonteCarloRuns % compute covariances: cov = marginals.marginalCovariance(currentPoseKey); covPosition = estR * cov(4:6,4:6) * estR'; - % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof end diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 2f9514b06..038a0eb04 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -19,11 +19,7 @@ for i=0:length(measurements) if i==0 %% first time step, add priors % Pose prior (poses used for all factors) - if options.includeBetweenFactors == 1 - initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); - else - initialPose = Pose3; - end + initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); % IMU velocity and bias priors @@ -90,19 +86,21 @@ for i=0:length(measurements) 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))); + noiseModels.noiseBiasBetween)); end if options.imuFactorType == 2 + use2ndOrderIntegration = true; % 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)); + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... % how bias changes over time + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... % how bias changes over time + diag(metadata.imu.BiasAccOmegaInit.^2), ... % prior on bias + use2ndOrderIntegration); % Generate IMU measurements with noise for j=1:length(measurements(i).imu) % all measurements to preintegrate imuAccel = measurements(i).imu(j).accel ...