diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 68673d2c79f40e69b19352c00788c457873f8e71..ede5bd88523c313fd3bbd4bac9a8d2efaada1f54 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -35,8 +35,8 @@ void CaptureLaser2D::processCapture()
     std::list<Eigen::Vector4s> corners;
     
     //extract corners from range data
-    //std::cout << "CaptureLaser2D::extractCorners..." << std::endl;
     extractCorners(corners);
+    //std::cout << corners.size() << " corners extracted" << std::endl;
     
     //generate a feature for each corner
     //std::cout << "CaptureLaser2D::createFeatures..." << std::endl;
@@ -125,27 +125,26 @@ void CaptureLaser2D::establishConstraints()
     			theta_distance -= 2 * M_PI;
 			if (distance2 < min_distance2)
 			{
-
 				if (theta_distance < max_theta_matching)
 				{
-//					std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl;
-//					std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
+					//std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl;
+					//std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
 
 					correspondent_landmark = (LandmarkCorner2D*)(*landmark_it);
 					min_distance2 = distance2;
 				}
 				else
 				{
-					std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
-					std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl;
-					std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl;
-					std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
+//					std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
+//					std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl;
+//					std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl;
+//					std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
 				}
 			}
 		}
     	if (correspondent_landmark == nullptr)
     	{
-//    		std::cout << "No landmark found. Creating a new one..." << std::endl;
+    		//std::cout << "No landmark found. Creating a new one..." << std::endl;
     		StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
     		getTop()->addState(new_landmark_state_position, feature_global_position);
     		StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
@@ -155,8 +154,8 @@ void CaptureLaser2D::establishConstraints()
     		LandmarkBase* corr_landmark(correspondent_landmark);
     		getTop()->getMapPtr()->addLandmark(corr_landmark);
 
-    		std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl;
-			std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << " orientation:" << *corr_landmark->getOPtr()->getPtr() << std::endl;
+    		//std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl;
+			//std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << " orientation:" << *corr_landmark->getOPtr()->getPtr() << std::endl;
     	}
     	else
     		correspondent_landmark->hit();
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 7f2ec062a92142a0b49b70a7c79bf3f0e1db9267..5948c5e8a8f6160814ea0049352c80175cae34ba 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,10 +1,15 @@
 #include "ceres_manager.h"
 
 CeresManager::CeresManager(ceres::Problem::Options _options) :
-	ceres_problem_(new ceres::Problem(_options)),
-	covariance_(covariance_options_)
+	ceres_problem_(new ceres::Problem(_options))
 {
+	ceres::Covariance::Options covariance_options;
+	covariance_options.algorithm_type = ceres::DENSE_SVD;
+	covariance_options.null_space_rank = -1;
+	covariance_ = new ceres::Covariance(covariance_options);
 }
+//num_threads(1), use_dense_linear_algebra(true), min_singular_value_threshold(1e-12), null_space_rank(-1), apply_loss_function(false)
+
 
 CeresManager::~CeresManager()
 {
@@ -19,6 +24,7 @@ CeresManager::~CeresManager()
 	std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
 	std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << "\n";
 	delete ceres_problem_;
+	delete covariance_;
 }
 
 ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_options)
@@ -36,23 +42,59 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
 	return ceres_summary_;
 }
 
