diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1de2b1f44..6162d8309 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,6 +11,8 @@ clc clear all close all +saveResults = 0; + %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses @@ -106,6 +108,13 @@ plot3DPoints(gtValues); plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal +% optimize +optimizer = GaussNewtonOptimizer(gtGraph, gtValues); +gtEstimate = optimizer.optimize(); +% estimate should match gtValues if graph is correct. +fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) ); +fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) ); + disp('Plotted ground truth') %% Monte Carlo Runs @@ -174,16 +183,18 @@ plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof box on set(gca,'Fontsize',16) title('NEES and ANEES'); -%print('-djpeg', horzcat('runs-',testName)); -saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); +if saveResults + saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); +end %% figure(1) box on set(gca,'Fontsize',16) title('Ground truth and estimates for each MC runs'); -%print('-djpeg', horzcat('gt-',testName)); -saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); +if saveResults + saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); +end %% Let us compute statistics on the overall NEES n = 3; % position vector dimension @@ -208,11 +219,11 @@ plot(r2*ones(size(ANEES,2),1),'k-.'); box on set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); -%print('-djpeg', horzcat('ANEES-',testName)); -saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); - -logFile = horzcat(folderName,'log-',testName); -%save(logFile) +if saveResults + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + logFile = horzcat(folderName,'log-',testName); + save(logFile) +end %% 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 ee6835367..cb10d14cd 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -74,13 +74,15 @@ for i=0:length(measurements) % 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 + %imuMeasurement.print('imuMeasurement'); + + % 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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 8712c818c..21c263301 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -64,11 +64,12 @@ if options.useRealData == 1 % gyro rate: Logmap(R_i' * R_i+1) / deltaT measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT; + % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Update velocity - currentVel = IMUdeltaPositionVector./deltaT; + currentVel = currentVel + measurements(i).imu(j).accel * deltaT; end end diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index 61830443a..73c60efc3 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -19,7 +19,7 @@ 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)); +gtRotation = Rot3; %Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); currentPose = Pose3(gtRotation, Point3(gtPosition)); end \ No newline at end of file