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

Added option for type 2 IMU factors

release/4.3a0
djensen3 2014-04-17 10:09:53 -04:00
parent b5f9862274
commit e1c13c87d7
3 changed files with 53 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,12 +12,13 @@ close all
%% 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 = 1; % if true, BetweenFactors will be generated between consecutive poses 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.includeCameraFactors = 0; % not fully implemented yet
options.trajectoryLength = 209; % length of the ground truth trajectory options.trajectoryLength = 209; % length of the ground truth trajectory
options.subsampleStep = 20; 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 %% Camera metadata
numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors

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

@ -63,23 +63,47 @@ for i=0:size(measurements.deltaMatrix, 1);
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
imuGyro = measurements.imu.gyro(i,:)' ... imuGyro = measurements.imu.gyro(i,:)' ...
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
% Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... if options.imuFactorType == 2
metadata.imu.zeroBias, ... % Initialize preintegration
metadata.imu.AccelerometerSigma.^2 * eye(3), ... imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
metadata.imu.GyroscopeSigma.^2 * eye(3), ... metadata.imu.zeroBias, ...
metadata.imu.IntegrationSigma.^2 * eye(3)); metadata.imu.AccelerometerSigma.^2 * eye(3), ...
% Preintegrate metadata.imu.GyroscopeSigma.^2 * eye(3), ...
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); metadata.imu.IntegrationSigma.^2 * eye(3), ...
% Add Imu factor metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); metadata.imu.BiasAccOmegaInit.^2 * eye(6));
% Add between factor on biases % Preintegrate
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); % Add Imu factor
% Additional prior on zerobias graph.add(CombinedImuFactor( ...
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... currentPoseKey-1, currentVelKey-1, ...
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); 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 end % end of IMU factor creation
%% Add Camera factors - UNDER CONSTRUCTION !!!! - %% Add Camera factors - UNDER CONSTRUCTION !!!! -

9
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -21,13 +21,17 @@ if options.useRealData == 1
% Limit the trajectory length % Limit the trajectory length
options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]); options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
fprintf('Scenario Ind: ');
for i=1:options.trajectoryLength+1 for i=1:options.trajectoryLength+1
% Update the pose key % Update the pose key
currentPoseKey = symbol('x', i-1); currentPoseKey = symbol('x', i-1);
% Generate the current pose % 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)]); gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
% truth in ENU % truth in ENU
dX = gtECEF(1) - initialPositionECEF(1); dX = gtECEF(1) - initialPositionECEF(1);
@ -50,6 +54,7 @@ if options.useRealData == 1
measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
end end
end end
fprintf('\n');
else else
%% Create a random trajectory as ground truth %% Create a random trajectory as ground truth
currentPose = Pose3; % initial pose % initial pose currentPose = Pose3; % initial pose % initial pose