diff --git a/include/adc_landmarks_slam_solver_alg.h b/include/adc_landmarks_slam_solver_alg.h
index 438c999a3472d4e1e3e6a7071b7cfa68b6f1adfc..a626ef8e62910992fe3b96bfb5cbdbe67f8fccc8 100644
--- a/include/adc_landmarks_slam_solver_alg.h
+++ b/include/adc_landmarks_slam_solver_alg.h
@@ -127,6 +127,14 @@ class AdcLandmarksSlamSolverAlgorithm
       */
     void get_covariance_options(ceres::Covariance::Options& _cov_opt);
 
+    /**
+      * \brief Function to get frames_data size.
+      *
+      * \return The actual size.
+      *
+      */
+    unsigned int get_frames_data_size(void);
+
    /**
     * \brief define config type
     *
diff --git a/include/adc_landmarks_slam_solver_alg_node.h b/include/adc_landmarks_slam_solver_alg_node.h
index 0045a7e4ae9608c0c0bb09eab1a1ab4b0d0d1501..f52f13860f79cbbd539b50aa9e78f89a38052554 100644
--- a/include/adc_landmarks_slam_solver_alg_node.h
+++ b/include/adc_landmarks_slam_solver_alg_node.h
@@ -98,6 +98,7 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
   private:
     bool init_tf_map_odom; ///< Boolean to know if the initial tf map odom was received.
     Eigen::Vector3d tf_map_odom; ///< Odom's x, y and rotation from fixed frame.
+    Eigen::Vector3d tf_odom_base; ///< Robot base's x, y and rotation from odom frame.
 
     bool new_features_data; ///< Bool to know that new features has been received.
     std::string features_frame; ///< Features origin frame.
@@ -108,7 +109,7 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
     std::map<double, Landmark_info> landmarks; ///< Landmarks.
 
     ros::Time last_update_t; ///< Last frame update time.
-    Eigen::Vector3d last_update_odom; ///< Last frame update odometry.
+    Eigen::Vector3d last_update_odom_base; ///< Last frame update odometry.
     bool update_problem; ///< Flag to update and solve the Ceres problem.
 
     ros::Time last_publish_t; ///< Last publish pose time.
@@ -210,6 +211,13 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
      */
     double mahalanobis_distance(Eigen::Vector2d feature_pos, double feature_r, double feature_th, Eigen::Vector2d landmark_pos);
 
+    /**
+     * \brief Function to update tf from map to odom at a requested time stamp.
+     *
+     * \param t The time stamp.
+     */
+    void update_tf_odom_base(ros::Time t);
+
     /**
      * \brief Function to publish landmarks markers.
      */
diff --git a/launch/test.launch b/launch/test.launch
index 95b6075ccf20b71838e6fcf3cb7381402df04069..5c3a30d90d8f0ec72e225b57c4442832ce9de878 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -1,6 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
+  <arg name="ns"                    default="adc_car"/>
   <arg name="output"                default="screen"/>
   <arg name="launch_prefix"         default=""/>
   <arg name="dr"                    default="true"/>
@@ -9,24 +10,26 @@
   <arg name="data_dir"              default="$(find iri_adc_landmarks_slam_solver)/data/"/>
   <arg name="landmarks_file"        default="landmarks.txt"/>
 
-  <arg name="estimated_pose_topic_name" default="/initialpose"/>
-  <arg name="features_topic_name" default="/iri_adc_tag_localization_filter/features"/>
+  <arg name="estimated_pose_topic_name" default="/$(arg ns)/initialpose"/>
+  <arg name="features_topic_name" default="/$(arg ns)/iri_adc_tag_localization_filter/features"/>
 
-  <include file="$(find iri_adc_landmarks_slam_solver)/launch/node.launch">
-    <arg name="node_name"     value="iri_adc_landmarks_slam_solver"/>
-    <arg name="output"        value="$(arg output)"/>
-    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
-    <arg name="config_file"           value="$(arg config_dir)/$(arg config_file)"/>
-    <arg name="landmarks_map_file"    value="$(arg data_dir)/$(arg landmarks_file)"/>
-    <arg name="estimated_pose_topic_name"  value="$(arg estimated_pose_topic_name)"/>
-    <arg name="features_topic_name"  value="$(arg features_topic_name)"/>
-  </include>
+  <group ns="$(arg ns)">
+    <include file="$(find iri_adc_landmarks_slam_solver)/launch/node.launch">
+      <arg name="node_name"     value="iri_adc_landmarks_slam_solver"/>
+      <arg name="output"        value="$(arg output)"/>
+      <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+      <arg name="config_file"           value="$(arg config_dir)/$(arg config_file)"/>
+      <arg name="landmarks_map_file"    value="$(arg data_dir)/$(arg landmarks_file)"/>
+      <arg name="estimated_pose_topic_name"  value="$(arg estimated_pose_topic_name)"/>
+      <arg name="features_topic_name"  value="$(arg features_topic_name)"/>
+    </include>
+  </group>
 
   <node name="rqt_reconfigure_iri_adc_landmarks_slam_solver"
         pkg ="rqt_reconfigure"
         type="rqt_reconfigure"
         if  ="$(arg dr)"
