diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 58aab197f8174e67b584742198ea2ce8fd88e1cb..87c9d9c3837b9a5c28c366e9bb84b9d358180687 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -88,8 +88,6 @@ license_header:
 build_and_test:bionic:
   stage: build_and_test
   image: labrobotica/wolf_deps:18.04
-  except:
-    - master
   script:
     - *build_and_test_definition
 
@@ -97,64 +95,153 @@ build_and_test:bionic:
 build_and_test:focal:
   stage: build_and_test
   image: labrobotica/wolf_deps:20.04
-  except:
-    - master
   script:
     - *build_and_test_definition
 
-############ DEPLOY PLUGINS ############
+############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
 deploy_imu:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+    branch: $WOLF_IMU_BRANCH
     strategy: depend
 
 deploy_gnss:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+    branch: $WOLF_GNSS_BRANCH
     strategy: depend
 
 deploy_vision:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+    branch: $WOLF_VISION_BRANCH
     strategy: depend
 
 deploy_laser:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+    branch: $WOLF_LASER_BRANCH
     strategy: depend
 
 deploy_apriltag:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+    branch: $WOLF_APRILTAG_BRANCH
     strategy: depend
 
 deploy_bodydynamics:
   stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+    branch: $WOLF_BODYDYNAMICS_BRANCH
+    strategy: depend
+
+############ DEPLOY PLUGINS FOR main ############
+deploy_imu_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+    branch: main
+    strategy: depend
+
+deploy_gnss_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+    branch: main
+    strategy: depend
+
+deploy_vision_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+    branch: main
+    strategy: depend
+
+deploy_laser_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+    branch: main
+    strategy: depend
+
+deploy_apriltag_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+    branch: main
+    strategy: depend
+
+deploy_bodydynamics_main:
+  stage: deploy_plugins
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    DEPLOY_CI_ROS: "false"
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+    branch: main
     strategy: depend
 
 ############ WAIT FOR PLUGINS ############
@@ -163,12 +250,11 @@ final_all_plugins:
   script:
     - echo "ALL PLUGINS PIPELINES SUCCEED!!!"
 
-############ DEPLOY WOLF_ROS_NODE ############
+############ DEPLOY WOLF_ROS_NODE EXCEPT FOR main ############
 deploy_wolf_ros_node:
   stage: deploy_ros
-  only:
-    variables:
-      - $DEPLOY_CI_ROS == "true"
+  rules: 
+    - if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
     WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -177,7 +263,6 @@ deploy_wolf_ros_node:
     WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
     WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH
     WOLF_BODYDYNAMICS_BRANCH: $WOLF_BODYDYNAMICS_BRANCH
-    WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH
     WOLF_ROS_IMU_BRANCH: $WOLF_ROS_IMU_BRANCH
     WOLF_ROS_GNSS_BRANCH: $WOLF_ROS_GNSS_BRANCH
     WOLF_ROS_LASER_BRANCH: $WOLF_ROS_LASER_BRANCH
@@ -186,11 +271,34 @@ deploy_wolf_ros_node:
     WOLF_ROS_BODYDYNAMICS_BRANCH: $WOLF_ROS_BODYDYNAMICS_BRANCH
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
+    branch: $WOLF_ROS_CORE_BRANCH
+
+############ DEPLOY WOLF_ROS_NODE FOR main ############
+deploy_wolf_ros_node_main:
+  stage: deploy_ros
+  rules: 
+    - if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+  variables:
+    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_IMU_BRANCH: main
+    WOLF_GNSS_BRANCH: main
+    WOLF_LASER_BRANCH: main
+    WOLF_VISION_BRANCH: main
+    WOLF_APRILTAG_BRANCH: main
+    WOLF_BODYDYNAMICS_BRANCH: main
+    WOLF_ROS_IMU_BRANCH: main
+    WOLF_ROS_GNSS_BRANCH: main
+    WOLF_ROS_LASER_BRANCH: main
+    WOLF_ROS_VISION_BRANCH: main
+    WOLF_ROS_APRILTAG_BRANCH: main
+    WOLF_ROS_BODYDYNAMICS_BRANCH: main
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
+    branch: main
 
 no_deploy_wolf_ros_node:
   stage: deploy_ros
   script: 
