diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index b05638bbd32cb9e2d5a504c48b54332cc2cb71d5..ac411a2116276d05cefdeb510336e7fd73bdcd31 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -180,9 +180,14 @@ void HokuyoLaserUnit::updateScanData()
             for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
             {
                 LaserScanStep step;
-                step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
-                step.distance = (float)device.lengthData[stepIndex]; // Data is already in mm
-                scan.push_back(step);
+                long distance = device.lengthData[stepIndex];
+                // TODO: Extract the min and max valid value for distance into parameters
+                if (distance >= 21 && distance <= 30000)
+                {
+                    step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
+                    step.distance = (float)distance; // Data is already in mm
+                    scan.push_back(step);
+                }
             }
 
             // TODO: Better names for the devices?