Update observation model (#21)

ToshikiNakamura0412 · May 27, 2024 · cf8f327 · cf8f327
1 parent 57164d3
commit cf8f327
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 12 deletions.
diff --git a/include/utils/particle.h b/include/utils/particle.h
@@ -90,12 +90,10 @@ class Particle
    * @param laser_angle laser angle
    * @param map map
    * @param laser_range laser range
-   * @param sensor_noise_ratio sensor noise ratio
    * @return float distance to the wall
    */
   float calc_dist_to_wall(
-      float x, float y, const float laser_angle, const nav_msgs::OccupancyGrid &map, const float laser_range,
-      const float sensor_noise_ratio);
+      float x, float y, const float laser_angle, const float laser_range, const nav_msgs::OccupancyGrid &map);
 
   /**
    * @brief Calculate the normal distribution

diff --git a/src/utils/particle.cpp b/src/utils/particle.cpp
@@ -34,8 +34,7 @@ float Particle::likelihood(
       continue;
 
     const float angle = i * laser.angle_increment + laser.angle_min;
-    const float range =
-        calc_dist_to_wall(pose_.x(), pose_.y(), angle + pose_.yaw(), map, laser.ranges[i], sensor_noise_ratio);
+    const float range = calc_dist_to_wall(pose_.x(), pose_.y(), angle + pose_.yaw(), laser.ranges[i], map);
     likelihood += norm_pdf(range, laser.ranges[i], laser.ranges[i] * sensor_noise_ratio);
   }
 
@@ -55,17 +54,15 @@ float Particle::likelihood(
       continue;
 
     const float angle = atan2(cloud.points[i].y, cloud.points[i].x);
-    const float range =
-        calc_dist_to_wall(pose_.x(), pose_.y(), angle + pose_.yaw(), map, laser_range, sensor_noise_ratio);
+    const float range = calc_dist_to_wall(pose_.x(), pose_.y(), angle + pose_.yaw(), laser_range, map);
     likelihood += norm_pdf(range, laser_range, laser_range * sensor_noise_ratio);
   }
 
   return likelihood;
 }
 
 float Particle::calc_dist_to_wall(
-    float x, float y, const float laser_angle, const nav_msgs::OccupancyGrid &map, const float laser_range,
-    const float sensor_noise_ratio)
+    float x, float y, const float laser_angle, const float laser_range, const nav_msgs::OccupancyGrid &map)
 {
   const float search_step = map.info.resolution;
   const float search_limit = laser_range;
@@ -78,14 +75,14 @@ float Particle::calc_dist_to_wall(
     const int grid_index = xy_to_grid_index(x, y, map.info);
 
     if (!in_map(grid_index, map.data.size()))
-      return search_limit * 2.0;
+      break;
     else if (map.data[grid_index] == -1)
-      return search_limit * 2.0;
+      break;
     else if (map.data[grid_index] == 100)
       return dist;
   }
 
-  return search_limit * sensor_noise_ratio * 5.0;
+  return search_limit * 2.0;
 }
 
 float Particle::norm_pdf(const float x, const float mean, const float stddev)