enhance histogram filter doc (AtsushiSakai#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

Author: AtsushiSakai

Localization/histogram_filter/histogram_filter.py

@@ -71,7 +71,7 @@ def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
     d = math.hypot(x - z[iz, 1], y - z[iz, 2])
 
     # likelihood
-    pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
+    pdf = norm.pdf(d - z[iz, 0], 0.0, std)
 
     return pdf
 
@@ -88,7 +88,7 @@ def observation_update(grid_map, z, std):
     return grid_map
 
 
-def calc_input():
+def calc_control_input():
     v = 1.0  # [m/s]
     yaw_rate = 0.1  # [rad/s]
     u = np.array([v, yaw_rate]).reshape(2, 1)
@@ -113,6 +113,7 @@ def motion_model(x, u):
 
 def draw_heat_map(data, mx, my):
     max_value = max([max(i_data) for i_data in data])
+    plt.grid(False)
     plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
     plt.axis("equal")
 
@@ -197,6 +198,7 @@ def motion_update(grid_map, u, yaw):
         grid_map.dx -= x_shift * grid_map.xy_resolution
         grid_map.dy -= y_shift * grid_map.xy_resolution
 
+    # Add motion noise
     grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
 
     return grid_map
@@ -230,9 +232,9 @@ def main():
 
     while SIM_TIME >= time:
         time += DT
-        print("Time:", time)
+        print(f"{time=:.1f}")
 
-        u = calc_input()
+        u = calc_control_input()
 
         yaw = xTrue[2, 0]  # Orientation is known
         xTrue, z, ud = observation(xTrue, u, RF_ID)
@@ -249,8 +251,9 @@ def main():
             plt.plot(xTrue[0, :], xTrue[1, :], "xr")
             plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
             for i in range(z.shape[0]):
-                plt.plot([xTrue[0, :], z[i, 1]], [
-                    xTrue[1, :], z[i, 2]], "-k")
+                plt.plot([xTrue[0, 0], z[i, 1]],
+                         [xTrue[1, 0], z[i, 2]],
+                         "-k")
             plt.title("Time[s]:" + str(time)[0: 4])
             plt.pause(0.1)
 

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/1.png

@@ -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/localization/histogram_filter_localization/2.png

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