diff --git a/ext/as-integration b/ext/as-integration
deleted file mode 160000
index 9147a3e3b..000000000
--- a/ext/as-integration
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 9147a3e3bb1693d62402556543fd59e69daf2d99
diff --git a/ext/hesai-lidar b/ext/hesai-lidar
deleted file mode 160000
index 0329ec593..000000000
--- a/ext/hesai-lidar
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 0329ec593891a4bcddcccb903972eada54a68594
diff --git a/ext/kiss-icp b/ext/kiss-icp
deleted file mode 160000
index 0fb532c2d..000000000
--- a/ext/kiss-icp
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 0fb532c2d5c69b2bd992e4710d9993eaf8235512
diff --git a/ext/xsens-imu b/ext/xsens-imu
deleted file mode 160000
index 5f0c22209..000000000
--- a/ext/xsens-imu
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 5f0c222098ce759ba3a8904b528bd22a457b7897
diff --git a/src/launcher/launch/se_planning_control.launch.py b/src/launcher/launch/se_planning_control.launch.py
index 1f76765dc..515b813bd 100644
--- a/src/launcher/launch/se_planning_control.launch.py
+++ b/src/launcher/launch/se_planning_control.launch.py
@@ -8,7 +8,7 @@ def generate_launch_description():
     se_launch_description = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("ekf_state_est"), "launch", "state_estimation.launch.py"]
+                [FindPackageShare("slam"), "launch", "slam.launch.py"]
             )
         ),
     )