-void CeresManager::computeCovariances(StateBaseList* _state_list_ptr, StateBase* _current_state_unit)
+void CeresManager::computeCovariances(WolfProblem* _problem_ptr)//StateBaseList* _state_list_ptr, StateBase* _current_state_unit)
 {
-	std::cout << "_state_list_ptr.size() " << _state_list_ptr->size() << std::endl;
+//	std::cout << "_state_list_ptr.size() " << _state_list_ptr->size() << std::endl;
+//
+//	// create vector of pointer pairs
+//	std::vector<std::pair<const double*, const double*>> covariance_blocks;
+////	for (auto st_it = _state_list_ptr->begin(); st_it != _state_list_ptr->end(); st_it++)
+////		if ((*st_it)->getPtr() != _current_state_unit->getPtr())
+////			covariance_blocks.push_back(std::pair<const double*, const double*>((*st_it)->getPtr(),_current_state_unit->getPtr()));
+//
+//	WolfScalar* block_1_ptr = _current_state_unit->getPtr();
+//	WolfScalar* block_2_ptr = _current_state_unit->getPtr();
+//	std::cout << "are blocks? " << ceres_problem_->HasParameterBlock(block_1_ptr) << ceres_problem_->HasParameterBlock(block_2_ptr) << std::endl;
+//	covariance_blocks.push_back(std::make_pair(block_1_ptr,block_2_ptr));
+//	std::cout << "covariance_blocks.size() " << covariance_blocks.size() << std::endl;
+//	// Compute covariances
+//	covariance_.Compute(covariance_blocks, ceres_problem_);
 
-	// create vector of pointer pairs
 	std::vector<std::pair<const double*, const double*>> covariance_blocks;
-//	for (auto st_it = _state_list_ptr->begin(); st_it != _state_list_ptr->end(); st_it++)
-//		if ((*st_it)->getPtr() != _current_state_unit->getPtr())
-//			covariance_blocks.push_back(std::pair<const double*, const double*>((*st_it)->getPtr(),_current_state_unit->getPtr()));
-
-	WolfScalar* block_1_ptr = _current_state_unit->getPtr();
-	WolfScalar* block_2_ptr = _current_state_unit->getPtr();
-	std::cout << "are blocks? " << ceres_problem_->HasParameterBlock(block_1_ptr) << ceres_problem_->HasParameterBlock(block_2_ptr) << std::endl;
-	covariance_blocks.push_back(std::make_pair(block_1_ptr,block_2_ptr));
-	std::cout << "covariance_blocks.size() " << covariance_blocks.size() << std::endl;
-	// Compute covariances
-	covariance_.Compute(covariance_blocks, ceres_problem_);
+
+	// Last frame state units
+	double* current_position_ptr = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr();
+	double* current_orientation_ptr = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getOPtr()->getPtr();
+	covariance_blocks.push_back(std::make_pair(current_position_ptr,current_position_ptr));
+	covariance_blocks.push_back(std::make_pair(current_position_ptr,current_orientation_ptr));
+	covariance_blocks.push_back(std::make_pair(current_orientation_ptr,current_orientation_ptr));
+
+	// Landmarks and cross-covariance with current frame
+	for(auto landmark_it = _problem_ptr->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=_problem_ptr->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+	{
+		double* landmark_position_ptr = (*landmark_it)->getPPtr()->getPtr();
+		double* landmark_orientation_ptr = (*landmark_it)->getOPtr()->getPtr();
+
+		covariance_blocks.push_back(std::make_pair(landmark_position_ptr,landmark_position_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_position_ptr,current_position_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_position_ptr,current_orientation_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_position_ptr,landmark_orientation_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,landmark_orientation_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,current_position_ptr));
+		covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,current_orientation_ptr));
+	}
+	covariance_->Compute(covariance_blocks, ceres_problem_);
+
+	double c11[2 * 2];
+	double c22[1 * 1];
+	double c12[2 * 1];
+	covariance_->GetCovarianceBlock(current_position_ptr, current_position_ptr, c11);
+	covariance_->GetCovarianceBlock(current_position_ptr, current_orientation_ptr, c12);
+	covariance_->GetCovarianceBlock(current_orientation_ptr, current_orientation_ptr, c22);
+
+//	std::cout << "COV\n: " << c11[0] << " " << c11[1] << " " << c12[0] << std::endl <<
+//							   c11[2] << " " << c11[3] << " " << c12[1] << std::endl <<
+//							   c12[0] << " " << c12[1] << " " << c22[0] << std::endl;
 }
 
 void CeresManager::update(WolfProblem* _problem_ptr)
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index f43aee28e64e051e153be640350576832a5bad37..5b19540f531f5748c58406a22dc034fd892c3d98 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -29,8 +29,7 @@ class CeresManager
 	protected:
 //		std::map<unsigned int, ceres::ResidualBlockId> constraint_map_;
 		ceres::Problem* ceres_problem_;
-		ceres::Covariance covariance_;
-		ceres::Covariance::Options covariance_options_;
+		ceres::Covariance* covariance_;
 
 	public:
 		CeresManager(ceres::Problem::Options _options);
