From 862f5c7af3cae1c7ce7caf7ef52eefa248413f8d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 29 Apr 2014 15:46:43 -0400 Subject: [PATCH] changed timestep to make IMU measurements more realistic. removed noise model from IMU type 2 constructor --- .../+imuSimulator/covarianceAnalysisBetween.m | 19 +++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 23 +++++++++++++++---- .../covarianceAnalysisCreateTrajectory.m | 2 +- 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index a2a7c3a7c..4f60ae2ef 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/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); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 038a0eb04..d87fa490b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 3933870b5..b66df3fc9 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/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