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

minor changes

release/4.3a0
djensen3 2014-04-16 15:25:05 -04:00
parent b85ebb501d
commit 1432fb773b
2 changed files with 35 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,7 +12,7 @@ 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 = 0; % 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 = 0; % 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 = 4; % length of the ground truth trajectory
options.subsampleStep = 20; options.subsampleStep = 20;
@ -78,7 +78,7 @@ 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, gtValues] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values gtValues, ... % ground truth Values
gtNoiseModels, ... % noise models to use in this graph gtNoiseModels, ... % noise models to use in this graph
@ -87,8 +87,8 @@ gtMeasurementNoise.cameraPixelNoiseVector = [0 0];
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')
@ -104,6 +104,7 @@ disp('Plotted ground truth')
%% Monte Carlo Runs %% Monte Carlo Runs
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
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
@ -126,7 +127,16 @@ for k=1:numMonteCarloRuns
graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose));
end end
graph.print('graph') % 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')
% optimize % optimize
optimizer = GaussNewtonOptimizer(graph, gtValues); optimizer = GaussNewtonOptimizer(graph, gtValues);

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

@ -81,31 +81,31 @@ 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 = 1: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; 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 of IMU measurements end % end of IMU measurements
end end