@@ -39,7 +38,7 @@ class CeresManager
 
 		ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
 
-		void computeCovariances(StateBaseList* _state_units_list, StateBase* _current_state_unit);
+		void computeCovariances(WolfProblem* _problem_ptr);//StateBaseList* _state_units_list, StateBase* _current_state_unit);
 
 		void update(const WolfProblemPtr _problem_ptr);
 
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 165566c201af24871dbcbb4d80ea81ace5c3b4c8..a127da1d7164f265db97e04c45bf784898f9c691 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -114,18 +114,15 @@ class WolfManager
         FrameBaseIter first_window_frame_;
 
     public:
-        WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const unsigned int& _w_size=10) :
+        WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame, const unsigned int& _w_size=10) :
         	use_complex_angles_(_complex_angle),
 			problem_(new WolfProblem(_state_length)),
 			sensor_prior_(_sensor_prior),
 			window_size_(_w_size)
 		{
-        	Eigen::VectorXs init_frame(use_complex_angles_ ? 4 : 3);
-        	if (use_complex_angles_)
-        		init_frame << 2, 8, 1, 0;
-        	else
-        		init_frame << 2, 8, 0;
-        	createFrame(init_frame, 0);
+        	createFrame(_init_frame, TimeStamp());
+        	problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
+
 		}
 
         virtual ~WolfManager()
@@ -238,40 +235,20 @@ class WolfManager
         	}
         }
 
-        const Eigen::VectorXs getState() const
-        {
-        	return problem_->getState();
-        }
-
-        StateBaseList* getStateListPtr()
-		{
-        	return problem_->getStateListPtr();
-		}
-
         Eigen::VectorXs getVehiclePose()
 		{
         	return Eigen::Map<Eigen::VectorXs>(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3);
 		}
 
-        void getConstraintsList(ConstraintBaseList& corr_list)
-        {
-        	problem_->getTrajectoryPtr()->getConstraintList(corr_list);
-        }
-
         WolfProblem* getProblemPtr()
         {
         	return problem_;
         }
-
-        void printTree()
-        {
-        	problem_->print();
-        }
 };
 
 int main(int argc, char** argv)
 {
-	std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n";
+	std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
 
 	// USER INPUT ============================================================================================
 	if (argc!=4 || atoi(argv[1])<1 || atoi(argv[1])>1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1)
@@ -284,9 +261,6 @@ int main(int argc, char** argv)
 		return -1;
 	}
 
-	clock_t t1, t2;
-	t1=clock();
-
 	unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
 	unsigned int window_size = (unsigned int) atoi(argv[2]);
 	bool complex_angle = (bool) atoi(argv[3]);
@@ -317,8 +291,7 @@ int main(int argc, char** argv)
 	std::ofstream log_file, landmark_file;  //output file
 
 	// Faramotics stuff
-	Cpose3d viewPoint;
-	Cpose3d devicePose, laser1Pose, laser2Pose;
+	Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
 	vector<Cpose3d> devicePoses;
 	vector<float> scan1, scan2;
 	string modelFileName;
@@ -333,7 +306,6 @@ int main(int argc, char** argv)
 	viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/2, viewPoint.rt.pitch()+30.*M_PI/180., viewPoint.rt.roll() );
 	viewPoint.moveForward(-15);
 	//glut initialization
-	//glutInit(&argc, argv);
     faramotics::initGLUT(argc, argv);
     
 	//create a viewer for the 3D model and scan points
@@ -345,30 +317,28 @@ int main(int argc, char** argv)
     
 	//variables
 	Eigen::Vector2s odom_reading, gps_fix_reading;
-	Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
 	Eigen::VectorXs pose_odom(3); //current odometry integred pose
 	Eigen::VectorXs ground_truth(n_execution*3); //all true poses
 	Eigen::VectorXs odom_trajectory(n_execution*3); //open loop trajectory
-	Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
-	Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
-	TimeStamp time_stamp;
 	Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+	clock_t t1, t2;
 
 	// Wolf manager initialization
