diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 01e0b7782b8dae156db2966d162192eef8859015..568bd9472541c0f7b559901cc506766d3fc53c79 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -105,7 +105,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         for (auto sb : fr_ptr->getStateBlockVec())
           if (sb)
             for(auto sb2 : fr_ptr->getStateBlockVec())
-              if (sb)
+              if (sb2)
               {
                 state_block_pairs.emplace_back(sb, sb2);
                 double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 088e9effe597908d556b1e8a8f8ea48226fe4822..97cc1744c92f602ff902b5edc63561b104f4eaba 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
 FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
-            state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed.
+            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
             is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(NON_KEY_FRAME),
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index 416eca9058efde6d43073ba1e94c4af493166e65..66813522eee1ce7da1290b43b0b2eb194c71e095 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -27,7 +27,7 @@
 #include "ceres_wrapper/ceres_manager.h"
 
 int main()
-{
+ {
     /*
      * ============= PROBLEM DEFINITION ==================
      *
@@ -117,12 +117,12 @@ int main()
 
     // sensor odometer 2D
     IntrinsicsOdom2DPtr intrinsics_odo      = std::make_shared<IntrinsicsOdom2D>();
-    intrinsics_odo->k_disp_to_disp          = 0.1;
-    intrinsics_odo->k_rot_to_rot            = 0.1;
     SensorBasePtr sensor_odo                = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
 
     // processor odometer 2D
     ProcessorParamsOdom2DPtr params_odo     = std::make_shared<ProcessorParamsOdom2D>();
+    params_odo->voting_active               = true;
+    params_odo->time_tolerance              = 0.1;
     params_odo->max_time_span               = 999;
     params_odo->dist_traveled               = 0.95; // Will make KFs automatically every 1m displacement
     params_odo->angle_turned                = 999;
@@ -133,14 +133,16 @@ int main()
 
     // sensor Range and Bearing
     IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
-    intrinsics_rb->noise_bearing_degrees_std = 1.0;
     intrinsics_rb->noise_range_metres_std   = 0.1;
+    intrinsics_rb->noise_bearing_degrees_std = 1.0;
     SensorBasePtr sensor_rb                 = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
+    sensor_rb->fix();
     // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only)
-    // sensor_rb->getOPtr()->unfix();
+    sensor_rb->getOPtr()->unfix();
 
     // processor Range and Bearing
     ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
+    params_rb->voting_active                = false;
     params_rb->time_tolerance               = 0.01;
     ProcessorBasePtr processor_rb           = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
 
@@ -164,10 +166,10 @@ int main()
     // STEP 1 --------------------------------------------------------------
 
     // initialize
-    TimeStamp   t(0.0);
+    TimeStamp   t(0.0);                     // t : 0.0
     Vector3s    x(0,0,0);
     Matrix3s    P = Matrix3s::Identity() * 0.1;
-    problem->setPrior(x, P, t, 0.5);             // KF1
+    problem->setPrior(x, P, t, 0.5);             // KF1 : (0,0,0)
 
     // observe lmks
     ids.resize(1); ranges.resize(1); bearings.resize(1);
@@ -175,14 +177,14 @@ int main()
     ranges      << 1.0;                     // see drawing
     bearings    << M_PI/2;
     CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);
+    sensor_rb   ->process(cap_rb);          // L1 : (1,2)
 
     // STEP 2 --------------------------------------------------------------
-    t += 1.0;
+    t += 1.0;                     // t : 1.0
 
     // motion
     CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
-    sensor_odo  ->process(cap_motion);      // KF2
+    sensor_odo  ->process(cap_motion);      // KF2 : (1,0,0)
 
     // observe lmks
     ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -190,14 +192,14 @@ int main()
     ranges      << sqrt(2.0), 1.0;          // see drawing
     bearings    << 3*M_PI/4, M_PI/2;
     cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);
+    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2)
 
     // STEP 3 --------------------------------------------------------------
-    t += 1.0;
+    t += 1.0;                     // t : 2.0
 
     // motion
     cap_motion  ->setTimeStamp(t);
-    sensor_odo  ->process(cap_motion);      // KF3
+    sensor_odo  ->process(cap_motion);      // KF3 : (2,0,0)
 
     // observe lmks
     ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -205,9 +207,7 @@ int main()
     ranges      << sqrt(2.0), 1.0;          // see drawing
     bearings    << 3*M_PI/4, M_PI/2;
     cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);
-
-    problem->print(4,1,1,1);
+    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
 
 
     // SOLVE ================================================================
@@ -216,25 +216,30 @@ int main()
     WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
     std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE(report);                     // should show a very low iteration number (possibly 1)
-    problem->print(4,1,1,1);
+    problem->print(4,0,1,1);
 
     // PERTURB initial guess
     WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
     for (auto sen : problem->getHardwarePtr()->getSensorList())
         for (auto sb : sen->getStateBlockVec())
             if (sb && !sb->isFixed())
-                sb->setState(VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
     for (auto kf : problem->getTrajectoryPtr()->getFrameList())
-        kf->setState(Vector3s::Random() * 0.5);                 // We perturb A LOT !
+        if (kf->isKey())
+            for (auto sb : kf->getStateBlockVec())
+                if (sb && !sb->isFixed())
+                    sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
     for (auto lmk : problem->getMapPtr()->getLandmarkList())
-        lmk->getPPtr()->setState(Vector2s::Random());           // We perturb A LOT !
-    problem->print(4,1,1,1);
+        for (auto sb : lmk->getStateBlockVec())
+            if (sb && !sb->isFixed())
+                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
+    problem->print(2,0,1,1);
 
     // SOLVE again
     WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
     report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE(report);                     // should show a very high iteration number (more than 10, or than 100!)
-    problem->print(4,1,1,1);
+    problem->print(1,1,1,1);
 
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")