report on sensorModel+sensorModel particleFilter cleanup

report/main.aux

@@ -2,19 +2,19 @@
 \@writefile{toc}{\contentsline {section}{\numberline {I}MAPPING AND SLAM}{1}\protected@file@percent }
 \newlabel{eq:RTR}{{1}{1}}
 \newlabel{eq:fullRTR}{{2}{1}}
-\@writefile{lof}{\contentsline {figure}{\numberline {1}{\ignorespaces Particle distribution obtained at the end of drive square with action only\relax }}{2}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {2}{\ignorespaces Particle distribution obtained at the end of obstacle slam with action only\relax }}{2}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {1}{\ignorespaces Particle distribution obtained at the end of drive square with action only\relax }}{1}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {2}{\ignorespaces Particle distribution obtained at the end of obstacle slam with action only\relax }}{1}\protected@file@percent }
 \@writefile{lof}{\contentsline {figure}{\numberline {3}{\ignorespaces Particle distribution obtained at the end of straight line calm with action only\relax }}{2}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {4}{\ignorespaces Particle distribution obtained at the end of drive square with localization only\relax }}{3}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {5}{\ignorespaces Particle distribution obtained at the end of obstacle slam with localization only\relax }}{3}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {6}{\ignorespaces Particle distribution obtained at the end of straight line with localization only\relax }}{3}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {7}{\ignorespaces Particle distribution obtained at the end of Obstacle Slam with Full SLAM\relax }}{4}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {8}{\ignorespaces Particle distribution obtained at the end of Maze LowRes with Full SLAM\relax }}{4}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {9}{\ignorespaces Particle distribution obtained at the end of Maze HiRes with Full SLAM\relax }}{4}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {10}{\ignorespaces Particle distribution obtained at the end of Maze Hard with Full SLAM\relax }}{5}\protected@file@percent }
-\@writefile{toc}{\contentsline {section}{\numberline {II}PATH PLANNING AND EXPLORATION}{5}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {11}{\ignorespaces Obstacle distance grip obtained at the end of Obstacle Slam with Full SLAM\relax }}{5}\protected@file@percent }
-\@writefile{lof}{\contentsline {figure}{\numberline {12}{\ignorespaces Path obtained at the end of Maze HiRes with A* path planning\relax }}{5}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {4}{\ignorespaces Particle distribution obtained at the end of drive square with localization only\relax }}{2}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {5}{\ignorespaces Particle distribution obtained at the end of obstacle slam with localization only\relax }}{2}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {6}{\ignorespaces Particle distribution obtained at the end of straight line with localization only\relax }}{2}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {7}{\ignorespaces Particle distribution obtained at the end of Obstacle Slam with Full SLAM\relax }}{3}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {8}{\ignorespaces Particle distribution obtained at the end of Maze LowRes with Full SLAM\relax }}{3}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {9}{\ignorespaces Particle distribution obtained at the end of Maze HiRes with Full SLAM\relax }}{3}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {10}{\ignorespaces Particle distribution obtained at the end of Maze Hard with Full SLAM\relax }}{3}\protected@file@percent }
+\@writefile{toc}{\contentsline {section}{\numberline {II}PATH PLANNING AND EXPLORATION}{3}\protected@file@percent }
+\@writefile{lof}{\contentsline {figure}{\numberline {11}{\ignorespaces Obstacle distance grip obtained at the end of Obstacle Slam with Full SLAM\relax }}{3}\protected@file@percent }
 \citation{*}
 \bibstyle{IEEEtran}
 \bibdata{references.bib}
+\@writefile{lof}{\contentsline {figure}{\numberline {12}{\ignorespaces Path obtained at the end of Maze HiRes with A* path planning\relax }}{4}\protected@file@percent }

report/main.tex

@@ -1,4 +1,4 @@
-\documentclass[journal,onecolumn]{IEEEtran}
+\documentclass[journal,twocolumn]{IEEEtran}
 
 % include a few useful packages; you will probably want more
 \usepackage{graphicx} 
@@ -34,32 +34,12 @@ \section{MAPPING AND SLAM}
 
 \subsection*{1.1 - Mapping - Occupancy Grid} 
 
