12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed bug with IMU bias. added option to delay start of GPS factors

release/4.3a0
djensen3 2014-04-23 16:06:03 -04:00
parent 87f9e5bb2c
commit c1ab0053eb
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -11,25 +11,26 @@ clc
clear all clear all
close all close all
saveResults = 0; saveResults = 1;
%% Configuration %% Configuration
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj 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.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 = 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 implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors 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.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 = 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) 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 %% Camera metadata
K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration 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.AccelerometerSigma = 1e-3;
metadata.imu.GyroscopeSigma = 1e-5; metadata.imu.GyroscopeSigma = 1e-5;
metadata.imu.IntegrationSigma = 1e-5; metadata.imu.IntegrationSigma = 1e-5;
metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; metadata.imu.BiasAccelerometerSigma = 1e-4; %metadata.imu.epsBias;
metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; metadata.imu.BiasGyroscopeSigma = 1e-4; %metadata.imu.epsBias;
metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias;
metadata.imu.g = [0;0;0]; metadata.imu.g = [0;0;0];
metadata.imu.omegaCoriolis = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0];
@ -154,8 +155,8 @@ for k=1:numMonteCarloRuns
% Create a random bias for each run % Create a random bias for each run
if options.imuNonzeroBias == 1 if options.imuNonzeroBias == 1
metadata.imu.accelBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1);
metadata.imu.gyroBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); metadata.imu.gyroConstantBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1);
else else
metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.accelConstantBiasVector = zeros(3,1);
metadata.imu.gyroConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1);

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -146,7 +146,7 @@ for i=0:length(measurements)
end % end of Camera factor creation end % end of Camera factor creation
%% Add GPS factors %% Add GPS factors
if options.includeGPSFactors == 1 if options.includeGPSFactors == 1 && i >= options.gpsStartPose
gpsPosition = measurements(i).gpsPositionVector ... gpsPosition = measurements(i).gpsPositionVector ...
+ measurementNoise.gpsNoiseVector .* randn(3,1); + measurementNoise.gpsNoiseVector .* randn(3,1);
graph.add(PriorFactorPose3(currentPoseKey, ... graph.add(PriorFactorPose3(currentPoseKey, ...