enhance histogram filter doc (#623)

* enhance histogram filter doc

* enhance histogram filter doc

* enhance histogram filter doc

* enhance histogram filter doc

* enhance histogram filter doc

* enhance histogram filter doc

* Update docs/modules/localization/histogram_filter_localization/histogram_filter_localization.rst

* Update docs/modules/localization/histogram_filter_localization/histogram_filter_localization.rst

* fix duplicated math lables

* fix duplicated math lables

* fix duplicated math lables

* update doc

* update doc

* update doc

* update doc

* update doc

* update doc

docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.rst

@@ -120,10 +120,10 @@ Its Jacobian matrix is
 
 :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}`
 
-Extented Kalman Filter
+Extended Kalman Filter
 ~~~~~~~~~~~~~~~~~~~~~~
 
-Localization process using Extendted Kalman Filter:EKF is
+Localization process using Extended Kalman Filter:EKF is
 
 === Predict ===
 

docs/modules/localization/histogram_filter_localization/histogram_filter_localization.rst

@@ -9,14 +9,106 @@ The red cross is true position, black points are RFID positions.
 
 The blue grid shows a position probability of histogram filter.
 
-In this simulation, x,y are unknown, yaw is known.
+In this simulation, we assume the robot's yaw orientation and RFID's positions are known,
+but x,y positions are unknown.
+
+The filter uses speed input and range observations from RFID for localization.
+
+Initial position information is not needed.
+
+Filtering algorithm
+~~~~~~~~~~~~~~~~~~~~
+
+Histogram filter is a discrete Bayes filter in continuous space.
+
+It uses regular girds to manage probability of the robot existence.
+
+If a grid has higher probability, it means that the robot is likely to be there.
+
+In the simulation, we want to estimate x-y position, so we use 2D grid data.
+
+There are 4 steps for the histogram filter to estimate the probability distribution.
+
+Step1: Filter initialization
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Histogram filter does not need initial position information.
+
+In that case, we can initialize each grid probability as a same value.
+
+If we can use initial position information, we can set initial probabilities based on it.
+
+:ref:`gaussian_grid_map` might be useful when the initial position information is provided as gaussian distribution.
+
+Step2: Predict probability by motion
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+In histogram filter, when a robot move to a next grid,
+all probability information of each grid are shifted towards the movement direction.
+
+This process represents the change in the probability distribution as the robot moves.
+
+After the robot has moved, the probability distribution needs reflect
+the estimation error due to the movement.
+
+For example, the position probability is peaky with observations:
+
+.. image:: histogram_filter_localization/1.png
+   :width: 400px
+
+But, the probability is getting uncertain without observations:
+
+.. image:: histogram_filter_localization/2.png
+   :width: 400px
+
+
+The `gaussian filter <https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.gaussian_filter.html>`_
+is used in the simulation for adding noize.
+
+Step3: Update probability by observation
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+In this step, all probabilities are updated by observations,
+this is the update step of bayesian filter.
+
+The probability update formula is different by the used sensor model.
+
+This simulation uses range observation model.
+
+The probability of each grid is updated by this formula:
+
+.. math:: p_t=p_{t-1}*h(z)
+
+.. math:: h(z)=\frac{\exp \left(-(d - z)^{2} / 2\right)}{\sqrt{2 \pi}}
+
+- :math:`p_t` is the probability at the time `t`.
+
+- :math:`h(z)` is the observation probability with the observation `z`.
+
+- :math:`d` is the known distance from the RD-ID to the grid center.
+
+When the `d` is 3.0, the `h(z)` distribution is:
+
+.. image:: histogram_filter_localization/4.png
+   :width: 400px
+
+The observation probability distribution looks a circle when a RF-ID is observed:
+
+.. image:: histogram_filter_localization/3.png
+   :width: 400px
+
+Step4: Estimate position from probability
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+In each time step, we can calculate the final robot position from the current probability distribution.
+There are two ways to calculate the final positions:
+
+1. Using the maximum probability grid position.
+
+2. Using the average of probability weighted grind position.
 
-The filter integrates speed input and range observations from RFID for
-localization.
 
-Initial position is not needed.
 
 References:
 ~~~~~~~~~~~
 
--  `PROBABILISTIC ROBOTICS`_
+- `PROBABILISTIC ROBOTICS`_
+- `Robust Vehicle Localization in Urban Environments Using Probabilistic Maps <http://driving.stanford.edu/papers/ICRA2010.pdf>`_

docs/modules/mapping/gaussian_grid_map/gaussian_grid_map.rst

@@ -1,3 +1,5 @@
+.. _gaussian_grid_map:
+
 Gaussian grid map
 -----------------