diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m
new file mode 100644
index 000000000..22b6a2de5
--- /dev/null
+++ b/matlab/+gtsam/plotFlyingResults.m
@@ -0,0 +1,41 @@
+function plotFlyingResults(pts3d, covariance, values, marginals)
+% plot the visible points on the cylinders and trajectories
+% author: Zhaoyang Lv
+
+import gtsam.*
+
+haveMarginals = exist('marginals', 'var');
+keys = KeyVector(values.keys);
+
+holdstate = ishold;
+hold on
+
+keys = KeyVector(values.keys);
+
+%% plot trajectories
+for i = 0:keys.size - 1
+    if exist('h_result', 'var')
+        delete(h_result);
+    end
+
+    key = keys.at(i);
+    pose = keys.at(key);
+    
+    P = marginals.marginalCovariance(key);
+    
+    h_result = gtsam.plotPose3(pose, P, 1);
+end
+
+%% plot point covariance
+
+if exist('h_result', 'var')
+    delete(h_result);
+end
+
+
+
+if ~holdstate
+    hold off
+end
+    
+end
\ No newline at end of file
diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m
deleted file mode 100644
index f59434d96..000000000
--- a/matlab/+gtsam/plotProjectedCylinderSamples.m
+++ /dev/null
@@ -1,42 +0,0 @@
-function plotProjectedCylinderSamples(pts3d, covariance, options, figID)
-% plot the visible points on the cylinders
-% author: Zhaoyang Lv
-
-    import gtsam.*
-
-    figure(figID);
-
-    holdstate = ishold;
-    hold on
-
-    pointsNum = length(pts3d);
-    for i = 1:pointsNum
-        %gtsam.plotPoint3(p, 'g', covariance{i});
-        plotPoint3(pts3d{i}, 'r', covariance{i}); 
-        hold on
-    end
-    
-%     for i=1:pointsNum
-%         ray = pts2dTracksStereo{i}.between(cameraPose.translation()).vector();
-%         dist = norm(ray);
-%         
-%         p = plot3(pts2dTracksStereo{i}.x, pts2dTracksStereo{i}.y, pts2dTracksStereo{i}.z, ...
-%             'o', 'MarkerFaceColor', 'Green');
-%         
-%         for t=0:0.1:dist
-%             marchingRay = ray * t;
-%             p.XData = pts2dTracksStereo{i}.x + marchingRay(1);
-%             p.YData = pts2dTracksStereo{i}.y + marchingRay(2);
-%             p.ZData = pts2dTracksStereo{i}.z + marchingRay(3);
-%             drawnow update
-%         end
-%         
-%     end
-
-    axis equal;
-    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
-    
-    if ~holdstate
-        hold off
-    end
-end
\ No newline at end of file
diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m
index a4e1a2e5c..04c896188 100644
--- a/matlab/+gtsam/points2DTrackMonocular.m
+++ b/matlab/+gtsam/points2DTrackMonocular.m
@@ -101,20 +101,8 @@ for i = 1:pointsNum
     end
 end
 
-% for k = 1:cameraPosesNum
-%     num = length(pts3d{k}.data);
-%     for i = 1:num
-%         pts2dTracksMono.pt3d{i} = pts3d{k}.data{i};
-%         pts2dTracksMono.Z{i} = pts3d{k}.Z{i};
-%         pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
-%     end
-% end
-
-%% plot the result with covariance ellipses
-hold on;
 %plot3DPoints(initialEstimate, [], marginals);
 %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
-plot3DTrajectory(initialEstimate, '*', 1, 8);
 view(3);
 
 
diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m
index 2359654d3..95f225ea5 100644
--- a/matlab/+gtsam/points2DTrackStereo.m
+++ b/matlab/+gtsam/points2DTrackStereo.m
@@ -1,4 +1,4 @@
-function pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders)
+function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders)
 % Assess how accurately we can reconstruct points from a particular monocular camera setup. 
 % After creation of the factor graph for each track, linearize it around ground truth. 
 % There is no optimization