diff --git a/src/perception_sensor_lib/CMakeLists.txt b/src/perception_sensor_lib/CMakeLists.txt
index 9e31f0b7f..79c1ec9a2 100644
--- a/src/perception_sensor_lib/CMakeLists.txt
+++ b/src/perception_sensor_lib/CMakeLists.txt
@@ -35,6 +35,7 @@ set(IMPLEMENTATION_FILES
   src/data_association/nearest_neighbour.cpp
   src/data_association/nearest_neighbor.cpp
   src/observation_model/base_observation_model.cpp
+  src/loop_closure/lap_counter.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED ${IMPLEMENTATION_FILES})
diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp
new file mode 100644
index 000000000..cc3e50a8d
--- /dev/null
+++ b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp
@@ -0,0 +1,42 @@
+#pragma once
+
+#include "loop_closure.hpp"
+
+/**
+ * @brief Implementation of LoopClosure that detects when the robot returns near 
+ *        the origin and re‑observes any of the first X cones from the map.
+ */
+class LapCounter : public LoopClosure {
+public:
+  /**
+   * @param threshold_dist   distance (m) around origin to trigger closure
+   * @param first_x_cones    consider a loop if you see any of these first X cones
+   * @param border_width     distance to givve to start searching for loop closure again
+   * @param minimum_confidence minimum number of observations to confirm loop closure
+   */
+  LapCounter(double threshold_dist, int first_x_cones, int border_width, int minimum_confidence);
+
+  /**
+   * @brief Detects loop closure when returning to origin and seeing initial cones
+   * @param current_pose     your latest pose in world frame
+   * @param map_cones        full map of cones (in insertion order)
+   * @param associations     one entry per observation:
+   *                         >=3 → matched map_cones[(j-3)/2]
+   *                         -1 → new landmark, -2 → no match
+   * @param observations     raw observations (unused in this implementation)
+   * @return                 {true,0.0} once you re‑see any of map_cones[0..X-1]
+   */
+  Result detect(
+    const Eigen::Vector3d& current_pose,
+    const Eigen::VectorXi& map_cones,
+    const Eigen::VectorXi& associations,
+    const Eigen::VectorXd& observations) const override;
+
+private:
+  double threshold_dist_;
+  int first_x_cones_;
+  int border_width_;
+  int minimum_confidence_;
+  mutable int confidence_ {0};
+  mutable bool searching_{false};
+};
\ No newline at end of file
diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp
new file mode 100644
index 000000000..e31fb04df
--- /dev/null
+++ b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <common_lib/structures/cone.hpp>
+#include <vector>
+#include <utility>
+
+/**
+ * @brief Interface for detecting loop closures
+ */
+class LoopClosure {
+public:
+
+  /**
+   * @brief Result of loop closure detection
+   * @param detected    true if a loop closure was detected
+   * @param offset      offset in meters of the map deviation
+   */
+  struct Result{
+    bool detected;
+    double offset;
+  };
+
+  virtual ~LoopClosure() = default;
+
+  /**
+   * @brief Call every time you have new observations.
+   * @param current_pose     your latest pose in world frame
+   * @param map_cones        full map of cones (in insertion order)
+   * @param associations     one entry per observation:
+   *                         >=3 → matched map_cones[(j-3)/2]
+   *                         -1 → new landmark, -2 → no match
+   * @param observations     raw observations
+   * @return                 result indicating if loop closure was detected
+   */
+  virtual Result detect(
+    const Eigen::Vector3d& current_pose,
+    const Eigen::VectorXi& map_cones,
+    const Eigen::VectorXi& associations,
+    const Eigen::VectorXd& observations) const = 0;
+};
\ No newline at end of file
diff --git a/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp b/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp
new file mode 100644
index 000000000..fbf1ea030
--- /dev/null
+++ b/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp
@@ -0,0 +1,61 @@
+#include "perception_sensor_lib/loop_closure/lap_counter.hpp"
+#include <cmath>
+
+LapCounter::LapCounter(double threshold_dist, int first_x_cones, int border_width, int minimum_confidence)
+  : threshold_dist_(threshold_dist),
+    first_x_cones_(first_x_cones),
+    border_width_(border_width),
+    minimum_confidence_(minimum_confidence)
+{}
+
+LoopClosure::Result LapCounter::detect(
+  const Eigen::Vector3d& current_pose,
+  const Eigen::VectorXi& map_cones,
+  const Eigen::VectorXi& associations,
+  const Eigen::VectorXd& observations) const
+{
+  double dx = current_pose.x();
+  double dy = current_pose.y();
+  double dist = std::sqrt(dx * dx + dy * dy);
+
+  // add a border to give some space until we start searching again
+  if (!searching_) {
+    if (dist > threshold_dist_ + border_width_) {
+      searching_ = true;
+    } else {
+      return {false, 0.0};
+    }
+  }
+
+  // If we're still far from the origin, no closure
+  if (dist > threshold_dist_) {
+    return {false, 0.0};
+  }
+
+  bool match_found = false;
+  // Look for match with any of the first X cones
+  for (int i = 0; i < associations.size(); ++i) {
+    int j = associations[i];
+    if (j >= 3) {
+      int map_idx = (j - 3) / 2;  // Index into map_cones
+      if (map_idx < first_x_cones_) {
+        match_found = true;  // Update the outer match_found variable
+        break;  // We found a match, no need to continue searching
+      }
+    }
+  }
+  
+  if (match_found) {
+    confidence_++;
+    if (confidence_ >= minimum_confidence_) {
+      // We found loop closure, reset confidence and return
+      confidence_ = 0;
+      searching_ = false;  // Reset search flag
+      return {true, 0.0};
+    }
+  } else {
+    confidence_ = 0;
+  }
+  
+  return {false, 0.0};
+}
\ No newline at end of file
diff --git a/src/slam/include/ros_node/slam_node.hpp b/src/slam/include/ros_node/slam_node.hpp
index 9d4181498..7c3676a3e 100644
--- a/src/slam/include/ros_node/slam_node.hpp
+++ b/src/slam/include/ros_node/slam_node.hpp
@@ -40,6 +40,8 @@ class SLAMNode : public rclcpp::Node {
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr _position_publisher_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr _execution_time_publisher_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr _covariance_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _lap_counter_publisher_;
+  
   std::shared_ptr<tf2_ros::TransformBroadcaster> _tf_broadcaster_;
   rclcpp::TimerBase::SharedPtr _timer_;      /**< timer */
   std::shared_ptr<SLAMSolver> _slam_solver_; /**< SLAM solver object */
@@ -89,6 +91,12 @@ class SLAMNode : public rclcpp::Node {
    */
   void _publish_covariance();
 
+  /**
+   * @brief publishes the lap counter
+   *
+   */
+  void _publish_lap_counter();
+
 public:
   // /**
   //  * @brief Constructor of the main node, most things are received by launch parameter
diff --git a/src/slam/include/slam_solver/ekf_slam_solver.hpp b/src/slam/include/slam_solver/ekf_slam_solver.hpp
index a04d4948d..60d4dd85b 100644
--- a/src/slam/include/slam_solver/ekf_slam_solver.hpp
+++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp
@@ -8,6 +8,7 @@
 #include "perception_sensor_lib/observation_model/base_observation_model.hpp"
 #include "slam_solver/slam_solver.hpp"
 
+
 class EKFSLAMSolver : public SLAMSolver {
   SLAMParameters slam_parameters_;
   std::shared_ptr<ObservationModel> observation_model_;
@@ -103,7 +104,8 @@ class EKFSLAMSolver : public SLAMSolver {
   EKFSLAMSolver(const SLAMParameters& params,
                 std::shared_ptr<DataAssociationModel> data_association,
                 std::shared_ptr<V2PMotionModel> motion_model,
-                std::shared_ptr<std::vector<double>> execution_times);
+                std::shared_ptr<std::vector<double>> execution_times,
+                std::shared_ptr<LoopClosure> loop_closure);
 
   /**
    * @brief Initialize the EKF SLAM solver
@@ -133,4 +135,11 @@ class EKFSLAMSolver : public SLAMSolver {
    * @return Eigen::MatrixXd covariance matrix
    */
   Eigen::MatrixXd get_covariance() override { return covariance_; }
+
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  int get_lap_counter() override { return lap_counter_; }
 };
diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp
index 9ffa954cb..134e3b3ea 100644
--- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp
+++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp
@@ -12,6 +12,8 @@
 #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp"
 #include "slam_solver/graph_slam_solver/pose_updater.hpp"
 #include "slam_solver/slam_solver.hpp"
+#include <perception_sensor_lib/loop_closure/lap_counter.hpp>
+
 
 /**
  * @brief Graph SLAM solver class
@@ -57,7 +59,8 @@ class GraphSLAMSolver : public SLAMSolver {
   GraphSLAMSolver(const SLAMParameters& params,
                   std::shared_ptr<DataAssociationModel> data_association,
                   std::shared_ptr<V2PMotionModel> motion_model,
-                  std::shared_ptr<std::vector<double>> execution_times);
+                  std::shared_ptr<std::vector<double>> execution_times,
+                  std::shared_ptr<LoopClosure> loop_closure);
 
   ~GraphSLAMSolver() = default;
 
@@ -103,6 +106,13 @@ class GraphSLAMSolver : public SLAMSolver {
    */
   Eigen::MatrixXd get_covariance() override;
 
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  int get_lap_counter() override { return lap_counter_; }
+
   /**
    * Timekeeping array
    * - 0: total motion time (prediction)
diff --git a/src/slam/include/slam_solver/map.hpp b/src/slam/include/slam_solver/map.hpp
index 25a104727..ee8c86a47 100644
--- a/src/slam/include/slam_solver/map.hpp
+++ b/src/slam/include/slam_solver/map.hpp
@@ -14,21 +14,21 @@
 const std::map<std::string,
                std::function<std::shared_ptr<SLAMSolver>(
                    const SLAMParameters&, std::shared_ptr<DataAssociationModel>,
-                   std::shared_ptr<V2PMotionModel>, std::shared_ptr<std::vector<double>>)>,
+                   std::shared_ptr<V2PMotionModel>, std::shared_ptr<std::vector<double>>, std::shared_ptr<LoopClosure>)>,
                std::less<>>
     slam_solver_constructors_map = {
         {"graph_slam",
          [](const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
             std::shared_ptr<V2PMotionModel> motion_model,
-            std::shared_ptr<std::vector<double>> execution_times) -> std::shared_ptr<SLAMSolver> {
+            std::shared_ptr<std::vector<double>> execution_times, std::shared_ptr<LoopClosure> loop_closure) -> std::shared_ptr<SLAMSolver> {
            return std::make_shared<GraphSLAMSolver>(params, data_association, motion_model,
-                                                    execution_times);
+                                                    execution_times, loop_closure);
          }},
         {"ekf_slam",
          [](const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
             std::shared_ptr<V2PMotionModel> motion_model,
-            std::shared_ptr<std::vector<double>> execution_times) -> std::shared_ptr<SLAMSolver> {
+            std::shared_ptr<std::vector<double>> execution_times, std::shared_ptr<LoopClosure> loop_closure) -> std::shared_ptr<SLAMSolver> {
            return std::make_shared<EKFSLAMSolver>(params, data_association, motion_model,
-                                                  execution_times);
+                                                  execution_times, loop_closure);
          }},
 };
diff --git a/src/slam/include/slam_solver/slam_solver.hpp b/src/slam/include/slam_solver/slam_solver.hpp
index 4115743a4..aed14fa12 100644
--- a/src/slam/include/slam_solver/slam_solver.hpp
+++ b/src/slam/include/slam_solver/slam_solver.hpp
@@ -7,6 +7,7 @@
 #include "common_lib/structures/velocities.hpp"
 #include "motion_lib/v2p_models/base_v2p_motion_model.hpp"
 #include "perception_sensor_lib/data_association/base_data_association.hpp"
+#include "perception_sensor_lib/loop_closure/loop_closure.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "slam_config/general_config.hpp"
 
@@ -23,12 +24,16 @@ class SLAMSolver {
   std::shared_ptr<std::vector<double>>
       _execution_times_;  //< Execution times: 0 -> total motion; 1 -> total
                           // observation; the rest are solver specific
+  std::shared_ptr<LoopClosure> _loop_closure_;  //< Loop closure object pointer
 
   rclcpp::Time _last_pose_update_ = rclcpp::Time(0);
   rclcpp::Time _last_observation_update_ = rclcpp::Time(0);
 
   bool _received_first_velocities_ =
       false;  //< Flag to check if the first velocities have been received
+  
+  
+  int lap_counter_ = 0;  //< Lap counter for the graph SLAM solver
 
 public:
   /**
@@ -38,10 +43,12 @@ class SLAMSolver {
    * @param data_association Data association module
    * @param motion_model Motion model
    * @param execution_times Timekeeping array
+   * @param loop_closure Loop closure model
    */
   SLAMSolver(const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
              std::shared_ptr<V2PMotionModel> motion_model,
-             std::shared_ptr<std::vector<double>> execution_times);
+             std::shared_ptr<std::vector<double>> execution_times, 
+             std::shared_ptr<LoopClosure> loop_closure);
 
   virtual ~SLAMSolver() = default;
 
@@ -88,4 +95,11 @@ class SLAMSolver {
    * @return Eigen::MatrixXd
    */
   virtual Eigen::MatrixXd get_covariance() = 0;
+
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  virtual int get_lap_counter() = 0;
 };
\ No newline at end of file
diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp
index 677b70afb..b22c1e327 100644
--- a/src/slam/src/ros_node/slam_node.cpp
+++ b/src/slam/src/ros_node/slam_node.cpp
@@ -12,6 +12,7 @@
 #include "common_lib/structures/position.hpp"
 #include "motion_lib/v2p_models/map.hpp"
 #include "perception_sensor_lib/data_association/map.hpp"
+#include "perception_sensor_lib/loop_closure/lap_counter.hpp"
 #include "slam_solver/map.hpp"
 
 /*---------------------- Constructor --------------------*/
@@ -28,10 +29,11 @@ SLAMNode::SLAMNode(const SLAMParameters &params) : Node("slam") {
           params.observation_y_noise_));
 
   this->_execution_times_ = std::make_shared<std::vector<double>>(20, 0.0);