-    - echo "NOT deploying CI for wolf_ros_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS"
-  except:
-    variables:
-      - $DEPLOY_CI_ROS == "true"
+    - echo "NOT deploying CI for wolf_ros_node, since DEPLOY_CI_ROS is $DEPLOY_CI_ROS (not true)"
+  rules: 
+    - if: $DEPLOY_CI_ROS != "true"
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index d41a1a273a7aa50313ebcab126b0f9f38997406c..39492e1975dbe66cabf86ac9c751d3bd72584deb 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -138,8 +138,8 @@ int main()
     SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
 
     // recover sensor pointers and other stuff for later use (access by sensor name)
-    SensorBasePtr sensor_odo    = problem->getSensor("sen odom");
-    SensorBasePtr sensor_rb     = problem->getSensor("sen rb");
+    SensorBasePtr sensor_odo    = problem->findSensor("sen odom");
+    SensorBasePtr sensor_rb     = problem->findSensor("sen rb");
 
     // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
     TimeStamp     t(0.0);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index ce1a393f6f1a21e015d66c7a336f889c9d8be218..69e55ea8556dc03a323e0aa62e90d125b93015a0 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -153,8 +153,14 @@ class Problem : public std::enable_shared_from_this<Problem>
         /** \brief get a sensor pointer by its name
          * \param _sensor_name The sensor name, as it was installed with installSensor()
          */