-	SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std);
-	//SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std);
+	SensorOdom2D odom_sensor(Eigen::Vector6s::Zero(), odom_std, odom_std);
+	SensorGPSFix gps_sensor(Eigen::Vector6s::Zero(), gps_std);
 	Eigen::Vector6s laser_1_pose, laser_2_pose;
 	laser_1_pose << 1.2,0,0,0,0,0; //laser 1
 	laser_2_pose << -1.2,0,0,0,0,M_PI; //laser 2
 	SensorLaser2D laser_1_sensor(laser_1_pose);
 	SensorLaser2D laser_2_sensor(laser_2_pose);
-	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, window_size);
 
 	// Initial pose
 	pose_odom << 2,8,0;
 	ground_truth.head(3) = pose_odom;
 	odom_trajectory.head(3) = pose_odom;
 
+	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, pose_odom, window_size);
+
 	std::cout << "START TRAJECTORY..." << std::endl;
 	// START TRAJECTORY ============================================================================================
 	for (uint step=1; step < n_execution; step++)
@@ -400,9 +370,9 @@ int main(int argc, char** argv)
 		odom_trajectory.segment(step*3,3) = pose_odom;
 
 		// compute GPS
-		//gps_fix_reading << devicePose.pt(0), devicePose.pt(1), 0;
-		//gps_fix_reading(0) += distribution_gps(generator);
-		//gps_fix_reading(1) += distribution_gps(generator);
+		gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
+		gps_fix_reading(0) += distribution_gps(generator);
+		gps_fix_reading(1) += distribution_gps(generator);
 
 		//compute scans
 		scan1.clear();
@@ -428,12 +398,10 @@ int main(int argc, char** argv)
 		//std::cout << "ADD CAPTURES..." << std::endl;
 		t1=clock();
 		// adding new sensor captures
-	    time_stamp.setToNow();
-		wolf_manager->addCapture(new CaptureOdom2D(time_stamp, &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2)));
-		//wolf_manager->addCapture(new CaptureGPSFix(time_stamp, &gps_sensor, gps_fix_reading, gps_std * MatrixXs::Identity(3,3)));
-		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_1_sensor, scan1_reading));
-		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_2_sensor, scan2_reading));
-
+		wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2)));
+//		wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1_reading));
+		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2_reading));
         // updating problem
 		wolf_manager->update();
 		mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC;
@@ -448,7 +416,7 @@ int main(int argc, char** argv)
 
 
 		// SOLVE OPTIMIZATION ---------------------------
-		std::cout << "SOLVING..." << std::endl;
+		//std::cout << "SOLVING..." << std::endl;
 		t1=clock();
 		ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
 		//std::cout << summary.FullReport() << std::endl;
@@ -457,8 +425,7 @@ int main(int argc, char** argv)
 		// COMPUTE COVARIANCES ---------------------------
 		//std::cout << "COMPUTING COVARIANCES..." << std::endl;
 		t1=clock();
-//		if (step > 2)
-//			ceres_manager->computeCovariances(wolf_manager->getProblemPtr()->getStateListPtr(), wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr());
+		//ceres_manager->computeCovariances(wolf_manager->getProblemPtr());
 		mean_times(4) += ((double)clock()-t1)/CLOCKS_PER_SEC;
 
 		// DRAWING STUFF ---------------------------
@@ -466,7 +433,7 @@ int main(int argc, char** argv)
 		// draw detected corners
 		std::list<Eigen::Vector4s> corner_list;
 		std::vector<double> corner_vector;
-		CaptureLaser2D last_scan(time_stamp, &laser_1_sensor, scan1_reading);
+		CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1_reading);
 		last_scan.extractCorners(corner_list);
 		for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
 		{
@@ -487,15 +454,13 @@ int main(int argc, char** argv)
 		myRender->drawLandmarks(landmark_vector);
 
 		// draw localization and sensors
-		Cpose3d estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
 		estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0),wolf_manager->getVehiclePose()(1),0.2,wolf_manager->getVehiclePose()(2),0,0);
 		estimated_laser_1_pose.setPose(estimated_vehicle_pose);
 		estimated_laser_1_pose.moveForward(laser_1_pose(0));
 		estimated_laser_2_pose.setPose(estimated_vehicle_pose);
 		estimated_laser_2_pose.moveForward(laser_2_pose(0));
 		estimated_laser_2_pose.rt.setEuler( estimated_laser_2_pose.rt.head()+M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll() );
