diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index ef804e470490cf51b73dacc0a860dc0bfb177bb0..6a56a4cf01dcac30f46b2d498c26e91b12754f37 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -150,7 +150,7 @@ double AdcLandmarksSlamSolverAlgorithm::mahalanobis_distance(Eigen::Vector2d _fe
 
 double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _feature_pos, double _feature_r, double _feature_th, int _type, const std::string& _source_frame)
 {
-  double min_dist = this->config_.landmark_mahalanobis_dist + 1.0;
+  double min_dist = 99999999999.0;
   std::map<double, LandmarkInfo>::iterator min_dist_land = this->landmarks_.end();
   for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
   {
@@ -162,9 +162,10 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _fe
       // std::cout << "check dist " << dist << std::endl;
       if (dist <= this->config_.landmark_mahalanobis_dist)
       {
-        if (dist < min_dist)
+        Eigen::Vector2d d = landmark_pos - _feature_pos;
+        if (d.norm() < min_dist)
         {
-          min_dist = dist;
+          min_dist = d.norm();
           min_dist_land = it;
         }
       }
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index 97b8b6331282447317441942d3431420c27043bd..8480ee3aa8e88927e79cd82a1a1e441961be9035 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -640,7 +640,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
   //Check if a landmark candidate has been seen enough times
   if (!this->config_.landmarks_candidates_filter_en)
     return true;
-  double min_dist = this->config_.landmark_mahalanobis_dist + 1.0;
+  double min_dist = 999999999.0;
   std::list<LandmarkCandidate>::iterator min_dist_land = this->landmarks_candidates_.end();
   for (auto it = this->landmarks_candidates_.begin(); it != this->landmarks_candidates_.end(); ++it)
   {
@@ -652,9 +652,10 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
       double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
       if (dist <= this->config_.landmark_mahalanobis_dist)
       {
-        if (dist < min_dist)
+        Eigen::Vector2d d = landmark_pos - feature_pos;
+        if (d.norm() < min_dist)
         {
-          min_dist = dist;
+          min_dist = d.norm();
           min_dist_land = it;
         }
       }