diff --git a/src/ground_segmentation_alg.cpp b/src/ground_segmentation_alg.cpp
index 7db306481f4678567b125e8bd86d6a92c3df07ef..dc76775db2b501ab51fb368aaa9975b3e4516ab8 100644
--- a/src/ground_segmentation_alg.cpp
+++ b/src/ground_segmentation_alg.cpp
@@ -88,11 +88,11 @@ void GroundSegmentationAlgorithm::segmentGround(
       vertex.id = discarded_vertex_id++;
     }
 
-    vertex.type = visualization_msgs::Marker::CUBE;
+    vertex.type = visualization_msgs::Marker::SPHERE;
 
     vertex.pose.position.x = ground_ref.x;
     vertex.pose.position.y = ground_ref.y;
-    vertex.pose.position.z = ground_ref.z + 0.15 / 2.0;
+    vertex.pose.position.z = ground_ref.z;
 
     vertex.pose.orientation.x = 0.0;
     vertex.pose.orientation.y = 0.0;
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index c649e31743ab214ce6f9ca1962106771268902aa..1da6b2fa1be2b2b005e53c996949916c4a4a63da 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -38,6 +38,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   }
   else
   {
+    // we estimate the slopes in x and y axis, so it is the tangent of the angle, but we set the values in degrees because
+    // it results more intuitive
     this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
     this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
   }
@@ -89,8 +91,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
         "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_additive_noise_deg_per_meter' not found");
   }
   else
-    this->alg_.filtering_configuration_.propagation_additive_noise = config_.propagation_additive_noise_deg_per_meter
-        * M_PI / 180.0;
+    this->alg_.filtering_configuration_.propagation_additive_noise = tan(config_.propagation_additive_noise_deg_per_meter
+        * M_PI / 180.0);
 
   if (!this->private_node_handle_.getParam("z_observation_std_dev", this->config_.z_observation_std_dev))
   {
@@ -434,8 +436,8 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   this->alg_.filtering_configuration_.search_limit_in_shadow_area = config.search_limit_in_shadow_area;
 
   this->alg_.filtering_configuration_.propagation_z_additive_noise = config.propagation_z_additive_noise_per_meter;
-  this->alg_.filtering_configuration_.propagation_additive_noise = config.propagation_additive_noise_deg_per_meter
-      * M_PI / 180.0;
+  this->alg_.filtering_configuration_.propagation_additive_noise = tan(config.propagation_additive_noise_deg_per_meter
+      * M_PI / 180.0);
   this->alg_.filtering_configuration_.z_observation_std_dev = config.z_observation_std_dev;
 
   this->alg_.filtering_configuration_.robot_height = config.robot_height;
@@ -476,7 +478,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   std::cout << "propagation_z_additive_noise in m/m  =          "
       << this->alg_.filtering_configuration_.propagation_z_additive_noise << std::endl;
   std::cout << "propagation_additive_noise in deg/m  =          "
-      << this->alg_.filtering_configuration_.propagation_additive_noise * 180.0 / M_PI << std::endl;
+      << atan((double)this->alg_.filtering_configuration_.propagation_additive_noise) * 180.0 / M_PI << std::endl;
   std::cout << "z_observation_std_dev       =                   "
       << this->alg_.filtering_configuration_.z_observation_std_dev << std::endl;