+  std::shared_ptr<LoopClosure> loop_closure = std::make_shared<LapCounter>(4,10,5,3);
 
   // Initialize SLAM solver object
   this->_slam_solver_ = slam_solver_constructors_map.at(params.slam_solver_name_)(
-      params, data_association, motion_model, this->_execution_times_);
+      params, data_association, motion_model, this->_execution_times_, loop_closure);
 
   _perception_map_ = std::vector<common_lib::structures::Cone>();
   _vehicle_state_velocities_ = common_lib::structures::Velocities();
@@ -67,6 +69,9 @@ SLAMNode::SLAMNode(const SLAMParameters &params) : Node("slam") {
       "/state_estimation/slam_execution_time", 10);
   this->_covariance_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
       "/state_estimation/slam_covariance", 10);
+  this->_lap_counter_publisher_ = this->create_publisher<std_msgs::msg::Float64>(
+      "/state_estimation/lap_counter", 10);
+
   this->_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
   RCLCPP_INFO(this->get_logger(), "SLAM Node has been initialized");
@@ -106,6 +111,7 @@ void SLAMNode::_perception_subscription_callback(const custom_interfaces::msg::C
 
   this->_publish_vehicle_pose();
   this->_publish_map();
+  this->_publish_lap_counter();
 
   // Timekeeping
   rclcpp::Time end_time = this->get_clock()->now();
@@ -213,4 +219,10 @@ void SLAMNode::_publish_covariance() {
     }
   }
   this->_covariance_publisher_->publish(covariance_msg);
