diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 4565017604fc766e0e797371c16e38e508fbe49f..6f5980d7ced970ffae4aae48459b800545a4158b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -6,6 +6,25 @@ stages:
   - deploy_ros
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_IMU_BRANCH
+  - echo $WOLF_GNSS_BRANCH
+  - echo $WOLF_LASER_BRANCH
+  - echo $WOLF_VISION_BRANCH
+  - echo $WOLF_APRILTAG_BRANCH
+  - echo $WOLF_BODYDYNAMICS_BRANCH
+  - echo $GNSSUTILS_BRANCH
+  - echo $LASERSCANUTILS_BRANCH
+  - echo $DEPLOY_CI_ROS
+  - echo $WOLF_ROS_IMU_BRANCH
+  - echo $WOLF_ROS_GNSS_BRANCH
+  - echo $WOLF_ROS_LASER_BRANCH
+  - echo $WOLF_ROS_VISION_BRANCH
+  - echo $WOLF_ROS_APRILTAG_BRANCH
+  - echo $WOLF_ROS_BODYDYNAMICS_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -80,6 +99,7 @@ license_header:
   stage: license
   image: labrobotica/wolf_deps:20.04
   before_script:  
+    - *print_variables_definition
     - *preliminaries_definition
   script: 
     - *license_header_definition
@@ -89,6 +109,7 @@ build_and_test:bionic:
   stage: build_and_test
   image: labrobotica/wolf_deps:18.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ UBUNTU 20.04 TESTS ############
@@ -96,6 +117,7 @@ build_and_test:focal:
   stage: build_and_test
   image: labrobotica/wolf_deps:20.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
@@ -117,6 +139,7 @@ deploy_gnss:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
     GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
   trigger: 
@@ -142,6 +165,7 @@ deploy_laser:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -154,6 +178,7 @@ deploy_apriltag:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -166,6 +191,7 @@ deploy_bodydynamics:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
@@ -178,7 +204,7 @@ deploy_imu_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
@@ -190,7 +216,8 @@ deploy_gnss_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
@@ -202,7 +229,7 @@ deploy_vision_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
@@ -214,7 +241,8 @@ deploy_laser_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -226,7 +254,8 @@ deploy_apriltag_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_VISION_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -238,7 +267,8 @@ deploy_bodydynamics_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_IMU_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 69e55ea8556dc03a323e0aa62e90d125b93015a0..9dfb7cae3e62adac06662e9414c75764045148e2 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -37,13 +37,11 @@ struct ParamsProcessorBase;
 
 //wolf includes
 #include "core/common/wolf.h"
+#include "core/utils/params_server.h"
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
-#include "core/utils/params_server.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
-#include <core/processor/motion_provider.h>
 #include "core/state_block/state_composite.h"
+#include "core/processor/motion_provider.h"
 
 // std includes
 #include <mutex>
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 88b5f411dc6b718cab9f5e1a2a70071abffa4f8e..66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
 // wolf
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
-//#include "core/common/factory.h"
+#include "core/common/factory.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 75f23c36e4d86da038bf21c7fc17577c05a637d1..12ec2c4f0b14c5800be2fa617902c466bca7e6cf 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -21,40 +21,18 @@
 //--------LICENSE_END--------
 // wolf includes
 #include "core/problem/problem.h"
-#include "core/hardware/hardware_base.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
 #include "core/map/factory_map.h"
-#include "core/sensor/sensor_base.h"
 #include "core/sensor/factory_sensor.h"
 #include "core/processor/factory_processor.h"
-#include "core/processor/processor_motion.h"
-#include "core/processor/processor_tracker.h"
-#include "core/capture/capture_pose.h"
 #include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
-#include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
 #include "core/tree_manager/factory_tree_manager.h"
 #include "core/tree_manager/tree_manager_base.h"
-
-#include "core/utils/params_server.h"
 #include "core/utils/loader.h"
-#include "core/utils/check_log.h"
-#include "core/math/covariance.h"
-#include "core/state_block/factory_state_block.h"
-
-// C++ includes
-#include <algorithm>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <unordered_set>
+
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 512d941b7669c13e22a87aee23c5a8f84ed44329..3d3794e75b1a87461832695c26341b8f40fc11f5 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -216,10 +216,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->link(getProblem());
 
-                // // make F; append incoming to new F
-                // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
-                // incoming_ptr_->link(frm);
-
                 // Establish factors
                 establishFactors();
 
@@ -232,7 +228,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // make F; append incoming to new F
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_   ->link(frame);
                 origin_ptr_     = last_ptr_;
                 last_ptr_       = incoming_ptr_;
@@ -250,7 +246,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Replace last frame for a new one in incoming
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_->link(frame);
                 last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
 
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index a8dd748ea29aa2a63eb72003b5c37465d62c3d17..771d19b7414b2a4a7342c8afea09d08d19f1b750 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/processor/processor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index 68a44ff61adaabb99cb16cc1bc1c72017de6d813..dcb0cf0c10e428a6dfce876a1765e8c0d245e1a2 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -33,6 +33,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index 02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85..0cb47d441d9b8a0786a9fcf8d00952a682072c4a 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index 102729bd91dcfca4f51a567f47d790ce1de6c6b7..bfbe151c43315c21e5f636e2632fa9ceb614c9d4 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_pose.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 8283b62b8ef35e591dad8533256a517ad65f7718..c37bed03e30da209b9b644d49b92011afeeb5f93 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -626,7 +626,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
     // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
     int N = 6;
-    Vector3d data;
+    Vector2d data;
     Matrix2d data_cov; data_cov.setIdentity();
 
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index a5e0beabb5447222a27bd02138144997d3389a00..23c0afdf5a4bcf4ef2586d7e63b692ae4c953906 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
 
     MatrixXd J_D(7,7), J_d(7,7);
 
-    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
+    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
     WOLF_DEBUG("J_D:\n ", J_D);
     WOLF_DEBUG("J_d:\n ", J_d);