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

fixed bug with missing priors. Added IMU noise measurements to Monte Carlo runs

release/4.3a0
djensen3 2014-04-16 16:20:10 -04:00
parent 1432fb773b
commit b5f9862274
3 changed files with 70 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,11 +10,11 @@ clear all
close all close all
%% Configuration %% Configuration
options.useRealData = 0; % 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 type 1 Factors will be generated for the trajectory
options.includeCameraFactors = 0; % not fully implemented yet options.includeCameraFactors = 0; % not fully implemented yet
options.trajectoryLength = 4; % length of the ground truth trajectory options.trajectoryLength = 209; % length of the ground truth trajectory
options.subsampleStep = 20; options.subsampleStep = 20;
numMonteCarloRuns = 2; numMonteCarloRuns = 2;
@ -51,8 +51,14 @@ noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias);
noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4);
sigma_accel = metadata.imu.AccelerometerSigma;
sigma_gyro = metadata.imu.GyroscopeSigma;
noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel];
noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro];
%% Between metadata %% Between metadata
sigma_ang = 1e-2; sigma_cart = 1; sigma_ang = 1e-3; sigma_cart = 1e-3;
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3); %noisePose = noiseModel.Isotropic.Sigma(6, 1e-3);
@ -73,10 +79,10 @@ gtNoiseModels.noisePriorPose = noisePose;
gtNoiseModels.noisePriorBias = noisePriorBias; gtNoiseModels.noisePriorBias = noisePriorBias;
% Set measurement noise to 0, because this is ground truth % Set measurement noise to 0, because this is ground truth
gtMeasurementNoise.poseNoiseVector = [0 0 0 0 0 0]; gtMeasurementNoise.poseNoiseVector = [0; 0; 0; 0; 0; 0;];
gtMeasurementNoise.imu.accelNoiseVector = [0 0 0]; gtMeasurementNoise.imu.accelNoiseVector = [0; 0; 0];
gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0]; gtMeasurementNoise.imu.gyroNoiseVector = [0; 0; 0];
gtMeasurementNoise.cameraPixelNoiseVector = [0 0]; gtMeasurementNoise.cameraPixelNoiseVector = [0; 0];
gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements gtMeasurements, ... % ground truth measurements
@ -87,8 +93,8 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
metadata); % misc data necessary for factor creation metadata); % misc data necessary for factor creation
%% Display, printing, and plotting of ground truth %% Display, printing, and plotting of ground truth
gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); %gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
gtValues.print(sprintf('\nGround Truth Values:\n ')); %gtValues.print(sprintf('\nGround Truth Values:\n '));
warning('Additional prior on zerobias') warning('Additional prior on zerobias')
warning('Additional PriorFactorLieVector on velocities') warning('Additional PriorFactorLieVector on velocities')
@ -102,39 +108,31 @@ axis equal
disp('Plotted ground truth') disp('Plotted ground truth')
%% Monte Carlo Runs %% Monte Carlo Runs
% Set up noise models
monteCarloNoiseModels.noisePose = noisePose;
monteCarloNoiseModels.noiseVel = noiseVel;
monteCarloNoiseModels.noiseBias = noiseBias;
monteCarloNoiseModels.noisePriorPose = noisePose;
monteCarloNoiseModels.noisePriorBias = noisePriorBias;
% Set measurement noise for monte carlo runs
monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose;
monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel;
monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro;
monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0];
for k=1:numMonteCarloRuns for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d.\n', k'); fprintf('Monte Carlo Run %d.\n', k');
% create a new graph % Create a new graph using noisy measurements
graph = NonlinearFactorGraph; graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
% noisy prior gtValues, ... % ground truth Values
currentPoseKey = symbol('x', 0); monteCarloNoiseModels, ... % noise models to use in this graph
currentPose = gtValues.at(currentPoseKey); monteCarloMeasurementNoise, ... % noise to apply to measurements
gtMeasurements.posePrior = currentPose; options, ... % options for the graph (e.g. which factors to include)
noisyDelta = noiseVectorPose .* randn(6,1); metadata); % misc data necessary for factor creation
noisyInitialPose = Pose3.Expmap(noisyDelta);
graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
for i=1:size(gtMeasurements.deltaMatrix,1)
currentPoseKey = symbol('x', i);
% for each measurement: add noise and add to graph
noisyDelta = gtMeasurements.deltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1));
noisyDeltaPose = Pose3.Expmap(noisyDelta);
% Add the factors to the factor graph
graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose));
end
% graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
% gtMeasurements, ... % ground truth measurements
% gtValues, ... % ground truth Values
% gtNoiseModels, ... % noise models to use in this graph
% gtMeasurementNoise, ... % noise to apply to measurements
% options, ... % options for the graph (e.g. which factors to include)
% metadata); % misc data necessary for factor creation
%graph.print('graph') %graph.print('graph')

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

