diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 2a49b96da7b7fa4df0d7beb7296d944398bc4366..b7ddaffe9d694e6ba363936522998ff22fb9775f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -92,6 +92,7 @@ SET(HDRS
     node_linked.h
     processor_base.h
     processor_laser_2D.h
+    raw_data_satellite.h
     sensor_base.h
     sensor_odom_2D.h
     sensor_gps_fix.h
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index d665f7aebe1d7a77914c03effdab28450e3793e4..5a413a1e2f080e7cc81d34facd2c56ce3d9845ba 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -6,7 +6,7 @@ CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eige
 	data_(_data),
 	data_covariance_(_data_covariance)
 {
-    std::cout << "capture fix constructor " << std::endl;
+    //std::cout << "capture fix constructor " << std::endl;
 }
 
 CaptureFix::~CaptureFix()
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index ad535c270a7988e780cd3c0de743f2c310a3b4fd..b8c996418d6c81e1d7e987316cd66c3f9100762e 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -15,9 +15,8 @@ class CaptureOdom2D : public CaptureMotion
 {
 
     protected:
-        Eigen::VectorXs data_; ///< Raw data.
-        Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
+        //Eigen::VectorXs data_; ///< Raw data.
+        //Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
 
     public:
 
diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
index 978da76a800479b2c588386e00387f079c4f3609..f948cf58ed05dcba062afa286c107bece2e14976 100644
--- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
+++ b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
@@ -55,7 +55,7 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
             {
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          parameters[5], parameters[6], parameters[7], parameters[8], parameters[9], residuals);
@@ -107,13 +107,12 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<this->n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<this->n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -169,7 +168,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          parameters[5], parameters[6], parameters[7], parameters[8], residuals);
 
@@ -217,13 +216,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -278,7 +276,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          parameters[5], parameters[6], parameters[7], residuals);
 
@@ -323,13 +321,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -383,7 +380,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          parameters[5], parameters[6], residuals);
 
@@ -425,13 +422,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -484,7 +480,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          parameters[5], residuals);
 
@@ -523,13 +519,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -578,7 +573,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
                                          residuals);
 
@@ -614,13 +609,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -668,7 +662,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], residuals);
             // also compute jacobians
             else
@@ -699,13 +693,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -752,7 +745,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], residuals);
 
             // also compute jacobians
@@ -781,13 +774,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -833,7 +825,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], parameters[1], residuals);
 
             // also compute jacobians
@@ -859,13 +851,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
@@ -910,7 +901,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
             // only residuals
-            if (jacobians != nullptr)
+            if (jacobians == nullptr)
                 (*this->constraint_ptr_)(parameters[0], residuals);
             // also compute jacobians
             else
@@ -932,13 +923,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
                     residuals[i] = residuals_jets[i].a;
 
                 // fill the jacobian matrices
-                if (jacobians != nullptr)
-                    for (i = 0; i<n_blocks_; i++)
-                        if (jacobians[i] != nullptr)
-                            for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
-                                std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
-                                          residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
-                                          jacobians[i] + row * block_sizes_.at(i));
+                for (i = 0; i<n_blocks_; i++)
+                    if (jacobians[i] != nullptr)
+                        for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+                            std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+                                      jacobians[i] + row * block_sizes_.at(i));
             }
             return true;
         }
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 75bd626e9e7e6856e32f17a13a074f1d7585e76a..889361768288837c37f3fa2555a4950cfb73f971 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -37,7 +37,6 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
 
 	// run Ceres Solver
 	ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
-	//std::cout << "solved" << std::endl;
 
 	//return results
 	return ceres_summary_;
