diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp
index b4791a6..5c4158c 100644
--- a/slam3d/sensor/pcl/PointCloudSensor.cpp
+++ b/slam3d/sensor/pcl/PointCloudSensor.cpp
@@ -127,8 +127,8 @@ Transform align(PointCloudMeasurement::Ptr source,
 	PointCloud::Ptr filtered_target = target->getPointCloud();
 	if(config.point_cloud_density > 0)
 	{
-		filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density);
-		filtered_target = PointCloudSensor::downsample(target->getPointCloud(), config.point_cloud_density);
+		filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density, config.point_cloud_coord_limit);
+		filtered_target = PointCloudSensor::downsample(target->getPointCloud(), config.point_cloud_density, config.point_cloud_coord_limit);
 	}
 	
 	// Make sure that there are enough points left (ICP will crash if not)
@@ -202,12 +202,15 @@ PointCloud::Ptr PointCloudSensor::crop(PointCloud::Ptr in, const Eigen::Vector4f
 	}
 }
 
-PointCloud::Ptr PointCloudSensor::downsample(PointCloud::Ptr in, double leaf_size)
+PointCloud::Ptr PointCloudSensor::downsample(PointCloud::Ptr in, double leaf_size, double maxrange)
 {
 	if(in->size() > 0 && leaf_size > 0)
 	{
 		PointCloud::Ptr out(new PointCloud);
 		pcl::VoxelGrid<PointType> grid;
+		if (maxrange>0) {
+			grid.setFilterLimits(-maxrange,maxrange);
+		}
 		grid.setLeafSize (leaf_size, leaf_size, leaf_size);
 		grid.setInputCloud(in);
 		grid.filter(*out);
diff --git a/slam3d/sensor/pcl/PointCloudSensor.hpp b/slam3d/sensor/pcl/PointCloudSensor.hpp
index ea429cf..3e06238 100644
--- a/slam3d/sensor/pcl/PointCloudSensor.hpp
+++ b/slam3d/sensor/pcl/PointCloudSensor.hpp
@@ -179,7 +179,7 @@ namespace slam3d
 		 * @param source
 		 * @param resolution 
 		 */
-		static PointCloud::Ptr downsample(PointCloud::Ptr source, double resolution);
+		static PointCloud::Ptr downsample(PointCloud::Ptr source, double resolution, double maxrange = -1.0);
 		
 		/**
 		 * @brief Crop the source cloud to a square box with given corners.
diff --git a/slam3d/sensor/pcl/RegistrationParameters.hpp b/slam3d/sensor/pcl/RegistrationParameters.hpp
index 867a57b..376e72d 100644
--- a/slam3d/sensor/pcl/RegistrationParameters.hpp
+++ b/slam3d/sensor/pcl/RegistrationParameters.hpp
@@ -44,6 +44,10 @@ namespace slam3d
 		// pointclouds will be downsampled to this density before the alignement
 		double point_cloud_density = 0.2;
 
+		// point with a coordinage higher than point_cloud_coord_limit or lower than 
+		// -point_cloud_coord_limit are ignored, only active when point_cloud_density > 0
+		double point_cloud_coord_limit = 80;
+
 		// maximum fitness score (e.g., sum of squared distances from the source to the target)
 		// to accept the registration result
 		double max_fitness_score = 2.0;