diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
index f7fd84989..b6f79bf02 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
@@ -19,7 +19,8 @@ for i=0:length(measurements)
   if i==0
     %% first time step, add priors
     % Pose prior (poses used for all factors)
-    initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1));
+    noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* rand(6,1); 
+    initialPose = Pose3.Expmap(noisyInitialPoseVector);
     graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
     
     % IMU velocity and bias priors
@@ -183,10 +184,12 @@ for i=0:length(measurements)
 end % end of for over trajectory
 
 %% Add Camera Factors to the graph
+% Only factors for landmarks that have been viewed at least once are added
+% to the graph
 [find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
 if options.includeCameraFactors == 1
   for j = 1:options.numberOfLandmarks
-    if projectionFactorSeenBy(j) > 5
+    if projectionFactorSeenBy(j) > 0
       graph.add(SmartProjectionFactors(j));
     end
   end
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
index 8b55f85a6..eb735a5d7 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
@@ -30,7 +30,7 @@ if options.useRealData == 1
     currentPoseKey = symbol('x', i);
     currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
     %% FOR TESTING
-    %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
+    currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
     
     % add to values
     values.insert(currentPoseKey, currentPose);
diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
index bf8da3343..070c0fb4c 100644
--- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
+++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
@@ -18,8 +18,19 @@ 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.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
-currentPose = Pose3(gtRotation, Point3(gtPosition));
+gtPosition = Point3([xlt, ylt, zlt]');
+% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data
+%   yaw = measured positively to the right
+%   pitch = measured positively up
+%   roll = measured positively to right
+% Assumes vehice X forward, Y right, Z down
+%
+% In the gtScenario data
+%   heading (yaw) = measured positively to the left from Y-axis
+%   pitch = 
+%   roll = 
+% Coordinate frame is Y forward, X is right, Z is up
+gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), -gtScenario.Pitch(scenarioInd), -gtScenario.Roll(scenarioInd));
+currentPose = Pose3(gtBodyRotation, gtPosition);
 
 end
\ No newline at end of file