*****************
Grid Localization
*****************

The Markov localization doesn't consider raw measurement.

The grid localization algorithm can solve global localization problem using raw measurement.

The algorithm uses a histogram filter:

   *  Take a continuous environment.
   *  Discretize it in many regions.
   *  Calculate the belief for each region.

The Algorithm
=============

The algorithm is similar to the Bayes filter:

   *  We want to maintain a belief :math:`b(x_t)`.
   *  As we use a grid :math:`b(x_t)= \{p_{k,t}\}`.
      
      where :math:`p_{k,t}` is the probability over a cell :math:`\mathbf{x}_k`.

   *  All legitimate poses forms a partition of the space:

      :math:`\text{domain}(X_t) = \mathbf{x}_{1,t} \cup \mathbf{x}_{2,t} \cup \dots \cup \mathbf{x}_{K,t}`


The figure below illustrates how we can create a domain of all poses possible:

.. figure:: ./img/space_discretisation.drawio.png
    :align: center

|

.. note::

    Usually, we choose a granularity of 15 cm for the :math:`x` and :math:`y` dimension and  :math:`5` degrees for the rotational dimension.


The pseudocode of the algorithm is:

.. figure:: ./img/grid_localization.png
    :align: center

|

The algorithm takes four parameters:

   *  The current belief :math:`b(x_t) = \{p_{k,t}\}`.
   *  The control action :math:`u_t`.
   *  The measurement :math:`z_t`.
   *  The map.

The prediction is done with the motion model of the robot, that we covered in a previous topic.

The prediction is updated using the measurement model seen previously.

.. important::

    The :math:`\text{mean}(\mathbf{x}_k)` is the center of a grid cell :math:`\mathbf{x}_k`.


Grid resolutions
================

A key in grid localization is the resolution of the grid.

It impacts:

   *  The type of sensor model we can use.
   *  The way the belief is calculated.
   *  The type of results we can expect.

A common representation is the metric representation:

   *  The state space is divided into fine-grained cells of uniform sized.
   *  Like mentioned before 15 cm is the norm.

If we take a previous example with the robot in a hallway, we can divide the hallway in small cells:

.. figure:: ./img/grid_localization_01.png
    :align: center

|

We receive a measurement :math:`z_t` that we use to update the belief:

.. figure:: ./img/grid_localization_02.png
    :align: center

|

Then the robot is executing an action, which leads to a new predicted belief:

.. figure:: ./img/grid_localization_03.png
    :align: center

|

Then, it receives a new measurement:

.. figure:: ./img/grid_localization_04.png
    :align: center

|

And it continues:

.. figure:: ./img/grid_localization_05.png
    :align: center

|

.. note::

    High-resolution sensor needs some particular process, as the :math:`p(z_t|x_t)` may vary drastically inside each cell :math:`\mathbf{x}_{k,t}`.

    .. figure:: ./img/precision_sensors_cell_size.png
        :align: center