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

working on imu test

release/4.3a0
Luca 2014-04-10 19:26:53 -04:00
parent c20dd18ab7
commit 24157ca124
1 changed files with 105 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,88 +9,95 @@ clear all
close all close all
%% Configuration %% Configuration
useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj
includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory
includeCameraFactors = 0; % includeCameraFactors = 0; % not implemented yet
trajectoryLength = 50; trajectoryLength = 2; % length of the ground truth trajectory
deltaT = 1.0; % amount of time between IMU measurements %% Imu metadata
vel = [0 0 0]; % initial velocity (used for generating IMU measurements
g = [0; 0; 0]; % gravity
omegaCoriolis = [0; 0; 0]; % Coriolis
% Imu metadata
epsBias = 1e-20; epsBias = 1e-20;
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.GyroscopeSigma = 1e-7;
IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.IntegrationSigma = 1e-10;
IMU_metadata.BiasAccelerometerSigma = epsBias; IMU_metadata.BiasAccelerometerSigma = epsBias;
IMU_metadata.BiasGyroscopeSigma = epsBias; IMU_metadata.BiasGyroscopeSigma = epsBias;
IMU_metadata.BiasAccOmegaInit = epsBias; IMU_metadata.BiasAccOmegaInit = epsBias;
noiseVel = noiseModel.Isotropic.Sigma(3, 0.1);
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10);
noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
%% Create ground truth trajectory %% Between metadata
unsmooth_DP = 0.5; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on rotation norm
gtValues = Values;
gtGraph = NonlinearFactorGraph;
if useRealData == 1 if useRealData == 1
sigma_ang = 1e-4; sigma_ang = 1e-4; sigma_cart = 40;
sigma_cart = 40;
else else
sigma_ang = 1e-2; sigma_ang = 1e-2; sigma_cart = 0.1;
sigma_cart = 0.1;
end end
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);
%% Create ground truth trajectory
gtValues = Values;
gtGraph = NonlinearFactorGraph;
if useRealData == 1 if useRealData == 1
%% Create a ground truth trajectory using scenario 2 data % % % %% Create a ground truth trajectory from Real data (if available)
fprintf('\nUsing Scenario 2 ground truth data\n'); % % % fprintf('\nUsing real data as ground truth\n');
% load scenario 2 ground truth data % % % gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); % Time: [4201x1 double]
% Lat: [4201x1 double]
% Add first pose % Lon: [4201x1 double]
currentPoseKey = symbol('x', 0); % Alt: [4201x1 double]
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); % VEast: [4201x1 double]
initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; % VNorth: [4201x1 double]
currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose % VUp: [4201x1 double]
gtValues.insert(currentPoseKey, currentPose); % Roll: [4201x1 double]
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); % Pitch: [4201x1 double]
prevPose = currentPose; % Heading
% % %
% Limit the trajectory length % % % % Add first pose
trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]); % % % currentPoseKey = symbol('x', 0);
% % % initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
for i=2:trajectoryLength % % % initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
currentPoseKey = symbol('x', i-1); % % % currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]); % % % gtValues.insert(currentPoseKey, currentPose);
gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)]; % % % gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
currentPose = Pose3.Expmap([gtRotation; gtECEF]); % % % prevPose = currentPose;
% % %
% Generate measurements as the current pose measured in the frame of % % % % Limit the trajectory length
% the previous pose % % % trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]);
deltaPose = prevPose.between(currentPose); % % %
gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); % % % for i=2:trajectoryLength
prevPose = currentPose; % % % currentPoseKey = symbol('x', i-1);
% % % gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]);
% Add values % % % gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)];
gtValues.insert(currentPoseKey, currentPose); % % % currentPose = Pose3.Expmap([gtRotation; gtECEF]);
% % %
% Add the factor to the factor graph % % % % Generate measurements as the current pose measured in the frame of
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); % % % % the previous pose
end % % % deltaPose = prevPose.between(currentPose);
% % % gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
% % % prevPose = currentPose;
% % %
% % % % Add values
% % % gtValues.insert(currentPoseKey, currentPose);
% % %
% % % % Add the factor to the factor graph
% % % gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose));
% % % end
else else
%% Create a random trajectory as ground truth %% Create a random trajectory as ground truth
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
currentPose = Pose3; % initial pose % initial pose
deltaT = 1.0; % amount of time between IMU measurements
g = [0; 0; 0]; % gravity
omegaCoriolis = [0; 0; 0]; % Coriolis
unsmooth_DP = 0.5; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on rotation norm
fprintf('\nCreating a random ground truth trajectory\n'); fprintf('\nCreating a random ground truth trajectory\n');
% Add priors %% Add priors
currentPoseKey = symbol('x', 0); currentPoseKey = symbol('x', 0);
currentPose = Pose3; % initial pose
gtValues.insert(currentPoseKey, currentPose); gtValues.insert(currentPoseKey, currentPose);
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
@ -105,28 +112,31 @@ else
for i=1:trajectoryLength for i=1:trajectoryLength
currentPoseKey = symbol('x', i); currentPoseKey = symbol('x', i);
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); measurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
% "Deduce" ground truth measurements % "Deduce" ground truth measurements
% deltaPose are the gt measurements - save them in some structure % deltaPose are the gt measurements - save them in some structure
currentPose = currentPose.compose(deltaPose); currentPose = currentPose.compose(deltaPose);
gtValues.insert(currentPoseKey, currentPose); gtValues.insert(currentPoseKey, currentPose);
% Add the factors to the factor graph % Add the factors to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose));
% Add IMU factors % Add IMU factors
if includeIMUFactors == 1 if includeIMUFactors == 1
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
% create accel and gyro measurements based on % create accel and gyro measurements based on
gyro = gtDeltaMatrix(i, 1:3)./deltaT; measurements.imu.gyro = gtDeltaMatrix(i, 1:3)./deltaT;
accel = (gtDeltaMatrix(i, 4:6) - vel.*deltaT).*(2/(deltaT*deltaT)); % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
vel = gtDeltaMatrix(i,4:6)./deltaT; measurements.imu.accel = (gtDeltaMatrix(i, 4:6) - currentVel.*deltaT).*(2/(deltaT*deltaT));
% update current velocity
currentVel = gtDeltaMatrix(i,4:6)./deltaT;
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
zeroBias, ... zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
@ -147,9 +157,17 @@ else
end end
end end
gtPoses = Values;
for i=0:trajectoryLength
currentPoseKey = symbol('x', i);
currentPose = gtValues.at(currentPoseKey);
gtPoses.insert(currentPoseKey, currentPose);
end
figure(1) figure(1)
hold on; hold on;
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses));
axis equal axis equal
numMonteCarloRuns = 100; numMonteCarloRuns = 100;