Added flag to use scenario 2 ground truth data

matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m

@@ -0,0 +1,26 @@
+function pos_ECEF = LatLonHRad_to_ECEF(LatLonH)
+% convert latitude, longitude, height to XYZ in ECEF coordinates 
+% LatLonH(1) : latitude in radian 
+% LatLonH(2) : longitude in radian
+% LatLonH(3) : height in meter
+%
+% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial
+% Navigation", 1997. pp. 10 Eq 1.18
+%
+
+% constants
+a = 6378137.0; %m
+e_sqr =0.006694379990141317; 
+% b = 6356752.3142; % m 
+
+%RAD_PER_DEG = pi/180;
+phi = LatLonH(1);%*RAD_PER_DEG;
+lambda = LatLonH(2);%*RAD_PER_DEG;
+h = LatLonH(3);
+
+RN = a/sqrt(1 - e_sqr*sin(phi)^2);
+
+pos_ECEF = zeros(3,1);
+pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda);
+pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda);
+pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ;

matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m

@@ -8,62 +8,118 @@ clc
 clear all
 close all
 
+%% Configuration
+useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj
+
 %% Create ground truth trajectory
 trajectoryLength = 49;
 unsmooth_DP = 0.5; % controls smoothness on translation norm
-unsmooth_DR = 0.1; % controls smoothness on translation norm
+unsmooth_DR = 0.1; % controls smoothness on rotation norm
 
-% possibly create random trajectory as ground Truth
 gtValues = Values;
 gtGraph = NonlinearFactorGraph;
 
-sigma_ang = 1e-2;
+if useAspnData == 1
-sigma_cart = 0.1;
+  sigma_ang = 1e-4;
+  sigma_cart = 40;
+else
+  sigma_ang = 1e-2;
+  sigma_cart = 0.1;
+end
 noiseVector = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
 noise = noiseModel.Diagonal.Sigmas(noiseVector);
 
-currentPoseKey = symbol('x', 0);
+if useAspnData == 1
-currentPose = Pose3; % initial pose
+% Create a ground truth trajectory using scenario 2 data
-gtValues.insert(currentPoseKey, currentPose);
+  fprintf('\nUsing Scenario 2 ground truth data\n');
-gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
+  % load scenario 2 ground truth data
-
+  gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
-for i=1:trajectoryLength
+    
-  currentPoseKey = symbol('x', i);
+  % Add first pose
-  gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
+  currentPoseKey = symbol('x', 0);
-  gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
+  initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
-  gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
+  initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
-  deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
+  currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose
-  
-  % "Deduce" ground truth measurements
-  % deltaPose are the gt measurements - save them in some structure
-  currentPose = currentPose.compose(deltaPose);
   gtValues.insert(currentPoseKey, currentPose);
+  gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
+  prevPose = currentPose;
   
-  % Add the factors to the factor graph
+  % Limit the trajectory length
-  gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise));
+  trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]);
-end
+  
+  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, noise));
+  end
+else
+% Create a random trajectory as ground truth
+  fprintf('\nCreating a random ground truth trajectory\n');
+  % Add first pose
+  currentPoseKey = symbol('x', 0);
+  currentPose = Pose3; % initial pose
+  gtValues.insert(currentPoseKey, currentPose);
+  gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
 
+  for i=1:trajectoryLength
+    currentPoseKey = symbol('x', 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,:)');
+
+    % "Deduce" ground truth measurements
+    % deltaPose are the gt measurements - save them in some structure
+    currentPose = currentPose.compose(deltaPose);
+    gtValues.insert(currentPoseKey, currentPose);
+
+     % Add the factors to the factor graph
+    gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise));
+  end
+end
 figure(1)
 hold on;
 plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
 axis equal
 
-numMonteCarloRuns = 100;
+numMonteCarloRuns = 10;
 for k=1:numMonteCarloRuns
+  fprintf('Monte Carlo Run %d.\n', k');
   % create a new graph
   graph = NonlinearFactorGraph;
   
   % noisy prior
-  currentPoseKey = symbol('x', 0);
+  if useAspnData == 1
-  noisyDelta = noiseVector .* randn(6,1);
+    currentPoseKey = symbol('x', 0);
-  initialPose = Pose3.Expmap(noisyDelta);
+    initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
-  graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise));
+    initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
+    initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose
+    graph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
+  else
+    currentPoseKey = symbol('x', 0);
+    noisyDelta = noiseVector .* randn(6,1);
+    initialPose = Pose3.Expmap(noisyDelta);
+    graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise));
+  end
   
-  for i=1:trajectoryLength
+  for i=1:size(gtDeltaMatrix,1)
     currentPoseKey = symbol('x', i);
     
     % for each measurement: add noise and add to graph
-    noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1));
+    noisyDelta = gtDeltaMatrix(i,:)';% + (noiseVector .* randn(6,1));
     noisyDeltaPose = Pose3.Expmap(noisyDelta);
     
     % Add the factors to the factor graph
@@ -80,7 +136,7 @@ for k=1:numMonteCarloRuns
   marginals = Marginals(graph, estimate);
   
   % for each pose in the trajectory
-  for i=1:trajectoryLength+1
+  for i=1:size(gtDeltaMatrix,1)+1
     % compute estimation errors
     currentPoseKey = symbol('x', i-1);
     gtPosition  = gtValues.at(currentPoseKey).translation.vector;