diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 65ab61dad..1de2b1f44 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 70a9a5bda..ee6835367 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 9c54af687..8712c818c 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m new file mode 100644 index 000000000..61830443a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -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 \ No newline at end of file