-Write a paragraph or two description including the following:
-
-\begin{itemize}
-    \item Provide the values of the incremental log odds you're using for a free \& occupied cells.
-    \item Describe the method used determine which cells to update.
-    \item Comment on mapping behavior.
-\end{itemize}
-
 The mapping function is implemented in two stages - first we score the cells which are located at the endpoint of the rays. If the cells corresponding to the endpoint of the rays are in the given map, log odds for the cell are increased by `3', defining the boundaries. Next, the cells between the robot cell and the endpoint cell are scored. Bresenham's line algorithm is then used to rasterize the given ray. The log odds of these free cells are then decreased by `1'. 
 
 The maps generated when the robot is stationary as is in the case of convex\_grid\_10mx10m\_5cm.log are not quite accurate at far away points. As the robot is stationary the laser rays originating from that position diverge farther away from each other at long distances, because of which some of the cells in between those rays don't get scored. This problem is mitigated when the robot is in motion; wherein because of the motion of the robot the laser rays tend to cover most of the grid cells and hence we obtain a better map in the end.  
 
 \subsection*{1.2.1 - MCL - Action Model} 
 
- Write a paragraph or two description including the following:
- \begin{itemize}
-    \item Specify what type of action model you're using.
-    \item Provide all noise constants that you're using.
-    \item Comment on the your action model:
-        \begin{itemize}
-            \item Does the distribution grow as expected for the particles?
-            \item Do you think the action model constants will differ drastically from one
-mbot to another?
-        \end{itemize}
-\end{itemize}
-
 Out of the two most widely used motion models, odometry motion model is used over the velocity motion model for this project. Odometry information is obtained by integrating the wheel encoder information over periodic time intervals. It is however observed that even though the odometry action model isn't perfect, it provides a better approximation of the robot motion than the velocity model for predicting the robot motion.
 
 In order to predict the robot pose the odometry information (i.e. x, y and $\theta$) is changed to an RTR information given by equation \ref{eq:RTR}, i.e rotation-translate-rotation which is sampled with a Gaussian noise distribution in the form described in equation \ref{eq:fullRTR}. In the initial phases of the project the values of the noise constants were chosen as $k_1 = 0.8$ and $k_2 = 0.3$. On further inspection and to reduce the computation load to calculate the standard deviation for each odometry information, the standard deviations are given as constants, where ${\sigma}^2_{rot1} = 0.05$, ${\sigma}^2_{trans} = 0.005$ and ${\sigma}^2_{rot2} = 0.05$. These noise constants are chosen and tuned in order to achieve a better spread of distribution of the particles and to achieve a more accurate proposal which is used for the sensor model. It is expected to see the tuning parameters differ from one mbot to another, but the difference should be small as the make and model of the mbot is consistent The elements which could factor into a possible drastic difference between the tuning parameters from one mbot to another could be the systematic errors related to the specific mbot.
@@ -100,14 +80,7 @@ \subsection*{1.2.1 - MCL - Action Model}
 
 \subsection*{1.2.2 - MCL - Sensor Model \& Particle Filter} 
 
- Write a paragraph or two description including the following:
- \begin{itemize}
-    \item Specify what type of sensor model you're using, and how you are weighting and resampling.
-    \item Provide all noise constants that you're using.
-    \item Is the estimated pose closer to the true pose with your localized pose estimate compared to that from odometry?
-    \item Do the particles remain in a tight region as the robot moves?
-    \item Do the particles spread more aggressively with a certain motion type? (rotation, for example).
-\end{itemize}
+A simplified likelihood field model is implemented which overcomes the lack of smoothness and computational limitations of the sensor beam model. The sensor model function provides a score for a given ray, by inspecting the endpoints of the ray. If the endpoints of the ray are occupied, i.e. the endpoint of the ray is an obstacle, the log odds of that cell is returned. But in case the endpoint is not an obstacle, the ray is extrapolated in the forward and backward direction by one cell and a fraction (0.5) of their log odds is added to the score. These ray scores are then used as weights for a given particle. Since, the sensor model acts as a correction step, the pose estimate is almost similar to the true pose. It is observed that the sensor model performs well and the particles tend to remain in a tight region as the robot moves. The particles though have a tendency to spread if the robot rotates aggressively, but the come closer as the robot starts to move again.
 
 \begin{figure}[H]
 \centering

src/slam/particle_filter.cpp

@@ -30,9 +30,7 @@ void ParticleFilter::initializeFilterAtPose(const pose_xyt_t& pose)
         p.parent_pose = p.pose;
         p.weight = sampleWeight;
     }
-
     posterior_.back().pose = pose;
-
 }
 
 
@@ -49,27 +47,20 @@ pose_xyt_t ParticleFilter::updateFilter(const pose_xyt_t&      odometry,
         posterior_ = computeNormalizedPosterior(proposal, laser, map);
         posteriorPose_ = estimatePosteriorPose(posterior_);
     }
-
     posteriorPose_.utime = odometry.utime;
-
     return posteriorPose_;