-        SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
-        SensorBasePtr getSensor(const std::string& _sensor_name);
+        SensorBaseConstPtr findSensor(const std::string& _sensor_name) const;
+        SensorBasePtr findSensor(const std::string& _sensor_name);
+
+        /** \brief get a processor pointer by its name
+         * \param _processor_name The processor name, as it was installed with installProcessor()
+         */
+        ProcessorBaseConstPtr findProcessor(const std::string& _processor_name) const;
+        ProcessorBasePtr findProcessor(const std::string& _processor_name);
 
         /** \brief Factory method to install (create, and add to sensor) processors only from its properties
          *
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index f6974afbaf16c7bc42e1df43f46cd9c169494203..f609dc8efefbbb6fb7c74b7dac46ec8f052b0403 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -47,10 +47,10 @@ using std::shared_ptr;
 
 typedef map<TimeStamp, FeatureBasePtr>                                Track;
 typedef map<TimeStamp, FeatureBaseConstPtr>                           TrackConst;
-typedef map<size_t, FeatureBasePtr >                                  Snapshot;
-typedef map<size_t, FeatureBaseConstPtr >                             SnapshotConst;
-typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> >            TrackMatches; // matched feature pairs indexed by track_id
-typedef map<size_t, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> >  TrackMatchesConst; // matched feature pairs indexed by track_id
+typedef map<SizeStd, FeatureBasePtr >                                  Snapshot;
+typedef map<SizeStd, FeatureBaseConstPtr >                             SnapshotConst;
+typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> >            TrackMatches; // matched feature pairs indexed by track_id
+typedef map<SizeStd, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> >  TrackMatchesConst; // matched feature pairs indexed by track_id
 
 /** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
  * This class implements the following data structure:
@@ -76,7 +76,7 @@ typedef map<size_t, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> >  TrackMatch
  *
  *      Snapshot: capture-wise: a set of features tracked in a single Capture, indexed by track id:
  *
- *                              map<size_t track_id, FeatureBasePtr f>
+ *                              map<SizeStd track_id, FeatureBasePtr f>
  *
  * The class makes sure that both accesses are consistent each time some addition or removal of features is performed.
  *
@@ -129,12 +129,14 @@ class TrackMatrix
         CaptureBaseConstPtr     firstCapture(const SizeStd& _track_id) const;
         CaptureBasePtr          firstCapture(const SizeStd& _track_id);
 
+        list<SizeStd>    trackIds() const;
+
         // tracks across captures that belong to keyframe
-//        SizeStd         numKeyframeTracks();
-        TrackConst              trackAtKeyframes(size_t _track_id) const;
-        Track                   trackAtKeyframes(size_t _track_id);
-//        bool            markKeyframe(CaptureBasePtr _capture);
-//        bool            unmarkKeyframe(CaptureBasePtr _capture);
+        TrackConst              trackAtKeyframes(const SizeStd& _track_id) const;
+        Track                   trackAtKeyframes(const SizeStd& _track_id);
+
+        const map<SizeStd, Track>& getTracks() {return tracks_;}
+        const map<CaptureBasePtr, Snapshot >& getSnapshots() {return snapshots_;}
 
     private:
 
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 4200f81f7e3faac814a45bea8995a13436b6b7c3..f22772908c411fba8a096fb764af40fe6b39316e 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -51,12 +51,19 @@ class StateQuaternion : public StateBlock
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
         StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
 {
+    // TODO: remove these lines after issue #381 is addressed
+    assert(isValid(1e-5)  && "Quaternion unit norm is worse than 1e-5 tolerance!");
+    state_.normalize();
 }
 
 inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
         StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
 {
-    assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
+    assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
+
+    // TODO: remove these lines after issue #381 is addressed
+    assert(isValid(1e-5)  && "Quaternion unit norm is worse than 1e-5 tolerance!");
+    state_.normalize();
 }
 
 inline StateQuaternion::StateQuaternion(bool _fixed) :
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 2cec6eceefadc381c4ce7ba1dd63e0ec7ac0c2bf..fcef79178934508f840a2a2d51b918b85827c1af 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -203,7 +203,7 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
 }
 void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c  " : "");
+    _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac  " : "");
     if (_constr_by)
     {
         _stream << "  <--\t";
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 74949c604240639ac78bb72145a2b91e13802959..aa07e14a3cf1e3e4c2e55f93d9f71cf88b038807 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -297,8 +297,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                            const std::string& _corresponding_sensor_name, //
                                            const std::string& _params_filename)
 {
-    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
-    WOLF_INFO("installProcessor _corresponding_sensor_name", _corresponding_sensor_name, " found sensor name ", sen_ptr->getName())
+    SensorBasePtr sen_ptr = findSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
         throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
     if (_params_filename == "")
@@ -316,7 +315,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                            const std::string& _corresponding_sensor_name, //
                                            const ParamsServer& _server)
 {
-    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
+    SensorBasePtr sen_ptr = findSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
         throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
 
@@ -333,16 +332,34 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     return prc_ptr;
 }
 
-SensorBaseConstPtr Problem::getSensor(const std::string& _sensor_name) const
+SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
 {
     return getHardware()->getSensor(_sensor_name);
 }
 
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
+SensorBasePtr Problem::findSensor(const std::string& _sensor_name)
 {
     return getHardware()->getSensor(_sensor_name);
 }
 
+ProcessorBaseConstPtr Problem::findProcessor(const std::string& _processor_name) const
+{
+    for (const auto& sensor : getHardware()->getSensorList())
+        for (const auto& processor : sensor->getProcessorList())
+            if (processor->getName() == _processor_name)
+                return processor;
+    return nullptr;
+}
+
+ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
+{
+    for (const auto& sensor : getHardware()->getSensorList())
+        for (const auto& processor : sensor->getProcessorList())
+            if (processor->getName() == _processor_name)
+                return processor;
+    return nullptr;
+}
+
 FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
                                    const StateStructure& _frame_structure, //
                                    const SizeEigen _dim, //
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index c707c37eadae74f631b392317c286fe057713f76..512d941b7669c13e22a87aee23c5a8f84ed44329 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -87,7 +87,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
-            // TrackerLandmark: If we have given a map, all landmarks in the map are know. Process them.
+            // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
             processKnown();
 
             // Update pointers
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index ff2fceb341742d2ddce1a8ce9bc8abdfb1f1a381..de101750edfe66eec72fae0c3be295164e8b7d45 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -31,7 +31,7 @@
 namespace wolf
 {
 
-size_t TrackMatrix::track_id_count_ = 0;
+SizeStd TrackMatrix::track_id_count_ = 0;
 
 TrackMatrix::TrackMatrix()
 {
@@ -170,12 +170,12 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
     }
 }
 
-size_t TrackMatrix::numTracks() const
+SizeStd TrackMatrix::numTracks() const
 {
     return tracks_.size();
 }
 
-size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
+SizeStd TrackMatrix::trackSize(const SizeStd& _track_id) const
 {
     return track(_track_id).size();
 }
@@ -331,7 +331,7 @@ CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
     return firstFeature(_track_id)->getCapture();
 }
 
-TrackConst TrackMatrix::trackAtKeyframes(size_t _track_id) const
+TrackConst TrackMatrix::trackAtKeyframes(const SizeStd& _track_id) const
 {
     // We assemble a track_kf on the fly by checking each capture's frame.
     if (tracks_.count(_track_id))
@@ -350,7 +350,7 @@ TrackConst TrackMatrix::trackAtKeyframes(size_t _track_id) const
         return TrackConst();
 }
 
-Track TrackMatrix::trackAtKeyframes(size_t _track_id)
+Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
 {
     // We assemble a track_kf on the fly by checking each capture's frame.
     if (tracks_.count(_track_id))
@@ -369,4 +369,14 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id)
         return Track();
 }
 
+list<SizeStd> TrackMatrix::trackIds() const
+{
+    list<SizeStd> track_ids;
+    for (auto track : tracks_)
+    {
+        track_ids.push_back(track.first);
+    }
+    return track_ids;
+}
+
 }
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 33b358362e0247c570ccd6ae48d64bf7a89ef31b..6332a69928a558cfdede9125a73fd7ab7f835338 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -108,7 +108,7 @@ TEST(FactoryStateBlock, creator_StateBlock)
 
 TEST(FactoryStateBlock, creator_Quaternion)
 {
-    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4).normalized(), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -144,7 +144,7 @@ TEST(FactoryStateBlock, creator_H)
 
 TEST(FactoryStateBlock, creator_O_is_quaternion)
 {
-    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4).normalized(), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index fa4c1807c394cc4f24ed440ff1d4e79df9392829..522246910cc6bd3944f6924a832fbf028cb2b75c 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -101,7 +101,7 @@ TEST(Problem, Installers)
 {
     std::string wolf_root = _WOLF_ROOT_DIR;
     ProblemPtr P = Problem::create("PO", 3);
-    Eigen::Vector7d xs;
+    Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
 
     SensorBasePtr    S = P->installSensor   ("SensorOdom3d", "odometer",        xs,         wolf_root + "/test/yaml/sensor_odom_3d.yaml");
 
@@ -184,7 +184,8 @@ TEST(Problem, SetOrigin_PO_3d)
 {
     ProblemPtr P = Problem::create("PO", 3);
     TimeStamp       t0(0);
-    Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+    Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
+    vec7.tail(4).normalize();
     VectorComposite x0(vec7, "PO", {3,4});
     // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
     Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
@@ -242,9 +243,13 @@ TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  VectorXd(3)  );
-    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  VectorXd(7)  );
-    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  VectorXd(10) );
+    Vector3d xpo2;
+    Vector7d xpo3; xpo3 << 1,2,3,.5,.5,.5,.5;
+    VectorXd xpov3(10); xpov3 << 1,2,3,.5,.5,.5,.5,1,2,3;
+
+    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  xpo2  );
+    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  xpo3  );
+    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  xpov3 );
 
 
     // check that all frames are effectively in the trajectory
@@ -324,7 +329,8 @@ TEST(Problem, Covariances)
     std::string wolf_root = _WOLF_ROOT_DIR;
 
     ProblemPtr P = Problem::create("PO", 3);
-    Eigen::Vector7d xs3d;
+    Eigen::Vector7d xs3d; xs3d.setRandom(); xs3d.tail(4).normalize();
+
     Eigen::Vector3d xs2d;
 
     SensorBasePtr    Sm = P->installSensor   ("SensorOdom3d", "odometer",       xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 4294d2ab663a7d7e171c3b663a6cf2575c6c5049..d37c700a7daf6a45862a4d8b830b8b8e67767282 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -38,7 +38,7 @@ TEST(Pose, constructor)
 {
 
     auto intr = std::make_shared<ParamsSensorPose>();
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen = std::make_shared<SensorPose>(extr, intr);
 
@@ -54,7 +54,7 @@ TEST(Pose, getParams)
     intr->std_p = 2;
     intr->std_o = 3;
 
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen = std::make_shared<SensorPose>(extr, intr);
 
@@ -70,7 +70,7 @@ TEST(Pose, create)
     intr->std_p = 2;
     intr->std_o = 3;
 
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen_base = SensorPose::create("name", extr, intr);
 
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 914b8a7c3a989c52f86abbb2b8274732acf3aa2f..3d3ded934d762d21ba081779155f320db1b5327f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -114,7 +114,8 @@ TEST(TreeManager, keyFrameCallback)
 
     ASSERT_EQ(GM->n_KF_, 0);
 
-    auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
+    Vector7d x; x << 1,2,3,0,0,0,1;
+    auto F0 = P->emplaceFrame(0, "PO", 3, x );
     P->keyFrameCallback(F0, nullptr);
 
     ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index e394a7c59279bb4d0e52c94b7eef479406ce5797..448fc0d93d27e1db1ee76c6ae46223be81a3c505 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -1050,11 +1050,11 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
     TimeStamp t(0.0);
     double dt = 1;
     CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t,
-                                                               problem->getSensor("odom"),
+                                                               problem->findSensor("odom"),
                                                                data,
                                                                data_cov);
     CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t,
-                                                                  problem_bl->getSensor("odom"),
+                                                                  problem_bl->findSensor("odom"),
                                                                   data,
                                                                   data_cov);