@@ -100,24 +100,10 @@ for i = 1:pointsNum
    
 end
 
-
-% for k = 1:cameraPosesNum
-%     if isempty(pts3d{k}.data)
-%         continue;
-%     end
-% 
-%     for i = 1:length(pts3d{k}.data)
-%         pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i};
-%         pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i};
-%         pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
-%     end
-% end
-
 %% plot the result with covariance ellipses
-hold on;
-%plot3DPoints(initialEstimate, [], marginals);
+plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals);
+
 %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
-plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
-view(3);
+%view(3);
 
 end
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index ba9434080..08986f83c 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -10,100 +10,135 @@
 % @author Zhaoyang Lv
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+
 clear all;
 clc;
 clf;
+
 import gtsam.*
 
-%% define the options 
-% the testing field size
-options.fieldSize = Point2([100, 100]');
-% the number of cylinders
-options.cylinderNum = 20;
-% point density on cylinder
-options.density = 1;
+% test or run
+options.enableTests = false;
 
-% The number of camera poses
-options.poseNum = 20;
-% covariance scaling factor
+% the number of cylinders in the field
+options.cylinder.cylinderNum = 15; % pls be smaller than 20
+% cylinder size
+options.cylinder.radius = 3;  % pls be smaller than 5
+options.cylinder.height = 10;
+% point density on cylinder
+options.cylinder.pointDensity = 1;
+
+%% set up the camera
+%   parameters set according to the stereo camera:
+%   http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
+
+% set up monocular camera or stereo camera
+options.camera.IS_MONO = false;
+% the field of view of camera
+options.camera.fov = 120;
+% fps for image
+options.camera.fps = 25;
+% camera pixel resolution
+options.camera.resolution = Point2(752, 480);
+% camera horizon
+options.camera.horizon = 60;
+% camera baseline
+options.camera.baseline = 0.05;
+% camera focal length
+options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
+    options.camera.fov);
+% camera focal baseline
+options.camera.fB = options.camera.f * options.camera.baseline;
+% camera disparity
+options.camera.disparity = options.camera.fB / options.camera.horizon;
+% Monocular Camera Calibration
+options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2);
+% Stereo Camera Calibration
+options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
+
+% write video output
+options.writeVideo = true;
+% the testing field size (unit: meter)
+options.fieldSize = Point2([100, 100]');
+% camera flying speed (unit: meter / second)
+options.speed = 20;
+% number of camera poses in the simulated trajectory
+options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps);
+% display covariance scaling factor
 options.scale = 1;
 
-
-%% Camera Setup
-% Monocular Camera Calibration
-options.monoK = Cal3_S2(525,525,0,320,240);
-% Stereo Camera Calibration
-options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-% the image size of camera
-options.imageSize = Point2([640, 480]');
-% use Monocular camera or Stereo camera
-options.Mono = false;
-% fps for image
-options.fps = 20;
-% camera flying speed
-options.speed = 20;
+% if(options.writeVideo)
+%     videoObj = VideoWriter('Camera_Flying_Example.avi');
+%     videoObj.Quality = 100;
+%     videoObj.FrameRate = options.fps;
+% end
 
 
-%% test1: visibility test in monocular camera 
-cylinders{1}.centroid = Point3(30, 50, 5);
-cylinders{2}.centroid = Point3(50, 50, 5);
-cylinders{3}.centroid = Point3(70, 50, 5);
+%% This is for tests
+if options.enableTests
+    % test1: visibility test in monocular camera 
+    cylinders{1}.centroid = Point3(30, 50, 5);
+    cylinders{2}.centroid = Point3(50, 50, 5);
+    cylinders{3}.centroid = Point3(70, 50, 5);
 
-for i = 1:3
-    cylinders{i}.radius = 5;
-    cylinders{i}.height = 10;
-    
-    cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
-    cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
+    for i = 1:3
+        cylinders{i}.radius = 5;
+        cylinders{i}.height = 10;
+
+        cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
+        cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
+    end
+
+    camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
+        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
+        Point3([0,0,1]'), options.monoK); 
+
+    pose = camera.pose;
+    prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
+        options.camera.resolution, cylinders);
+
+    % test2: visibility test in stereo camera  
+    prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
+        pose, options.camera.resolution, cylinders);
 end
 
-camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
-    Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
-    Point3([0,0,1]'), options.monoK); 
-
-pose = camera.pose;
-prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
-
-%% test2: visibility test in stereo camera  
-prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
-
-%% generate a set of cylinders and samples
-cylinderNum = options.cylinderNum;
+%% generate a set of cylinders and point samples on cylinders
+cylinderNum = options.cylinder.cylinderNum;
 cylinders = cell(cylinderNum, 1);
-
-% It seems random generated cylinders doesn't work that well
-% Now it set up a circle of cylinders
+baseCentroid = cell(cylinderNum, 1);
 theta = 0;
-for i = 1:cylinderNum
+i = 1;
+while i <= cylinderNum
     theta = theta + 2*pi/10;
-    x = 30 * rand * cos(theta) + options.fieldSize.x/2;
+    x = 40 * rand * cos(theta) + options.fieldSize.x/2;
     y = 20 * rand * sin(theta) + options.fieldSize.y/2;
-    baseCentroid = Point2([x, y]');
-    cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density);
+    baseCentroid{i} = Point2([x, y]');
+    
+    % prevent two cylinders interact with each other
+    regenerate = false;
+    for j = 1:i-1
+        if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
+            regenerate = true;
+            break;
+        end
+    end
+    if regenerate 
+       continue;
+    end
+    
+    cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
+         options.cylinder.height, options.cylinder.pointDensity);
+    i = i+1;
 end
 
-
 %% plot all the cylinders and sampled points
 % now is plotting on a 100 * 100 field
 figID = 1;
 figure(figID);
 plotCylinderSamples(cylinders, options, figID);
 
-% %% generate ground truth camera trajectories: a circle 
-% KMono = Cal3_S2(525,525,0,320,240);
-% cameraPoses = cell(options.poseNum, 1);
-% theta = 0;
-% r = 40;
-% for i = 1:options.poseNum    
-%     theta = (i-1)*2*pi/options.poseNum;
-%     t = Point3([r*cos(theta) + options.fieldSize.x/2, ...
-%         r*sin(theta) + options.fieldSize.y/2, 10]');
-%     camera = SimpleCamera.Lookat(t, ... 
-%         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
-%         Point3([0,0,1]'), options.monoK);    
-%     cameraPoses{i} = camera.pose;
-% end
-
 %% generate ground truth camera trajectories: a line
 KMono = Cal3_S2(525,525,0,320,240);
 cameraPoses = cell(options.poseNum, 1);
@@ -113,24 +148,22 @@ for i = 1:options.poseNum
         15, 10]');
     camera = SimpleCamera.Lookat(t, ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
-        Point3([0,0,1]'), options.monoK);    
+        Point3([0,0,1]'), options.camera.monoK);    
     cameraPoses{i} = camera.pose;
 end
 
 
 %% set up camera and get measurements
-if options.Mono 
+if options.camera.IS_MONO 
     % use Monocular Camera
-    pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ...
-        options.imageSize, cylinders);
+    pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
+        options.camera.resolution, cylinders);
 else 
     % use Stereo Camera
-    pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ...
-        options.imageSize, cylinders);
+    [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ...
+        cameraPoses, options.camera.resolution, cylinders);
     
-    figID = 2;
-    plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID);
-
+    plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID);
 end
 
 %% plot all the projected points
@@ -142,6 +175,10 @@ end
 %plot3DTrajectory();
 
 
+%%close video
+% if(options.writeVideo)
+%     close(videoObj);
+% end