From 1432fb773bab6de8335775398ab25afd2cfbf06d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 16 Apr 2014 15:25:05 -0400 Subject: [PATCH] minor changes --- .../+imuSimulator/covarianceAnalysisBetween.m | 22 ++++++++--- .../covarianceAnalysisCreateTrajectory.m | 38 +++++++++---------- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 22ca92164..4be7678c1 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index cf0214927..6a6c3b594 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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