amost fixed imu simulation

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

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m

@@ -74,6 +74,9 @@ for i=0:length(measurements)
           
           % Preintegrate
           imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
+        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));
@@ -81,7 +84,6 @@ for i=0:length(measurements)
         graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
           noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
       end
-      end
       
       
       if options.imuFactorType == 2

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
       

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