-        args="iri_adc_landmarks_slam_solver">
+        args="$(arg ns)/iri_adc_landmarks_slam_solver">
   </node>
 
 </launch>
diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index 5689f306df68ec1a8b01fcc24e82a23ff07420b9..fe2261b39edc5706d7a27b319690d1a537ed8ddc 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -48,3 +48,8 @@ void AdcLandmarksSlamSolverAlgorithm::get_covariance_options(ceres::Covariance::
 {
   _cov_opt = this->cov_opt_;
 }
+
+unsigned int AdcLandmarksSlamSolverAlgorithm::get_frames_data_size(void)
+{
+	return this->frames_data_.size();
+}
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index c59a03c71ee6a996f5155ddadab77a058584f088..92a1bf99d8990b6ef8a0f8de4e06eda4e0c213af 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -88,6 +88,7 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   }
 
   //Landmarks file
+  std::map<double, Landmark_info>().swap(this->landmarks);
   if (this->config_.load_landmarks)
   {
     if(!this->private_node_handle_.getParam("landmarks_map_file", this->config_.landmarks_map_file))
@@ -111,12 +112,13 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   pthread_mutex_init(&this->features_mutex_,NULL);
 
   //Init class atributes
+  this->tf_odom_base << 0.0, 0.0, 0.0;
   this->new_features_data = false;
   this->new_estimated_pose = false;
 
   this->init_tf_map_odom = this->config_.amcl_localization;
   this->last_update_t = ros::Time::now();
-  this->last_update_odom << 0.0, 0.0, 0.0;
+  this->last_update_odom_base << 0.0, 0.0, 0.0;
   this->update_problem = false;
 
   this->last_publish_t = ros::Time::now();
@@ -125,7 +127,6 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
 
   std::vector<Feature_info>().swap(this->features);
   std::list<Landmark_candidate>().swap(this->landmarks_candidates);
-  std::map<double, Landmark_info>().swap(this->landmarks);
 
   // [init services]
   
@@ -154,11 +155,15 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
   this->estimated_pose_mutex_enter();
   // ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::mainNodeThread");
 
+  FrameData frame_data;
+  ros::Time t = ros::Time::now();
+  Eigen::Vector2d dist;
+  double inc_ang;
   if (this->init_tf_map_odom)
   {
-    if (this->new_estimated_pose)
+    //Calculate new tf_map_odom if necessary
+    if (this->new_estimated_pose && !this->config_.amcl_localization)
     {
-      this->new_estimated_pose = false;
       tf2::Transform tf_map_odom, tf_odom_base, tf_map_base;
       geometry_msgs::TransformStamped tf_odom_base_msg;
       if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t, ros::Duration(this->config_.tf_timeout)))
@@ -187,6 +192,65 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
       this->tf_map_odom(2) = yaw;
     }
 