-		std::vector<Cpose3d> estimated_poses_vector{estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose};
-		myRender->drawPoseAxisVector(estimated_poses_vector);
+		myRender->drawPoseAxisVector({estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose});
 
 		//Set view point and render the scene
 		//locate visualization view point, somewhere behind the device
@@ -511,12 +476,11 @@ int main(int argc, char** argv)
 		// TIME MANAGEMENT ---------------------------
 		double dt = ((double)clock()-t2)/CLOCKS_PER_SEC;
 		mean_times(6) += dt;
-
 		if (dt < 0.1)
 			usleep(100000-1e6*dt);
 
 //		std::cout << "\nTree after step..." << std::endl;
-//		wolf_manager->printTree();
+//		wolf_manager->getProblemPtr()->print();
 	}
 
 	// DISPLAY RESULTS ============================================================================================
@@ -531,7 +495,7 @@ int main(int argc, char** argv)
 	std::cout << "  loop time:          " << mean_times(6) << std::endl;
 
 //	std::cout << "\nTree before deleting..." << std::endl;
-//	wolf_manager->printTree();
+//	wolf_manager->getProblemPtr()->print();
 
 	// Draw Final result -------------------------
 	std::vector<double> landmark_vector;
@@ -584,8 +548,7 @@ int main(int argc, char** argv)
 			log_file << state_poses.segment(ii*3,3).transpose()
 					 << "\t" << ground_truth.segment(ii*3,3).transpose()
 					 << "\t" << (state_poses.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
-					 << "\t" << odom_trajectory.segment(ii*3,3).transpose()
-					 << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
+					 << "\t" << odom_trajectory.segment(ii*3,3).transpose() << std::endl;
 		log_file.close(); //close log file
 		std::cout << std::endl << "Result file " << filepath << std::endl;
 	}
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index 64e1a2f5f96dfdafbd16e8da825d9a8e6775fb72..9ad87ed7267cb2bab2e46764859f3626990479b4 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -504,6 +504,32 @@ class CeresManager
 			// run Ceres Solver
 			ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
 
+			// compute covariances
+			ceres::Covariance::Options covariance_options;
+			covariance_options.algorithm_type = ceres::DENSE_SVD;
+			covariance_options.null_space_rank = -1;
+			ceres::Covariance covariance_(covariance_options);
+
+			std::vector<double*> parameter_blocks;
+			ceres_problem_->GetParameterBlocks(&parameter_blocks);
+
+			std::vector<std::pair<const double*, const double*>> covariance_blocks;
+			covariance_blocks.push_back(std::make_pair(parameter_blocks[0],parameter_blocks[1]));
+			covariance_.Compute(covariance_blocks, ceres_problem_);
+
+			double covariance_12[3 * 3];
+			covariance_.GetCovarianceBlock(parameter_blocks[0], parameter_blocks[1], covariance_12);
+
+			std::cout << "12 cov: " << covariance_12[0] << " " <<
+										covariance_12[1] << " " <<
+										covariance_12[2] << " " <<
+										covariance_12[3] << " " <<
+										covariance_12[4] << " " <<
+										covariance_12[5] << " " <<
+										covariance_12[6] << " " <<
+										covariance_12[7] << " " <<
+										covariance_12[8] << " " <<std::endl;
+
 			//display results
 			return ceres_summary_;
 		}
diff --git a/src/state_base.cpp b/src/state_base.cpp
index 5ec13c7ee48b1e8bf2961ea57a9c13161e26ba4d..52ecb845c8f5c2b0a8f0f86df7b5efe690faf2e5 100644
--- a/src/state_base.cpp
+++ b/src/state_base.cpp
@@ -41,5 +41,6 @@ StateStatus StateBase::getStateStatus() const
 void StateBase::setStateStatus(const StateStatus& _status)
 {
 	status_=_status;
-	setPendingStatus(UPDATE_PENDING);
+	if (getPendingStatus() != ADD_PENDING)
+		setPendingStatus(UPDATE_PENDING);
 }