@ -1,4 +1,4 @@
function [ graph, values ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
% Create a factor graph based on provided measurements, values, and noises. % Create a factor graph based on provided measurements, values, and noises.
% Used for covariance analysis scripts. % Used for covariance analysis scripts.
% 'options' contains fields for including various factor types. % 'options' contains fields for including various factor types.
@ -20,19 +20,21 @@ for i=0:size(measurements.deltaMatrix, 1);
%% first time step, add priors %% first time step, add priors
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
warning('using identity rotation') warning('using identity rotation')
graph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModels.noisePose));
measurements.posePrior = currentPose;
% Pose prior (poses used for all factors)
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
% IMU velocity and bias priors
if options.includeIMUFactors == 1 if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0); currentVelKey = symbol('v', 0);
currentBiasKey = symbol('b', 0); currentBiasKey = symbol('b', 0);
currentVel = [0; 0; 0]; currentVel = [0; 0; 0];
values.insert(currentVelKey, LieVector(currentVel));
values.insert(currentBiasKey, metadata.imu.zeroBias);
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias)); graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias));
end end
% Camera priors
if options.includeCameraFactors == 1 if options.includeCameraFactors == 1
pointNoiseSigma = 0.1; pointNoiseSigma = 0.1;
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@ -46,7 +48,7 @@ for i=0:size(measurements.deltaMatrix, 1);
if options.includeBetweenFactors == 1 if options.includeBetweenFactors == 1
% Create the noisy pose estimate % Create the noisy pose estimate
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ... deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
+ (measurementNoise.poseNoiseVector' .* randn(6,1))); % added noise + (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise
% Add the between factor to the graph % Add the between factor to the graph
graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
end % end of Between pose factor creation end % end of Between pose factor creation
@ -58,9 +60,9 @@ for i=0:size(measurements.deltaMatrix, 1);
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
% Generate IMU measurements with noise % Generate IMU measurements with noise
imuAccel = measurements.imu.accel(i,:)' ... imuAccel = measurements.imu.accel(i,:)' ...
+ (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 % Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ... metadata.imu.zeroBias, ...

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

@ -81,30 +81,37 @@ end % end of else
%% Create IMU measurements and Values for the trajectory %% Create IMU measurements and Values for the trajectory
if options.includeIMUFactors == 1 if options.includeIMUFactors == 1
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
deltaT = 0.1; % amount of time between IMU measurements deltaT = 0.1; % amount of time between IMU measurements
% Iterate over the deltaMatrix to generate appropriate IMU measurements % Iterate over the deltaMatrix to generate appropriate IMU measurements
for i = 1:size(measurements.deltaMatrix, 1) for i = 0:size(measurements.deltaMatrix, 1)
% Update Keys % Update Keys
currentVelKey = symbol('v', i); currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i); currentBiasKey = symbol('b', i);
measurements.imu.deltaT(i) = deltaT; if i == 0
% Add initial values
currentVel = [0 0 0];
values.insert(currentVelKey, LieVector(currentVel'));
values.insert(currentBiasKey, metadata.imu.zeroBias);
else
measurements.imu.deltaT(i) = deltaT;
% create accel and gyro measurements based on % create accel and gyro measurements based on
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
% acc = (deltaPosition - initialVel * dT) * (2/dt^2) % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
% Update velocity % Update velocity
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
% Add Values: velocity and bias % Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel')); values.insert(currentVelKey, LieVector(currentVel'));
values.insert(currentBiasKey, metadata.imu.zeroBias); values.insert(currentBiasKey, metadata.imu.zeroBias);
end
end end
end % end of IMU measurements end % end of IMU measurements