+    //Is time to update problem?
+    update_tf_odom_base(t);
+    dist(0) = this->tf_odom_base(0) - this->last_update_odom_base(0);
+    dist(1) = this->tf_odom_base(1) - this->last_update_odom_base(1);
+    inc_ang = std::fabs(this->tf_odom_base(2) - this->last_update_odom_base(2));
+
+    if (((t - this->last_update_t) > ros::Duration(1/this->config_.update_problem_rate)) || (dist.norm() > this->config_.update_problem_distance) ||
+       (inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose)
+    {
+      this->last_update_t = t;
+      frame_data.ts = t;
+      this->last_update_odom_base = this->tf_odom_base;
+      //Get global robot_state
+      if (this->new_estimated_pose)
+        frame_data.robot_state = this->estimated_pose;
+      else
+      {
+        if (!this->config_.amcl_localization)
+        {
+          Eigen::Matrix3d rot;
+          rot <<  cos(this->tf_map_odom(2)), -sin(this->tf_map_odom(2)), 0.0,
+                  sin(this->tf_map_odom(2)), cos(this->tf_map_odom(2)), 0.0,
+                  0.0, 0.0, 1.0;
+          frame_data.robot_state = rot*this->tf_odom_base + this->tf_map_odom;
+        }
+        else
+        {
+          geometry_msgs::TransformStamped tf_robot;
+          if (!this->tf2_buffer.canTransform(this->config_.global_frame, this->config_.base_link_frame, frame_data.ts, ros::Duration(this->config_.tf_timeout)))
+          {
+            ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.global_frame << " to " << this->config_.base_link_frame << " at " << frame_data.ts);
+            this->estimated_pose_mutex_exit();
+            this->features_mutex_exit();
+            return;
+          }
+          tf_robot = this->tf2_buffer.lookupTransform(this->config_.global_frame, this->config_.base_link_frame, frame_data.ts);
+          frame_data.robot_state(0) = tf_robot.transform.translation.x;
+          frame_data.robot_state(1) = tf_robot.transform.translation.y;
+          tf2::Quaternion q(tf_robot.transform.rotation.x, tf_robot.transform.rotation.y, tf_robot.transform.rotation.z, tf_robot.transform.rotation.w);
+          double yaw, pitch, roll;
+          tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+          frame_data.robot_state(2) = yaw;
+        }
+      }
+
+      if (this->alg_.get_frames_data_size() == 0)//First
+      {
+        this->update_problem = false;
+        // this->alg_.add_frame(frame_data)
+        // this->frames_data.push_back(frame_data);
+        // if (this->config_.debug)
+        //   update_frame_markers();
+      }
+      else
+        this->update_problem = true;
+      ////////////////////
+      this->new_estimated_pose = false;
+    }
+
     if (this->new_features_data)
     {
       // std::cout << "new data" << std::endl;
@@ -323,7 +387,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool update_landmarks)
 
   if (this->features_frame == "")
   {
-    ROS_WARN("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter nor initialized.");
+    ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::check_landmarks -> Features frame id empty; AR tag filter not initialized.");
     return false;
   }
 
@@ -505,6 +569,25 @@ double AdcLandmarksSlamSolverAlgNode::mahalanobis_distance(Eigen::Vector2d featu
   return std::sqrt(dist2);
 }
 
+void AdcLandmarksSlamSolverAlgNode::update_tf_odom_base(ros::Time t)
+{
+  geometry_msgs::TransformStamped tf_odom_base_msg;
+  if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, t, ros::Duration(this->config_.tf_timeout)))
+  {
+    ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.odom_frame << " to " << this->config_.base_link_frame << " at " << t);
+    this->estimated_pose_mutex_exit();
+    this->features_mutex_exit();
+    return;
+  }
+  tf_odom_base_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, this->config_.base_link_frame, t);
+  this->tf_odom_base(0) = tf_odom_base_msg.transform.translation.x;
+  this->tf_odom_base(1) = tf_odom_base_msg.transform.translation.y;
+  tf2::Quaternion q(tf_odom_base_msg.transform.rotation.x, tf_odom_base_msg.transform.rotation.y, tf_odom_base_msg.transform.rotation.z, tf_odom_base_msg.transform.rotation.w);
+  double yaw, pitch, roll;
+  tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+  this->tf_odom_base(2) = yaw;
+}
+
 void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
 {
   ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
@@ -583,23 +666,20 @@ void AdcLandmarksSlamSolverAlgNode::estimated_pose_callback(const geometry_msgs:
   //this->alg_.lock();
   this->estimated_pose_mutex_enter();
 
-  if (!this->config_.amcl_localization)
-  {
-    this->new_estimated_pose = true;
-    this->init_tf_map_odom = true;
-    this->new_estimated_pose_t = msg->header.stamp;
-    this->estimated_pose(0) = msg->pose.pose.position.x;
-    this->estimated_pose(1) = msg->pose.pose.position.y;
-    this->estimated_sigma(0) = std::sqrt(msg->pose.covariance[0]);
-    this->estimated_sigma(1) = std::sqrt(msg->pose.covariance[7]);
-    this->estimated_sigma(2) = std::sqrt(msg->pose.covariance[35]);
+  this->new_estimated_pose = true;
+  this->init_tf_map_odom = true;
+  this->new_estimated_pose_t = msg->header.stamp;
+  this->estimated_pose(0) = msg->pose.pose.position.x;
+  this->estimated_pose(1) = msg->pose.pose.position.y;
+  this->estimated_sigma(0) = std::sqrt(msg->pose.covariance[0]);
+  this->estimated_sigma(1) = std::sqrt(msg->pose.covariance[7]);
+  this->estimated_sigma(2) = std::sqrt(msg->pose.covariance[35]);
 
-    tf2::Quaternion q;
-    q = tf2::Quaternion(0.0, 0.0, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
-    double yaw, pitch, roll;
-    tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
-    this->estimated_pose(2) = yaw;
-  }
+  tf2::Quaternion q;
+  q = tf2::Quaternion(0.0, 0.0, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
+  double yaw, pitch, roll;
+  tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+  this->estimated_pose(2) = yaw;
 
   //std::cout << msg->data << std::endl;
   //unlock previously blocked shared variables