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

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

@ -12,7 +12,7 @@ close all
%% Configuration
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.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.trajectoryLength = 4; % length of the ground truth trajectory
options.subsampleStep = 20;
@ -78,7 +78,7 @@ gtMeasurementNoise.imu.accelNoiseVector = [0 0 0];
gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0];
gtMeasurementNoise.cameraPixelNoiseVector = [0 0];
[gtGraph, gtValues] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values
gtNoiseModels, ... % noise models to use in this graph
@ -87,8 +87,8 @@ gtMeasurementNoise.cameraPixelNoiseVector = [0 0];
metadata); % misc data necessary for factor creation
%% Display, printing, and plotting of ground truth
%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
%gtValues.print(sprintf('\nGround Truth Values:\n '));
gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
gtValues.print(sprintf('\nGround Truth Values:\n '));
warning('Additional prior on zerobias')
warning('Additional PriorFactorLieVector on velocities')
@ -104,6 +104,7 @@ disp('Plotted ground truth')
%% Monte Carlo Runs
for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d.\n', k');
% create a new graph
graph = NonlinearFactorGraph;
@ -125,8 +126,17 @@ for k=1:numMonteCarloRuns
% Add the factors to the factor graph
graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose));
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
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
if options.includeIMUFactors == 1
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
deltaT = 0.1; % amount of time between IMU measurements
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
deltaT = 0.1; % amount of time between IMU measurements
% Iterate over the deltaMatrix to generate appropriate IMU measurements
for i = 1:size(measurements.deltaMatrix, 1)
% Update Keys
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
% Iterate over the deltaMatrix to generate appropriate IMU measurements
for i = 1:size(measurements.deltaMatrix, 1)
% Update Keys
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
measurements.imu.deltaT(i) = deltaT;
measurements.imu.deltaT(i) = deltaT;
% create accel and gyro measurements based on
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
% create accel and gyro measurements based on
measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i);
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ...
- currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i)));
% Update velocity
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
% Update velocity
currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i);
% Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel'));
values.insert(currentBiasKey, metadata.imu.zeroBias);
end
% Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel'));
values.insert(currentBiasKey, metadata.imu.zeroBias);
end
end % end of IMU measurements
end