diff --git a/cfg/ObstacleDetectionNormals.cfg b/cfg/ObstacleDetectionNormals.cfg
index c9c10542f9785ef5e2b1102b72b0ba8f8cf9a0bd..f1f1d5708b6c185d37e3d45eadd54c7964cf6d55 100755
--- a/cfg/ObstacleDetectionNormals.cfg
+++ b/cfg/ObstacleDetectionNormals.cfg
@@ -39,9 +39,7 @@ gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level  Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                     "Maximum velocity scale factor",  0.5,      0.0,  1.0)
-gen.add("filter_min_dist",         double_t,  0,                     "Min distance threshold",         0.3,        0,  100)
+gen.add("filter_min_dist",         double_t,  0,                     "Min distance threshold",         0.3,      0,  100)
 gen.add("normal_KSearch",          int_t,     0,                     "No of neighbors to estimate the surface",100,10, 200)
-gen.add("normal_x",                double_t,  0,                     "Accepted Normal X component",    1,)
-gen.add("normal_y",                double_t,  0,                     "Accepted Normal Y component",    -0.2,)
-gen.add("normal_z",                double_t,  0,                     "Accepted Normal Z component",    1,)
+gen.add("max_inc",                 double_t,  0,                     "maximum inclination in rad",     0.25,     0.0,  1.0)
 exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))
diff --git a/config/default_config.yaml b/config/default_config.yaml
index 9fab8e59286a729193783331cf022d19fc028cb2..1ce6dbef806f81974b01423a9fca4ec9de6bae1d 100755
--- a/config/default_config.yaml
+++ b/config/default_config.yaml
@@ -1,5 +1,3 @@
 filter_min_dist: 0.3
 normal_KSearch: 30
-normal_x: 1.001
-normal_y: 1.001
-normal_z: 0.95
+max_inc: 0.25
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index 7854b2d361e6adc37cf27509cdf8adb015dfab3f..33061adfa66d070ce6dcacd373b48974d5e154eb 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -58,18 +58,14 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
 
   *cloud_obs=*cloud_out;
 
-  std::cout << &this->config_ << std::endl;
-  std::cout << this->config_.normal_z << "," << this->config_.normal_y << "," << this->config_.normal_x << std::endl;
   for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
   {
     for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
     {
       if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
       {
-//        std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << cloud_out->points[pointIndex].normal_y << "," << cloud_out->points[pointIndex].normal_x << std::endl;
-        if (abs(cloud_out->points[pointIndex].normal_z) > this->config_.normal_z &&
-            cloud_out->points[pointIndex].normal_y < this->config_.normal_y &&
-            cloud_out->points[pointIndex].normal_x < this->config_.normal_x )
+        std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << this->config_.max_inc << std::endl;
+        if (fabs(cloud_out->points[pointIndex].normal_z) > cos(this->config_.max_inc))
         {
           cloud_out->points[pointIndex].r=0;
           cloud_out->points[pointIndex].g=255;