-    // return posterior_;
-
 }
 
 pose_xyt_t ParticleFilter::updateFilterActionOnly(const pose_xyt_t& odometry)
 {
     bool hasRobotMoved = actionModel_.updateAction(odometry);
-
     if(hasRobotMoved)
     {
         // auto prior = resamplePosteriorDistribution();
         auto proposal = computeProposalDistribution(posterior_);
         posterior_ = proposal;
     }
-
     posteriorPose_ = odometry;
-
     return posteriorPose_;
 }
 
@@ -95,12 +86,12 @@ std::vector<particle_t> ParticleFilter::resamplePosteriorDistribution(void)
     std::vector<particle_t> prior;
 
     int i = 0;
-    double M_inv = 1.0 / kNumParticles_; //float
+    double M_inv = 1.0 / kNumParticles_; 
     double c,r;
 
     r = (((double) rand()) / (double) RAND_MAX) * M_inv;
     c = posterior_[0].weight;
-    for (int m = 0; m < kNumParticles_; m++){  // start from m=0 and i=0
+    for (int m = 0; m < kNumParticles_; m++){ 
         double U = r + m * M_inv;
         while (U > c) {
             i++;
@@ -115,11 +106,9 @@ std::vector<particle_t> ParticleFilter::resamplePosteriorDistribution(void)
 std::vector<particle_t> ParticleFilter::computeProposalDistribution(const std::vector<particle_t>& prior)
 {
     std::vector<particle_t> proposal;
-
     for (auto& p : prior){
         proposal.push_back(actionModel_.applyAction(p));
     }
-
     return proposal;
 }
 
@@ -147,6 +136,7 @@ std::vector<particle_t> ParticleFilter::computeNormalizedPosterior(const std::ve
     for(auto& p : posterior){
         p.weight /= wSum;
     }
+
     return posterior;
 }
 
@@ -164,6 +154,7 @@ pose_xyt_t ParticleFilter::estimatePosteriorPose(const std::vector<particle_t>&
         weightedCos += p.weight * std::cos(p.pose.theta);
     }
     pose.theta = std::atan2(weightedSin, weightedCos);
+
     if((pose.x < 10000 && pose.y < 10000))
         return pose;
 }

src/slam/sensor_model.cpp

@@ -8,19 +8,12 @@
 SensorModel::SensorModel(void)
 : initialized_(false)
 {
-    ///////// TODO: Handle any initialization needed for your sensor model
 }
 
 
 double SensorModel::likelihood(const particle_t& sample, const lidar_t& scan, const OccupancyGrid& map)
 {
-    ///////////// TODO: Implement your sensor model for calculating the likelihood of a particle given a laser scan //////////
-
     double scanScore = 0.0;
-
-    // if(!initialized_){
-    //     sample.parent_pose = sample.pose;
-    // }
     MovingLaserScan movingScan(scan, sample.parent_pose, sample.pose);
 
 
@@ -45,34 +38,24 @@ double SensorModel::scoreRay(const adjusted_ray_t& ray, const OccupancyGrid& map
     rayExtended.y = (2 * ray.range * std::sin(ray.theta) * map.cellsPerMeter()) + rayStart.y;
 
     //one iteration breshenham on float of endpoint
-    // printf("\nend cell %d %d ----- ", rayEnd.x, rayEnd.y);
     double odds = map.logOdds(static_cast<int>(rayEnd.x), static_cast<int>(rayEnd.y));
 
     if (odds > 0) { // only > 0
         // return odds;  // no adding
     }
     else{
         odds = 0;
-        // printf("going in else function");
         int o1 = getCellodds(rayEnd.x, rayEnd.y, rayStart.x, rayStart.y, map);
-        // printf("cell up odds %d ----- ", o1);
         int o2 = getCellodds(rayEnd.x, rayEnd.y, rayExtended.x, rayExtended.y, map);
-        // printf("cell down odds %d ----- ", o2);
-
 
         if (o1 > 0) {
             odds += fraction * o1;
         }
         else if (o2 > 0){
             odds += fraction * o2;
         }
-
-        // return double instead of an integer
     }
-    // printf("cell odds parsed %f -----", odds);
-
     return odds;
-
 }
 
 int SensorModel::getCellodds(int x1, int y1, int x2, int y2, const OccupancyGrid& map)
@@ -99,6 +82,5 @@ int SensorModel::getCellodds(int x1, int y1, int x2, int y2, const OccupancyGrid
             y += sy;
     	}
 	}
-    // printf("\ncell %d %d -----", x, y);
     return map.logOdds(x,y);
 }