working on imu test
Luca
parent
c20dd18ab7
commit
24157ca124
|
|
@ -11,86 +11,93 @@ close all
|
|||
%% Configuration
|
||||
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
|
||||
includeCameraFactors = 0;
|
||||
trajectoryLength = 50;
|
||||
% includeCameraFactors = 0; % not implemented yet
|
||||
trajectoryLength = 2; % length of the ground truth trajectory
|
||||
|
||||
deltaT = 1.0; % amount of time between IMU measurements
|
||||
vel = [0 0 0]; % initial velocity (used for generating IMU measurements
|
||||
g = [0; 0; 0]; % gravity
|
||||
omegaCoriolis = [0; 0; 0]; % Coriolis
|
||||
|
||||
% Imu metadata
|
||||
%% Imu metadata
|
||||
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.GyroscopeSigma = 1e-7;
|
||||
IMU_metadata.IntegrationSigma = 1e-10;
|
||||
IMU_metadata.BiasAccelerometerSigma = epsBias;
|
||||
IMU_metadata.BiasGyroscopeSigma = epsBias;
|
||||
IMU_metadata.BiasAccOmegaInit = epsBias;
|
||||
|
||||
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10);
|
||||
noiseVel = noiseModel.Isotropic.Sigma(3, 0.1);
|
||||
noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
|
||||
|
||||
%% Create ground truth trajectory
|
||||
unsmooth_DP = 0.5; % controls smoothness on translation norm
|
||||
unsmooth_DR = 0.1; % controls smoothness on rotation norm
|
||||
|
||||
gtValues = Values;
|
||||
gtGraph = NonlinearFactorGraph;
|
||||
|
||||
%% Between metadata
|
||||
if useRealData == 1
|
||||
sigma_ang = 1e-4;
|
||||
sigma_cart = 40;
|
||||
sigma_ang = 1e-4; sigma_cart = 40;
|
||||
else
|
||||
sigma_ang = 1e-2;
|
||||
sigma_cart = 0.1;
|
||||
sigma_ang = 1e-2; sigma_cart = 0.1;
|
||||
end
|
||||
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
|
||||
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
|
||||
|
||||
%% Create ground truth trajectory
|
||||
gtValues = Values;
|
||||
gtGraph = NonlinearFactorGraph;
|
||||
|
||||
if useRealData == 1
|
||||
%% Create a ground truth trajectory using scenario 2 data
|
||||
fprintf('\nUsing Scenario 2 ground truth data\n');
|
||||
% load scenario 2 ground truth data
|
||||
gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
|
||||
|
||||
% Add first pose
|
||||
currentPoseKey = symbol('x', 0);
|
||||
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
|
||||
initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
|
||||
currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose
|
||||
gtValues.insert(currentPoseKey, currentPose);
|
||||
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
|
||||
prevPose = currentPose;
|
||||
|
||||
% Limit the trajectory length
|
||||
trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]);
|
||||
|
||||
for i=2:trajectoryLength
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]);
|
||||
gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)];
|
||||
currentPose = Pose3.Expmap([gtRotation; gtECEF]);
|
||||
|
||||
% Generate measurements as the current pose measured in the frame of
|
||||
% the previous pose
|
||||
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
|
||||
% % % %% Create a ground truth trajectory from Real data (if available)
|
||||
% % % fprintf('\nUsing real data as ground truth\n');
|
||||
% % % gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
|
||||
% Time: [4201x1 double]
|
||||
% Lat: [4201x1 double]
|
||||
% Lon: [4201x1 double]
|
||||
% Alt: [4201x1 double]
|
||||
% VEast: [4201x1 double]
|
||||
% VNorth: [4201x1 double]
|
||||
% VUp: [4201x1 double]
|
||||
% Roll: [4201x1 double]
|
||||
% Pitch: [4201x1 double]
|
||||
% Heading
|
||||
% % %
|
||||
% % % % Add first pose
|
||||
% % % currentPoseKey = symbol('x', 0);
|
||||
% % % initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
|
||||
% % % initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
|
||||
% % % currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose
|
||||
% % % gtValues.insert(currentPoseKey, currentPose);
|
||||
% % % gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
|
||||
% % % prevPose = currentPose;
|
||||
% % %
|
||||
% % % % Limit the trajectory length
|
||||
% % % trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]);
|
||||
% % %
|
||||
% % % for i=2:trajectoryLength
|
||||
% % % currentPoseKey = symbol('x', i-1);
|
||||
% % % gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]);
|
||||
% % % gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)];
|
||||
% % % currentPose = Pose3.Expmap([gtRotation; gtECEF]);
|
||||
% % %
|
||||
% % % % Generate measurements as the current pose measured in the frame of
|
||||
% % % % the previous pose
|
||||
% % % 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
|
||||
%% 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');
|
||||
% Add priors
|
||||
%% Add priors
|
||||
currentPoseKey = symbol('x', 0);
|
||||
currentPose = Pose3; % initial pose
|
||||
gtValues.insert(currentPoseKey, currentPose);
|
||||
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
|
||||
|
||||
|
|
@ -105,13 +112,11 @@ else
|
|||
|
||||
for i=1:trajectoryLength
|
||||
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
|
||||
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];
|
||||
deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
|
||||
measurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
|
||||
|
||||
% "Deduce" ground truth measurements
|
||||
% deltaPose are the gt measurements - save them in some structure
|
||||
|
|
@ -123,10 +128,15 @@ else
|
|||
|
||||
% Add IMU factors
|
||||
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
|
||||
gyro = gtDeltaMatrix(i, 1:3)./deltaT;
|
||||
accel = (gtDeltaMatrix(i, 4:6) - vel.*deltaT).*(2/(deltaT*deltaT));
|
||||
vel = gtDeltaMatrix(i,4:6)./deltaT;
|
||||
measurements.imu.gyro = gtDeltaMatrix(i, 1:3)./deltaT;
|
||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
||||
measurements.imu.accel = (gtDeltaMatrix(i, 4:6) - currentVel.*deltaT).*(2/(deltaT*deltaT));
|
||||
% update current velocity
|
||||
currentVel = gtDeltaMatrix(i,4:6)./deltaT;
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
|
||||
zeroBias, ...
|
||||
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
||||
|
|
@ -147,9 +157,17 @@ else
|
|||
end
|
||||
|
||||
end
|
||||
|
||||
gtPoses = Values;
|
||||
for i=0:trajectoryLength
|
||||
currentPoseKey = symbol('x', i);
|
||||
currentPose = gtValues.at(currentPoseKey);
|
||||
gtPoses.insert(currentPoseKey, currentPose);
|
||||
end
|
||||
|
||||
figure(1)
|
||||
hold on;
|
||||
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
|
||||
plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses));
|
||||
axis equal
|
||||
|
||||
numMonteCarloRuns = 100;
|
||||
|
|
|
|||
Loading…
Reference in New Issue