+}
+
+void SLAMNode::_publish_lap_counter() {
+  std_msgs::msg::Float64 lap_counter_msg;
+  lap_counter_msg.data = this->_slam_solver_->get_lap_counter();
+  this->_lap_counter_publisher_->publish(lap_counter_msg);
 }
\ No newline at end of file
diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp
index ce4937536..54d225e27 100644
--- a/src/slam/src/slam_solver/ekf_slam_solver.cpp
+++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp
@@ -3,8 +3,9 @@
 EKFSLAMSolver::EKFSLAMSolver(const SLAMParameters& params,
                              std::shared_ptr<DataAssociationModel> data_association,
                              std::shared_ptr<V2PMotionModel> motion_model,
-                             std::shared_ptr<std::vector<double>> execution_times)
-    : SLAMSolver(params, data_association, motion_model, execution_times),
+                             std::shared_ptr<std::vector<double>> execution_times,
+                             std::shared_ptr<LoopClosure> loop_closure)
+    : SLAMSolver(params, data_association, motion_model, execution_times, loop_closure),
       slam_parameters_(params) {
   this->covariance_ =
       Eigen::MatrixXd::Identity(3, 3) * 0.4;  // TODO: initialize with the right values
diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp
index f4e707a46..a610ab5bf 100644
--- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp
+++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp
@@ -31,8 +31,9 @@ Eigen::Vector3d gtsam_pose_to_eigen(const gtsam::Pose2& pose) {
 GraphSLAMSolver::GraphSLAMSolver(const SLAMParameters& params,
                                  std::shared_ptr<DataAssociationModel> data_association,
                                  std::shared_ptr<V2PMotionModel> motion_model,
-                                 std::shared_ptr<std::vector<double>> execution_times)
-    : SLAMSolver(params, data_association, motion_model, execution_times),
+                                 std::shared_ptr<std::vector<double>> execution_times,
+                                 std::shared_ptr<LoopClosure> loop_closure)
+    : SLAMSolver(params, data_association, motion_model, execution_times, loop_closure),
       _graph_slam_instance_(params, graph_slam_optimizer_constructors_map.at(
                                         params.slam_optimization_type_)(params)) {
   // TODO: transform into range and bearing noises
@@ -129,6 +130,17 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
   association_time = rclcpp::Clock().now();
   RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Associations calculated");
 
+  Eigen::Vector3d pose;
+  pose << state(0), state(1), state(2);
+
+  Eigen::VectorXi landmarks(state.size() - 3);
+  landmarks = state.segment(3, state.size() - 3).cast<int>();
+  LoopClosure::Result result = _loop_closure_->detect(pose, landmarks, associations, observations);
+  if (result.detected) {
+    lap_counter_++;
+    RCLCPP_INFO(rclcpp::get_logger("slam"), "Lap counter: %d", lap_counter_);
+  }
+
   {
     RCLCPP_DEBUG(rclcpp::get_logger("slam"),
                  "add_observations - Mutex locked - processing observations");
diff --git a/src/slam/src/slam_solver/slam_solver.cpp b/src/slam/src/slam_solver/slam_solver.cpp
index 1dc55045f..8b7b2635f 100644
--- a/src/slam/src/slam_solver/slam_solver.cpp
+++ b/src/slam/src/slam_solver/slam_solver.cpp
@@ -3,8 +3,10 @@
 SLAMSolver::SLAMSolver(const SLAMParameters& params,
                        std::shared_ptr<DataAssociationModel> data_association,
                        std::shared_ptr<V2PMotionModel> motion_model,
-                       std::shared_ptr<std::vector<double>> execution_times)
+                       std::shared_ptr<std::vector<double>> execution_times,
+                       std::shared_ptr<LoopClosure> loop_closure)
     : _params_(params),
       _data_association_(data_association),
       _motion_model_(motion_model),
-      _execution_times_(execution_times) {}
\ No newline at end of file
+      _execution_times_(execution_times),
+      _loop_closure_(loop_closure) {}
\ No newline at end of file
diff --git a/src/slam/test/slam_solver/ekf_slam_solver_test.cpp b/src/slam/test/slam_solver/ekf_slam_solver_test.cpp
index 4d9198bf4..1a22e51a4 100644
--- a/src/slam/test/slam_solver/ekf_slam_solver_test.cpp
+++ b/src/slam/test/slam_solver/ekf_slam_solver_test.cpp
@@ -16,9 +16,9 @@ class EKFSLAMSolverTest : public ::testing::Test {
                                   params.new_landmark_confidence_gate_, params.observation_x_noise_,
                                   params.observation_y_noise_));
     motion_model = v2p_models_map.at(params.motion_model_name_)();
-
+    
     ekf_slam_solver =
-        std::make_shared<EKFSLAMSolver>(params, data_association, motion_model, execution_time);
+        std::make_shared<EKFSLAMSolver>(params, data_association, motion_model, execution_time, nullptr);
   }
 
   std::shared_ptr<DataAssociationModel> data_association;
diff --git a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp
index 1cca52f26..5e76f2972 100644
--- a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp
+++ b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp
@@ -41,7 +41,7 @@ class GraphSlamSolverTest : public testing::Test {
     motion_model_ptr = mock_motion_model_ptr;
     data_association_ptr = mock_data_association_ptr;
     solver =
-        std::make_shared<GraphSLAMSolver>(params, data_association_ptr, motion_model_ptr, nullptr);
+        std::make_shared<GraphSLAMSolver>(params, data_association_ptr, motion_model_ptr, nullptr, nullptr);
     params.slam_optimization_period_ = 0.0;
   }