diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp
index 5cd2dc8e225754b0126b906cb5167428e83aadfd..21bdc72c5dc80f1349b9ef9f1c4e1d8c8b5b4f29 100644
--- a/src/examples/test_apriltag.cpp
+++ b/src/examples/test_apriltag.cpp
@@ -46,7 +46,7 @@ int main(int argc, char *argv[])
     // General execution options
     const bool APPLY_CONTRAST = false;
     const bool IMAGE_OUTPUT   = true;
-    const bool USEMAP         = true;
+    const bool USEMAP         = false;
 
 
     WOLF_INFO( "==================== processor apriltag test ======================" )
@@ -61,9 +61,9 @@ int main(int argc, char *argv[])
 
 
     WOLF_INFO( "====================    Configure Problem      ======================" )
-
-    SensorBasePtr sen       = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml");
-//    SensorBasePtr sen       = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml");
+    Eigen::Vector7s cam_extrinsics; cam_extrinsics << 0,0,0,  0,0,0,1;
+    SensorBasePtr sen       = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml");
+//    SensorBasePtr sen       = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml");
     SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen);
     ProcessorBasePtr prc    = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
 
diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp
index c91887d977b181497a9d6783be6af6caf0b7e96e..916ab0f9717c21f001888aa042e71698ef6c1b53 100644
--- a/src/processors/processor_tracker_landmark_apriltag.cpp
+++ b/src/processors/processor_tracker_landmark_apriltag.cpp
@@ -145,7 +145,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
         Eigen::Affine3ds M_ippe1, M_ippe2, M_april, M_PnP;
         Scalar rep_error1;
         Scalar rep_error2;
-        ippe_pose_estimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2);
+        ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2);
 
         // If not so sure about whether we have the right solution or not, do not create a feature
         // TODO: use parameters
@@ -157,7 +157,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
         // Slower than UMICH (iterative algorithm LM) but yield more precise results
         // Does not solve the ambiguity on the rotation however
         //////////////////
-        M_PnP = opencv_pose_estimation(det, cv_K_, tag_width);
+        M_PnP = opencvPoseEstimation(det, cv_K_, tag_width);
         //////////////////
 
         //////////////////
@@ -165,7 +165,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
         // Implementation found in the original Apriltag c implementation.
         // Analytical formula but high reprojection error for large angles
         //////////////////
-        // M_april = umich_pose_estimation(det, cv_K_, tag_width);
+        // M_april = umichPoseEstimation(det, cv_K_, tag_width);
         //////////////////
 
         WOLF_DEBUG("ippe1\n",   M_ippe1 .matrix());
@@ -175,23 +175,34 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
 
         c_M_t = M_ippe1;
 
+//        if (tag_id == 1){
+//            WOLF_INFO("TEST1: change solution of tag 1");
+//            c_M_t = M_ippe2;
+//        }
+
         // set the measured pose vector
         Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters
         Eigen::Vector7s pose;
         pose << translation, R2q(c_M_t.linear()).coeffs();
 
         // compute the covariance
-        Eigen::Matrix6s cov = getVarVec().asDiagonal() ;  // fixed dummy covariance
-//        Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_);  // Lie jacobians covariance
+//        Eigen::Matrix6s cov = getVarVec().asDiagonal() ;  // fixed dummy covariance
+        Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_);  // Lie jacobians covariance
 
         if (is_ambiguous){
-            WOLF_INFO("Ambiguity on estimated rotation is likely");
-            // Put a very high covariance on angles measurements
-            cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
+//            WOLF_INFO("Ambiguity on estimated rotation is likely");
+//            // Put a very high covariance on angles measurements
+//            cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
         }
-        WOLF_TRACE("cov diagonal: [", cov.diagonal(), "]");
+//        if (tag_id == 1){
+//            // Put a very high covariance on angles measurements
+//            WOLF_INFO("TEST2: Increase meas cov on tag 1");
+//            cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
+//        }
+
+//        WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]");
         // add to detected features list
-        detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov.inverse(), tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
+        detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, info, tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
 
         WOLF_TRACE("---------------------\n");
     }
@@ -201,20 +212,20 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
 
 // To compare with apriltag implementation
 // Returned translation is in tag units: needs to be multiplied by tag_width/2
-Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width){
+Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){
     // get corners from det
     std::vector<cv::Point2d> corners_pix(4);
     for (int i = 0; i < 4; i++)
     {
-        corners_pix[i].x = det->p[i][0];
-        corners_pix[i].y = det->p[i][1];
+        corners_pix[i].x = _det->p[i][0];
+        corners_pix[i].y = _det->p[i][1];
     }
 
     std::vector<cv::Point3d> obj_pts;
     // Same order as the 2D corners (anti clockwise, looking at the tag).
     // Looking at the tag, the reference frame is
     // X = Right, Y = Down, Z = Inside the plane
-    Scalar s = tag_width/2;
+    Scalar s = _tag_width/2;
     obj_pts.emplace_back(-s,  s, 0); // bottom left
     obj_pts.emplace_back( s,  s, 0); // bottom right
     obj_pts.emplace_back( s, -s, 0); // top right
@@ -225,7 +236,7 @@ Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(aprilt
     cv::Mat rvec, tvec;
     cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images
 
-    cv::solvePnP(obj_pts, corners_pix, K, dist_coeffs, rvec, tvec);
+    cv::solvePnP(obj_pts, corners_pix, _K, dist_coeffs, rvec, tvec);
 
     // Puts the result in a Eigen affine Transform
     cv::Matx33d rmat;
@@ -239,13 +250,13 @@ Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(aprilt
     return M;
 }
 
-Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umich_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width){
+Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){
     // To put in the usual camera frame with Z looking in front (RDF)
     Eigen::Affine3d c_M_ac;
     c_M_ac.matrix() = (Eigen::Vector4d() << 1, -1, -1, 1).finished().asDiagonal();
 
     Eigen::Affine3d M_april_raw;
-    matd_t *pose_matrix = homography_to_pose(det->H, -K(0,0), K(1,1), K(0,2), K(1,2)); // !! fx Negative sign advised by apriltag library commentary
+    matd_t *pose_matrix = homography_to_pose(_det->H, -_K(0,0), _K(1,1), _K(0,2), _K(1,2)); // !! fx Negative sign advised by apriltag library commentary
     // write it in Eigen form
     Eigen::Affine3d ac_M_t;
     for(int r=0; r<4; r++)
@@ -255,16 +266,16 @@ Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umich_pose_estimation(apriltag
     Eigen::Affine3d c_M_t = c_M_ac * ac_M_t;
 
     // Normalize transform
-    c_M_t.matrix().block(0,3,3,1) *= tag_width/2;
+    c_M_t.matrix().block(0,3,3,1) *= _tag_width/2;
 
     return c_M_t;
 }
 
-void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width,
-                            Eigen::Affine3d &M1,
-                            double &rep_error1,
-                            Eigen::Affine3d &M2,
-                            double &rep_error2){
+void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width,
+                            Eigen::Affine3d &_M1,
+                            double &_rep_error1,
+                            Eigen::Affine3d &_M2,
+                            double &_rep_error2){
 
     // camera coefficients
     cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images
@@ -273,14 +284,14 @@ void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t
     std::vector<cv::Point2d> corners_pix(4);
     for (int i = 0; i < 4; i++)
     {
-        corners_pix[i].x = det->p[i][0];
-        corners_pix[i].y = det->p[i][1];
+        corners_pix[i].x = _det->p[i][0];
+        corners_pix[i].y = _det->p[i][1];
     }
     std::vector<cv::Point3d> obj_pts;
     // Same order as the 2D corners (anti clockwise, looking at the tag).
     // Looking at the tag, the reference frame is
     // X = Right, Y = Down, Z = Inside the plane
-    Scalar s = tag_width/2;
+    Scalar s = _tag_width/2;
     obj_pts.emplace_back(-s,  s, 0); // bottom left
     obj_pts.emplace_back( s,  s, 0); // bottom right
     obj_pts.emplace_back( s, -s, 0); // top right
@@ -303,26 +314,26 @@ void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t
      * @param reprojErr2     Reprojection error of second solution
      */
     float err1, err2;
-    pose_solver.solveGeneric(obj_pts, corners_pix, K, dist_coeffs,
+    pose_solver.solveGeneric(obj_pts, corners_pix, _K, dist_coeffs,
                             rvec1, tvec1, err1,
                             rvec2, tvec2, err2);
-    rep_error1 = err1;
-    rep_error2 = err2;
+    _rep_error1 = err1;
+    _rep_error2 = err2;
 
     // Puts the result in a Eigen affine Transform
     cv::Matx33d rmat1;
     cv::Rodrigues(rvec1, rmat1);
     Eigen::Matrix3s R_eigen1; cv2eigen(rmat1, R_eigen1);
     Eigen::Vector3s t_eigen1; cv2eigen(tvec1, t_eigen1);
-    M1.matrix().block(0,0,3,3) = R_eigen1;
-    M1.matrix().block(0,3,3,1) = t_eigen1;
+    _M1.matrix().block(0,0,3,3) = R_eigen1;
+    _M1.matrix().block(0,3,3,1) = t_eigen1;
 
     cv::Matx33d rmat2;
     cv::Rodrigues(rvec2, rmat2);
     Eigen::Matrix3s R_eigen2; cv2eigen(rmat2, R_eigen2);
     Eigen::Vector3s t_eigen2; cv2eigen(tvec2, t_eigen2);
-    M2.matrix().block(0,0,3,3) = R_eigen2;
-    M2.matrix().block(0,3,3,1) = t_eigen2;
+    _M2.matrix().block(0,0,3,3) = R_eigen2;
+    _M2.matrix().block(0,3,3,1) = t_eigen2;
 }
 
 
diff --git a/src/processors/processor_tracker_landmark_apriltag.h b/src/processors/processor_tracker_landmark_apriltag.h
index a8ea31bcf1b1879ecb918fe8c2cbe717a5e6cc9c..888dadf714ae86227774a53902ec6edee035819b 100644
--- a/src/processors/processor_tracker_landmark_apriltag.h
+++ b/src/processors/processor_tracker_landmark_apriltag.h
@@ -115,20 +115,20 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark
         Eigen::Vector6s getVarVec();
         FeatureBaseList getIncomingDetections() const;
         FeatureBaseList getLastDetections() const;
-        Eigen::Affine3d opencv_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width);
-        Eigen::Affine3d umich_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width);
-        void ippe_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width,
-                                    Eigen::Affine3d &M1,
-                                    double &rep_error1,
-                                    Eigen::Affine3d &M2,
-                                    double &rep_error2);
-        Eigen::Matrix6s computeInformation(Eigen::Vector3s const &t, Eigen::Matrix3s const &R, Eigen::Matrix3s const &K, double const &tag_width, double const &sig_q);
-        void pinholeHomogeneous(Eigen::Matrix3s const & K, Eigen::Vector3s const & t,
-                                Eigen::Matrix3s const & R, Eigen::Vector3s const & p,
-                                Eigen::Vector3s &h, Eigen::Matrix3s &J_h_T, Eigen::Matrix3s &J_h_R);
-        void cornersToPose(const std::vector<cv::Point2d> &img_pts,
-                           const std::vector<Scalar> &k_vec,
-                           Eigen::Affine3ds &M);
+        Eigen::Affine3d opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width);
+        Eigen::Affine3d umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width);
+        void ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width,
+                                    Eigen::Affine3d &_M1,
+                                    double &_rep_error1,
+                                    Eigen::Affine3d &_M2,
+                                    double &_rep_error2);
+        Eigen::Matrix6s computeInformation(Eigen::Vector3s const &_t, Eigen::Matrix3s const &_R, Eigen::Matrix3s const &_K, double const &_tag_width, double const &_sig_q);
+        void pinholeHomogeneous(Eigen::Matrix3s const & _K, Eigen::Vector3s const & _t,
+                                Eigen::Matrix3s const & _R, Eigen::Vector3s const & _p,
+                                Eigen::Vector3s &_h, Eigen::Matrix3s &_J_h_T, Eigen::Matrix3s &_J_h_R);
+        void cornersToPose(const std::vector<cv::Point2d> &_img_pts,
+                           const std::vector<Scalar> &_k_vec,
+                           Eigen::Affine3ds &_M);
 
     protected:
         void advanceDerived();