@@ -224,7 +223,7 @@ void CeresManager::removeConstraint(const unsigned int& _corr_id)
 
 void CeresManager::addStateBlock(StateBlock* _st_ptr)
 {
-	//std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl;
+	//std::cout << "Adding State Unit with size: " <<  _st_ptr->getSize() << std::endl;
 	//_st_ptr->print();
 
 	switch (_st_ptr->getType())
diff --git a/src/constraint_container.h b/src/constraint_container.h
index dd45e22fc745ec8036400be795f8aa8c55a45813..53deffda0b552067c17f47a19a524c29ffe2ce4f 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -21,7 +21,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
 			corner_(_corner)
 		{
             assert(_corner >= 0 && _corner <= 3 && "Wrong corner id in constraint container constructor");
-            lmk_ptr_->addConstraintTo(this);
 		}
 
         /** \brief Default destructor (not recommended)
@@ -32,7 +31,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
 		virtual ~ConstraintContainer()
 		{
 			//std::cout << "deleting ConstraintContainer " << nodeId() << std::endl;
-			lmk_ptr_->removeConstraintTo(this);
 		}
 
 		LandmarkContainer* getLandmarkPtr()
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index d603f91655203a1d5faa4fae8f3fd8d28f754b95..e74217a38900fe5d01ee5c8a8ad7b1a03f9c9845 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -14,7 +14,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
 		ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, ConstraintStatus _status = CTR_ACTIVE) :
 			ConstraintSparse<3,2,1,2,1>(_ftr_ptr, CTR_CORNER_2D, _lmk_ptr, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
 		{
-		    landmark_ptr_->addConstraintTo(this);
+		    //
 		}
 
         /** \brief Default destructor (not recommended)
@@ -25,7 +25,6 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
 		virtual ~ConstraintCorner2D()
 		{
 			//std::cout << "deleting ConstraintCorner2D " << nodeId() << std::endl;
-		    landmark_ptr_->removeConstraintTo(this);
 		}
 
 		LandmarkCorner2D* getLandmarkPtr()
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index fef49f4b9b768cf193be59ffb4d5d5512d1eae69..6258758b88500251a3952ca6c47758650a766b45 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -30,6 +30,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
 		template <typename T>
 		bool operator()(const T* const _p, const T* const _o, T* _residuals) const
 		{
+            //std::cout << "computing constraint odom ..." << std::endl;
+
 		    _residuals[0] = (T(getMeasurement()(0)) - _p[0]) / T(sqrt(getMeasurementCovariance()(0,0)));
             _residuals[1] = (T(getMeasurement()(1)) - _p[1]) / T(sqrt(getMeasurementCovariance()(1,1)));
             _residuals[2] = T(getMeasurement()(2)) - _o[0];
@@ -49,6 +51,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
 //            std::cout << "residual:      " << _residuals[2] << std::endl << std::endl;
             _residuals[2] = _residuals[2] / T(sqrt(getMeasurementCovariance()(2,2)));
 
+            //std::cout << "constraint fix computed!" << std::endl;
+
 			return true;
 		}
 };
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 312d55c3e39b8f592d3dcaa95753b70e4ee875d3..83c9706ba5bb3fb246d429205966802e521c29c3 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -29,6 +29,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
         template<typename T>
         bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
         {
+            //std::cout << "computing constraint odom ..." << std::endl;
+
 //            std::cout << "_p1: ";
 //            for (int i=0; i < 2; i++)
 //                std::cout << "\n\t" << _p1[i];
@@ -72,6 +74,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
 
             _residuals[2] = _residuals[2] / T(sqrt(std::max(getMeasurementCovariance()(2, 2),1e-6)));
 
+            //std::cout << "constraint odom computed!" << std::endl;
+
             return true;
         }
 };
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index c637f88d73f1188d3b03d0e3d22c009d61696645..6973102865834ef7b0bfa412f6b65f20f3744c17 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -52,6 +52,11 @@ IF(faramotics_FOUND)
                                 ${pose_state_time_LIBRARIES} 
                                 ${faramotics_LIBRARIES}
                                 ${PROJECT_NAME})
+        ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
+        TARGET_LINK_LIBRARIES(test_autodiff 
+                                ${pose_state_time_LIBRARIES} 
+                                ${faramotics_LIBRARIES}
+                                ${PROJECT_NAME})
                                 
         IF(Suitesparse_FOUND)
             ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index a75890a5a1079db6836b04e711a28f78372f96b2..9250a851ce250ff1b84d3fcaa313892095332ebf 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -19,6 +19,7 @@
 //Wolf includes
 #include "state_block.h"
 #include "constraint_base.h"
+#include "sensor_laser_2D.h"
 #include "wolf_manager.h"
 
 // wolf solver
@@ -281,16 +282,16 @@ int main(int argc, char *argv[])
 
         // DRAWING STUFF ---------------------------
         // draw detected corners
-        std::list < laserscanutils::Corner > corner_list;
-        std::vector<double> corner_vector;
-        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-        last_scan.extractCorners(corner_list);
-        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-        {
-            corner_vector.push_back(corner_it->pt_(0));
-            corner_vector.push_back(corner_it->pt_(1));
-        }
-        myRender->drawCorners(laser1Pose, corner_vector);
+//        std::list < laserscanutils::Corner > corner_list;
+//        std::vector<double> corner_vector;
+//        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+//        last_scan.extractCorners(corner_list);
+//        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+//        {
+//            corner_vector.push_back(corner_it->pt_(0));
+//            corner_vector.push_back(corner_it->pt_(1));
+//        }
+//        myRender->drawCorners(laser1Pose, corner_vector);
 
         // draw landmarks
         std::vector<double> landmark_vector;
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index c3ac998f6a90ba086b377719850179943547e9a4..e3431fd54718399ed4f0fc427a274e671d23d10e 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -17,6 +17,7 @@
 
 //Wolf includes
 #include "wolf_manager.h"
+#include "sensor_laser_2D.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 //C includes for sleep, time and main args
@@ -228,16 +229,16 @@ int main(int argc, char** argv)
         // DRAWING STUFF ---------------------------
         t1 = clock();
         // draw detected corners
-        std::list < laserscanutils::Corner > corner_list;
-        std::vector<double> corner_vector;
-        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-        last_scan.extractCorners(corner_list);
-        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-        {
-            corner_vector.push_back(corner_it->pt_(0));
-            corner_vector.push_back(corner_it->pt_(1));
-        }
-        myRender->drawCorners(laser1Pose, corner_vector);
+//        std::list < laserscanutils::Corner > corner_list;
+//        std::vector<double> corner_vector;
+//        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+//        last_scan.extractCorners(corner_list);
+//        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+//        {
+//            corner_vector.push_back(corner_it->pt_(0));
+//            corner_vector.push_back(corner_it->pt_(1));
+//        }
+//        myRender->drawCorners(laser1Pose, corner_vector);
 
         // draw landmarks
         std::vector<double> landmark_vector;
diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp
index 4c60e41efb87d613b93284a4283605ee841a37b4..0eb5be10782e3d1511a83cf4ecf74ec8a8bcfc18 100644
--- a/src/feature_odom_2D.cpp
+++ b/src/feature_odom_2D.cpp
@@ -9,7 +9,7 @@ FeatureOdom2D::FeatureOdom2D(unsigned int _dim_measurement) :
 FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
     FeatureBase(_measurement, _meas_covariance)
 {
-	//
+    //std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
 }
 
 FeatureOdom2D::~FeatureOdom2D()
diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp
index 0b05a15ec498341b9662f91c4491598b8550d752..4f24b920c6346e75df90bfe920a2af162068bb5f 100644
--- a/src/processor_laser_2D.cpp
+++ b/src/processor_laser_2D.cpp
@@ -2,7 +2,7 @@
 
 
 ProcessorLaser2D::ProcessorLaser2D() :
-        sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time
+        //sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time TODO: in construction time upperNodePtr is nullptr, it crashes always, to be removed or changed to somewhere (JVN)
         capture_laser_ptr_(nullptr)
 {
 }
@@ -13,6 +13,9 @@ ProcessorLaser2D::~ProcessorLaser2D()
 
 void ProcessorLaser2D::extractFeatures(CaptureBase* capture_ptr_)
 {
+    // TODO: Or we always cast (so pointer is not needed) or we find the place to be casted once, but constructor is not the place.
+    sensor_laser_ptr_ = (SensorLaser2D*)(upperNodePtr());
+
 	//check CaptureBase pointer is the appropriate one for this Processor
 //	assert( capture_laser_ptr_ = dynamic_cast<CaptureLaser2D*>(capture_ptr_) && "Invalid Capture Type pointer" );
     capture_laser_ptr_ = (CaptureLaser2D*)(capture_ptr_);
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index e1f86e5ddd40e5c739b80d826ab85ed26c5bbf90..b8991339c3f17f13949173363279d645f027caca 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -14,7 +14,13 @@
 
 //Wolf includes
 #include "state_block.h"
-#include "constraint_base.h"
+#include "../constraint_sparse.h"
+#include "../constraint_fix.h"
+#include "../constraint_gps_2D.h"
+#include "../constraint_gps_pseudorange.h"
+#include "../constraint_odom_2D.h"
+#include "../constraint_corner_2D.h"
+#include "../constraint_container.h"
 #include "sparse_utils.h"
 
 // wolf solver
diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp
index 5fa139d0889fe97b9a97c6a6001520240166a588..2184fce633992497b9766757bf7c46969877c8b2 100644
--- a/src/wolf_manager.cpp
+++ b/src/wolf_manager.cpp
@@ -40,8 +40,6 @@ WolfManager::WolfManager(const FrameStructure _frame_structure,
     //std::cout << " initial_covariance" << std::endl;
     current_frame_->addCapture(initial_covariance);
     //std::cout << " addCapture" << std::endl;
-    initial_covariance->process();
-    //std::cout << " processCapture" << std::endl;
 
     // Current robot frame
     createFrame(_prior, TimeStamp(0));