code reorganization
Luca
parent
9bc0ddd4a2
commit
2e3dcd2ab7
|
|
@ -100,9 +100,6 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
|
|||
%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')
|
||||
|
||||
figure(1)
|
||||
hold on;
|
||||
plot3DPoints(gtValues);
|
||||
|
|
@ -150,9 +147,9 @@ for k=1:numMonteCarloRuns
|
|||
marginals = Marginals(graph, estimate);
|
||||
|
||||
% for each pose in the trajectory
|
||||
for i=1:size(gtMeasurements.deltaMatrix,1)+1
|
||||
for i=0:options.trajectoryLength
|
||||
% compute estimation errors
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
currentPoseKey = symbol('x', i);
|
||||
gtPosition = gtValues.at(currentPoseKey).translation.vector;
|
||||
estPosition = estimate.at(currentPoseKey).translation.vector;
|
||||
estR = estimate.at(currentPoseKey).rotation.matrix;
|
||||
|
|
@ -163,7 +160,7 @@ for k=1:numMonteCarloRuns
|
|||
covPosition = estR * cov(4:6,4:6) * estR';
|
||||
|
||||
% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
|
||||
NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
|
||||
NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
|
||||
end
|
||||
|
||||
figure(2)
|
||||
|
|
@ -215,7 +212,7 @@ title('NEES normalized by dof VS bounds');
|
|||
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
|
||||
|
||||
logFile = horzcat(folderName,'log-',testName);
|
||||
save(logFile)
|
||||
%save(logFile)
|
||||
|
||||
%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
|
||||
% the nees for a single experiment (i) is defined as
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
|
||||
function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
|
||||
% Create a factor graph based on provided measurements, values, and noises.
|
||||
% Used for covariance analysis scripts.
|
||||
% 'options' contains fields for including various factor types.
|
||||
|
|
@ -10,14 +10,14 @@ import gtsam.*;
|
|||
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% Iterate through the trajectory
|
||||
for i=0:size(measurements.deltaMatrix, 1);
|
||||
% Iterate through the trajectory
|
||||
for i=0:length(measurements)
|
||||
% Get the current pose
|
||||
currentPoseKey = symbol('x', i);
|
||||
currentPose = values.at(currentPoseKey);
|
||||
currentPose = values.at(currentPoseKey);
|
||||
|
||||
if i==0
|
||||
%% first time step, add priors
|
||||
%% first time step, add priors
|
||||
% Pose prior (poses used for all factors)
|
||||
initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
|
||||
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
|
||||
|
|
@ -25,11 +25,11 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
% IMU velocity and bias priors
|
||||
if options.includeIMUFactors == 1
|
||||
currentVelKey = symbol('v', 0);
|
||||
currentVel = values.at(currentVelKey).vector;
|
||||
currentVel = values.at(currentVelKey).vector;
|
||||
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
|
||||
|
||||
currentBiasKey = symbol('b', 0);
|
||||
currentBias = values.at(currentBiasKey);
|
||||
currentBias = values.at(currentBiasKey);
|
||||
graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
|
||||
end
|
||||
|
||||
|
|
@ -41,12 +41,12 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
end
|
||||
|
||||
else
|
||||
prevPoseKey = symbol('x', i-1);
|
||||
|
||||
|
||||
%% Add Between factors
|
||||
if options.includeBetweenFactors == 1
|
||||
prevPoseKey = symbol('x', i-1);
|
||||
% Create the noisy pose estimate
|
||||
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ...
|
||||
deltaPose = Pose3.Expmap(measurements(i).deltaVector ...
|
||||
+ (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise
|
||||
% Add the between factor to the graph
|
||||
graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
|
||||
|
|
@ -57,52 +57,55 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
% Update keys
|
||||
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
|
||||
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
|
||||
% Generate IMU measurements with noise
|
||||
imuAccel = measurements.imu.accel(i,:)' ...
|
||||
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
|
||||
imuGyro = measurements.imu.gyro(i,:)' ...
|
||||
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
|
||||
|
||||
if options.imuFactorType == 2
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.IntegrationSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.BiasAccOmegaInit.^2 * eye(6));
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% Add Imu factor
|
||||
graph.add(CombinedImuFactor( ...
|
||||
currentPoseKey-1, currentVelKey-1, ...
|
||||
currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, currentBiasKey, ...
|
||||
imuMeasurement, ...
|
||||
metadata.imu.g, metadata.imu.omegaCoriolis, ...
|
||||
noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
|
||||
else % IMU type 1
|
||||
|
||||
if options.imuFactorType == 1
|
||||
% Initialize preintegration
|
||||
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
|
||||
metadata.imu.zeroBias, ...
|
||||
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
metadata.imu.IntegrationSigma.^2 * eye(3));
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% Add Imu factor
|
||||
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
|
||||
% Add between factor on biases
|
||||
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
% Additional prior on zerobias
|
||||
%graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ...
|
||||
% noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
% Generate IMU measurements with noise
|
||||
for j=1:length(measurements(i).imu) % all measurements to preintegrate
|
||||
imuAccel = measurements(i).imu(j).accel ...
|
||||
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise
|
||||
imuGyro = measurements(i).imu(j).gyro ...
|
||||
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise
|
||||
|
||||
% Preintegrate
|
||||
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
|
||||
% Add Imu factor
|
||||
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
|
||||
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
|
||||
% Add between factor on biases
|
||||
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
|
||||
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
if options.imuFactorType == 2
|
||||
% % Initialize preintegration
|
||||
% imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
|
||||
% metadata.imu.zeroBias, ...
|
||||
% metadata.imu.AccelerometerSigma.^2 * eye(3), ...
|
||||
% metadata.imu.GyroscopeSigma.^2 * eye(3), ...
|
||||
% metadata.imu.IntegrationSigma.^2 * eye(3), ...
|
||||
% metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
|
||||
% metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
|
||||
% metadata.imu.BiasAccOmegaInit.^2 * eye(6));
|
||||
% % Preintegrate
|
||||
% imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
|
||||
% % Add Imu factor
|
||||
% graph.add(CombinedImuFactor( ...
|
||||
% currentPoseKey-1, currentVelKey-1, ...
|
||||
% currentPoseKey, currentVelKey, ...
|
||||
% currentBiasKey-1, currentBiasKey, ...
|
||||
% imuMeasurement, ...
|
||||
% metadata.imu.g, metadata.imu.omegaCoriolis, ...
|
||||
% noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
|
||||
end
|
||||
|
||||
end % end of IMU factor creation
|
||||
|
||||
%% Add Camera factors - UNDER CONSTRUCTION !!!! -
|
||||
|
|
@ -115,7 +118,7 @@ for i=0:size(measurements.deltaMatrix, 1);
|
|||
landmarkKey = symbol('p', j);
|
||||
try
|
||||
Z = gtCamera.project(gtLandmarkPoints(j));
|
||||
%% TO-DO probably want to do some type of filtering on the measurement values, because
|
||||
% TO-DO probably want to do some type of filtering on the measurement values, because
|
||||
% they might not all be valid
|
||||
graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K));
|
||||
catch
|
||||
|
|
|
|||
|
|
@ -9,8 +9,8 @@ import gtsam.*;
|
|||
|
||||
values = Values;
|
||||
|
||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
||||
warning('using identity rotation')
|
||||
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data')
|
||||
warning('using identity rotation')
|
||||
|
||||
if options.useRealData == 1
|
||||
%% Create a ground truth trajectory from Real data (if available)
|
||||
|
|
@ -18,47 +18,69 @@ if options.useRealData == 1
|
|||
gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
|
||||
'VEast', 'VNorth', 'VUp');
|
||||
|
||||
Org_lat = gtScenario.Lat(1);
|
||||
Org_lon = gtScenario.Lon(1);
|
||||
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
|
||||
|
||||
% Limit the trajectory length
|
||||
options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]);
|
||||
options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength]);
|
||||
fprintf('Scenario Ind: ');
|
||||
for i=1:options.trajectoryLength+1
|
||||
% Update the pose key
|
||||
currentPoseKey = symbol('x', i-1);
|
||||
|
||||
% Generate the current pose
|
||||
scenarioInd = options.subsampleStep * (i-1) + 1;
|
||||
|
||||
for i=0:options.trajectoryLength
|
||||
scenarioInd = options.subsampleStep * i + 1;
|
||||
fprintf('%d, ', scenarioInd);
|
||||
if mod(i,20) == 0
|
||||
fprintf('\n');
|
||||
end
|
||||
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
|
||||
% truth in ENU
|
||||
dX = gtECEF(1) - initialPositionECEF(1);
|
||||
dY = gtECEF(2) - initialPositionECEF(2);
|
||||
dZ = gtECEF(3) - initialPositionECEF(3);
|
||||
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
|
||||
if (mod(i,20) == 0) fprintf('\n'); end
|
||||
|
||||
gtPosition = [xlt, ylt, zlt]';
|
||||
gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
||||
currentPose = Pose3(gtRotation, Point3(gtPosition));
|
||||
|
||||
% Add values
|
||||
%% Generate the current pose
|
||||
currentPoseKey = symbol('x', i);
|
||||
currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
|
||||
% add to values
|
||||
values.insert(currentPoseKey, currentPose);
|
||||
|
||||
% Generate the measurement. The first pose is considered the prior, so
|
||||
% it has no measurement
|
||||
if i > 1
|
||||
%% gt Between measurements
|
||||
if options.includeBetweenFactors == 1 && i > 0
|
||||
prevPose = values.at(currentPoseKey - 1);
|
||||
deltaPose = prevPose.between(currentPose);
|
||||
measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
|
||||
measurements(i).deltaVector = Pose3.Logmap(deltaPose);
|
||||
end
|
||||
|
||||
%% gt IMU measurements
|
||||
if options.includeIMUFactors == 1
|
||||
currentVelKey = symbol('v', i);
|
||||
currentBiasKey = symbol('b', i);
|
||||
deltaT = 0.01; % amount of time between IMU measurements
|
||||
if i == 0
|
||||
currentVel = [0 0 0]';
|
||||
else
|
||||
% integrate & store intermediate measurements
|
||||
for j=1:options.subsampleStep % we integrate all the intermediate measurements
|
||||
scenarioIndIMU1 = scenarioInd+j-1;
|
||||
scenarioIndIMU2 = scenarioInd+j;
|
||||
IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
|
||||
IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
|
||||
IMUdeltaPose = IMUPose1.between(IMUPose2);
|
||||
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
|
||||
IMUdeltaRotVector = IMUdeltaPoseVector(1:3);
|
||||
IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
|
||||
|
||||
measurements(i).imu(j).deltaT = deltaT;
|
||||
|
||||
% gyro rate: Logmap(R_i' * R_i+1) / deltaT
|
||||
measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT;
|
||||
|
||||
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
|
||||
measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
|
||||
|
||||
% Update velocity
|
||||
currentVel = IMUdeltaPositionVector./deltaT;
|
||||
end
|
||||
end
|
||||
|
||||
% Add Values: velocity and bias
|
||||
values.insert(currentVelKey, LieVector(currentVel));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
end
|
||||
|
||||
end
|
||||
fprintf('\n');
|
||||
else
|
||||
error('Please use RealData')
|
||||
%% Create a random trajectory as ground truth
|
||||
currentPose = Pose3; % initial pose % initial pose
|
||||
|
||||
|
|
@ -87,41 +109,5 @@ else
|
|||
end % end of random trajectory creation
|
||||
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
|
||||
|
||||
% Iterate over the deltaMatrix to generate appropriate IMU measurements
|
||||
for i = 0:size(measurements.deltaMatrix, 1)
|
||||
% Update Keys
|
||||
currentVelKey = symbol('v', i);
|
||||
currentBiasKey = symbol('b', i);
|
||||
|
||||
if i == 0
|
||||
% Add initial values
|
||||
currentVel = [0 0 0];
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
else
|
||||
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);
|
||||
|
||||
% 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);
|
||||
|
||||
% Add Values: velocity and bias
|
||||
values.insert(currentVelKey, LieVector(currentVel'));
|
||||
values.insert(currentBiasKey, metadata.imu.zeroBias);
|
||||
end
|
||||
end
|
||||
end % end of IMU measurements
|
||||
|
||||
end % end of function
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,25 @@
|
|||
function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd)
|
||||
% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw)
|
||||
% The function picks the index 'scenarioInd' in those vectors and
|
||||
% computes the corresponding pose by
|
||||
% 1) Converting (Lat,Lon,Alt) to local coordinates
|
||||
% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
Org_lat = gtScenario.Lat(1);
|
||||
Org_lon = gtScenario.Lon(1);
|
||||
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
|
||||
|
||||
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
|
||||
% truth in ENU
|
||||
dX = gtECEF(1) - initialPositionECEF(1);
|
||||
dY = gtECEF(2) - initialPositionECEF(2);
|
||||
dZ = gtECEF(3) - initialPositionECEF(3);
|
||||
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
|
||||
|
||||
gtPosition = [xlt, ylt, zlt]';
|
||||
gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
|
||||
currentPose = Pose3(gtRotation, Point3(gtPosition));
|
||||
|
||||
end
|
||||
Loading…
Reference in New Issue