Mobile Robot Localization

• Navigation is one of the most challenging competences required of a mobile robot.

• Success in navigation requires success at the four building blocks of navigation:
• perception (the robot must interpret its sensors to extract meaningful data);
• localization (the robot must determine its position in the environment;
• cognition (the robot must decide how to act to achieve its goals);
• motion control (the robot must modulate its motor outputs to achieve the desired trajectory).
• Of these four components , localization has received the greatest research attention in the
past decade, and as a result, significant advances have been made on this front.

Building Blocks of Navigation

The robot must have the ability to perceive the environment via its sensors in order to create
the proper data for finding its location (localization) and determining how it should go to its
destination in the produced map (path planning). The desired destination is found by the robot
through processing of the desired task/ mission command with the help of the cognition
process. The path is then provided as input to the robot’s motion controller which drives the
actuators such that the robot follows the commanded path.
The robot is operating in Real World Environment. The environment influences all components
of the navigation system. It is the source of all sensory data that feeds into perception. It
presents the physical challenges and obstacles that the robot must navigate around, which
impacts localization, cognition, and motion control. Changes in the environment (like moving
objects or alterations in the terrain) are detected by perception, requiring updates to
localization and adjustments in motion control.
Perception: Perception is used to interpret sensor data to create a representation of the
surrounding environment. Perception involves processing raw data from various sensors (like
cameras, LIDAR, infrared, or ultrasonic sensors) to detect objects, recognize patterns, and
assess environmental conditions. Advanced perception might also involve machine learning
to distinguish between different types of objects and predict their behavior. Perception data
assists in the localization process by providing landmarks and features that can be used to
 refine the robot's understanding of its position. The perceived information about the
environment is used to make real-time adjustments to the robot's movement to avoid obstacles
and adapt to changing conditions.
Localization: The Purpose of localization is to establish the robot's precise location within a
known or unknown environment. It typically uses various sensors to gather data and employs
algorithms to pinpoint the robot's location. Common methods include GPS (for outdoor
localization), odometry (calculating position based on wheel rotations), or more complex
approaches like SLAM (Simultaneous Localization and Mapping), where the robot builds a
map of an unknown environment while simultaneously keeping track of its position within that
map. The result of localization is usually a set of coordinates or a position vector that defines
the robot's location on a global map. This information is crucial for the robot to navigate
effectively and accurately.
Cognition: In cognition step involves plan and decide on the actions the robot should take to
achieve its goals. Cognition encompasses the path planning algorithms that take the robot's
current position and the intended destination to compute the optimal route. It includes
considering the global map and any constraints, such as no-go zones or preferred paths. The
main output is the "path" which is a series of waypoints or a continuous trajectory that the
robot should follow to reach its target destination.
Motion Control: Motion Control actuate the robot's movements in accordance with the planned
path and to ensure that movements are precise and stable. Motion control takes the path
generated by the cognition stage and translates it into commands for the robot's motors and
actuators. This component includes control systems and feedback loops that adjust the robot's
actions based on sensor data to handle real-world dynamics and kinematics. The motion
control system constantly receives data about the robot's interactions with the real world from
the perception system. This allows for dynamic adjustments to be made if the robot encounters
unexpected obstacles or changes in the terrain.

Mobile Robot Localization and Map building Techniques

• Mobile robot localisation
– Odometry, Dead reckoning
– Map based localisation
– Markov Localisation
– Kalman Filter
• Autonomous map building
– SLAM,
– EKF SLAM

Odometry and Dead reckoning:

Odometry:
• This is a method used to estimate the position and orientation of a robot by using data
from motion sensors(Encoders).
• For a wheeled robot, the data often comes from the rotation of its wheels.
• By tracking the number of wheel rotations,
and knowing the circumference of the wheels,
the robot can estimate how far it has moved.
• It can also track the difference in the number
of rotations between different wheels to
estimate its change in orientation.
• However, the odometry method tends to
accumulate errors over time.
• The longer the robot moves, the larger the
potential discrepancy between the estimated
position and the actual position, due to factors such as wheel slippage or uneven
terrain.

Odometry: Error sources

• Deterministic Errors:
• These are error’s whose values are definite and can be predicted or calculated.
• They follow a pattern and occur consistently.
• For Example: a bias in a measurement system could be a deterministic error. If a
scale always measures 1 kg heavier than the actual weight, that's a deterministic error:
it can be accurately quantified (as 1 kg) and it always happens in the same way.
• Non-Deterministic Errors:
• These are random errors that cannot be predicted or cannot be represented with a
simple deterministic model.
• They are usually due to unpredictable or unknown changes in the system or the
environment.
• For Example: measurement noise in a sensor, which can vary randomly due to
electrical noise or environmental factors.
Deterministic errors can be eliminated by proper calibration of the system. Non-
deterministic errors have to be described by error models and will always lead to
uncertain position estimate.
• Major Error Sources:
– Limited resolution during integration (time increments, measurement resolution)
– Misalignment of the wheels (deterministic)
– Unequal wheel diameter (deterministic)
– Variation in the contact point of the wheel (Non-deterministic)
– Unequal floor contact (slipping, not planar) (Non-deterministic)
 Dead Reckoning:
• It involves determining the current position based on a previously known position, and
then estimating the current position based on known or estimated speeds and course
over elapsed time.
• In robotics, dead reckoning often involves keeping track of the robot's motion (such as
velocity and direction) from internal sensors, like accelerometers or gyroscopes, and
integrating this data over time to estimate the robot's current position and orientation.
• Like odometry, dead reckoning is subject to accumulating errors over time.
Both odometry and dead reckoning are fundamental techniques used in mobile robot
localization, but they differ in scope and sensor reliance:
Odometry
• Odometry estimates a robot’s position and orientation by using data from motion sensors—
primarily wheel encoders in wheeled robots. By tracking the number of wheel rotations
and knowing the wheel circumference, the robot calculates its linear displacement.
Differences in the rotations of individual wheels also allow estimation of changes in
orientation.
• Characteristics:
o Provides incremental updates of position based solely on wheel rotations.
o Highly dependent on the mechanical design (wheel size, alignment, etc.).
o Susceptible to cumulative errors over time due to factors like wheel slippage,
uneven terrain, and deterministic (e.g., misalignment) or non-deterministic (e.g.,
slipping) error sources.
• Application:
Odometry is typically used for short-term, high-resolution motion tracking and forms the
backbone of many localization systems where the environment is predictable. It is often
integrated with other methods (e.g., Kalman filters, Markov localization) to correct its
inherent drift over time.
Dead Reckoning
• Dead reckoning is a broader navigation approach that estimates the current position by
integrating the robot’s known velocity and heading over time from a previously determined
position. Unlike pure odometry, dead reckoning can incorporate data from multiple sensors
such as accelerometers, gyroscopes, and sometimes even inertial measurement units
(IMUs) to update the pose.
• Characteristics:
o Combines various sensor inputs to continuously update the robot’s position.
o Not limited to wheel encoder data, making it more versatile in environments where
wheels might not be the sole source of movement (e.g., tracked or legged robots).
o Also accumulates errors over time, but the integration of additional sensor data can
sometimes help mitigate these errors.
• Application:
Dead reckoning is especially useful in scenarios where external positioning references
(like GPS or environmental landmarks) are unavailable or unreliable—such as in indoor
environments, underwater, or in cluttered spaces. It often serves as one component of a
sensor-fusion strategy in systems like SLAM (Simultaneous Localization and Mapping),
where its estimates are corrected by exteroceptive sensor data.
Map Based Localization:
Probabilistic, map-based localization is a fundamental problem in robotics, which involves
determining the position of a robot within an environment. The map-based part of the term
indicates that the robot has a map of its environment, and it uses this map along with sensor
readings to estimate its position. Probabilistic denotes the fact that uncertainties are involved in
this process, so the position is represented as a probability distribution over the set of all
possible positions.

There are several popular probabilistic map-based localization techniques:

1. Monte Carlo Localization (MCL), also known as Particle Filter Localization, is a popular
algorithm for localization. It represents the probability distribution of the robot's pose by a
set of particles. Each particle represents a potential position that the robot could be in. The
particles are weighted based on how consistent they are with sensor readings and are
resampled over time.
2. Kalman Filter Localization uses a mathematical framework to interpret sensor data and
estimate the robot's position and velocity, along with uncertainties. The filter assumes the
system is linear and noise is Gaussian.
3. Markov Localization, also known as Histogram Filter Localization, divides the map into
a grid and maintains a probability distribution over all grid cells. The robot's pose is
estimated to be in one of these cells.
4. Grid-based Localization also divides the map into a grid, but it is a simpler technique that
does not account for the direction the robot is facing.
5. Simultaneous Localization and Mapping (SLAM) is a more complex problem that
involves building a map of the environment while also estimating the robot's position
within the map. There are various techniques used to solve the SLAM problem, including
Extended Kalman Filters (EKF), FastSLAM, and GraphSLAM.

In all these methods, the robot uses its sensors to perceive the environment and estimate its
position within the environment. The goal is to make this estimate as accurate as possible,
despite various sources of uncertainty such as sensor noise and control errors.
 General Steps involved in Map based Localization

1. Prediction based on previous estimate and odometry:

• The robot uses its previous position estimate and combines it with odometry data (data
providing incremental changes in position over time) to predict its current position.
• This prediction takes into account motion models of the robot, which is essentially how
the robot moves (e.g., straight, turn, etc.).
• This step spreads out the belief about the robot's possible location.
2. Observation with on-board sensors:
• In this step, the robot collects sensory data from its on-board sensors.
• These could be a variety of sensors such as cameras, LiDAR, sonar, etc., depending
on the specific robot.
• The data provides the robot with information about its environment.
3. Measurement prediction based on prediction and map:
• It is about predicting what a robot's sensors should perceive given its estimated current
position and a known map of its environment.
• Given the predicted current position of the robot and the known map of the
environment, the robot predicts what the sensor readings should be.
4. Matching of observation and map:
• Here, the actual sensor observations are compared with the predicted position.
• The comparison gives a measure of how likely the current position estimate is, given
the sensor readings.
• If the actual and predicted position align well, this increases the likelihood of the robot
being at the estimated position.
5. Position update (Estimation) (posteriori position):
• The position estimate is then updated based on the likelihood computed in the
previous step.
• This is where the term "probabilistic" comes into play: if the observations are consistent
with multiple positions, then a probability distribution over those positions is
maintained.
• The updated position, often called the "posterior" position, is the result of combining
the prior belief (before considering the current sensor readings) with the likelihood
given the current sensor readings.

Challenges of Localization
If one could attach an accurate GPS (global positioning system) sensor to a mobile
robot, much of the localization problem would be obviated. The GPS would inform the
robot of its exact position, indoors and outdoors, so that the answer to the question,
“Where am I?” would always be immediately available. Unfortunately, such a sensor
is not currently practical. The existing GPS network provides accuracy to within several
meters, which is unacceptable for localizing human-scale mobile robots as well as
miniature mobile robots such as desk robots and the body-navigating nanorobots of
the future. Furthermore, GPS technologies cannot function indoors or in obstructed
areas and are thus limited in their workspace.
1. Sensor Noise: Sensors are the fundamental robot input for the process of
perception, and therefore the degree to which sensors can discriminate the world state
is critical. Sensor noise induces a limitation on the consistency of sensor readings in
the same environmental state and, therefore, on the number of useful bits available
from each sensor reading. Often, the source of sensor noise problems is that some
environmental features are not captured by the robot’s representation and are thus
overlooked.
Consider the noise level (i.e., apparent random error) of ultrasonic range-measuring
sensors (e.g., sonars). When a sonar transducer emits sound toward a relatively
smooth and angled surface, much of the signal will coherently reflect away, failing to
generate a return echo. Depending on the material characteristics, a small amount of
energy may return nonetheless. When this level is close to the gain threshold of the
sonar sensor, then the sonar will, at times, succeed and, at other times, fail to detect
the object. From the robot’s perspective, a virtually unchanged environmental state
will result in two different possible sonar readings: one short and one long. The poor
signal-to-noise ratio of a sonar sensor is further confounded by interference between
multiple sonar emitters. Often, research robots have between twelve and forty eight
sonars on a single platform. In acoustically reflective environments, multipath
interference is possible between the sonar emissions of one transducer and the echo
detection circuitry of another transducer. The result can be dramatically large errors
(i.e., underestimation) in ranging values due to a set of coincidental angles. Such
errors occur rarely, less than 1% of the time, and are virtually random from the robot’s
perspective. Sensor noise reduces the useful information content of sensor readings.
Clearly, the solution is to take multiple readings into account, employing temporal
fusion or multisensor fusion to increase the overall information content of the robot’s
inputs.

2. Sensor aliasing: Sensor aliasing in mobile robots refers to the limitation of

sensors that causes them to provide ambiguous or non-unique data about the
environment, making it challenging to accurately perceive and thus localize the robot.
This problem occurs when different environmental states produce identical or very
similar sensor readings, leading to confusion about the actual state of the environment.
In humans, each place usually looks different to our visual system, aiding our ability to
localize ourselves. However, if we're in an environment with little visual uniqueness,
such as a dark room or a hedge maze without distinctive landmarks, our ability to
determine our location deteriorates. For robots, sensor aliasing is common because
many sensors, like narrow-beam rangefinders (ultrasonic or infrared), only provide
limited information—like the distance to the nearest object in one direction—without
context such as color, texture, or material. Because of this, different environmental
setups might give the same sensor output, leading to a many-to-one mapping from the
environment to the sensor readings. For example, a robot using sonar might not
differentiate between a human and a cardboard box; both would appear as obstacles,
but the robot wouldn't have enough information to make a proper decision on whether
to interact or to navigate around the object. The problem posed to navigation because
of sensor aliasing is that, even with noise-free sensors, the amount of information is
generally insufficient to identify the robot’s position from a single-percept reading.
Thus, techniques must be employed by the robot programmer that base the robot’s
localization on a series of readings and, thus, sufficient information to recover the
robot’s position over time.

3. Effector noise: Effector noise refers to the inherent inaccuracies and

unpredictability in how a robot's actuators (effectors) perform movements or actions.
Just as sensors can provide noisy or unreliable data, effectors can also behave in
ways that are not entirely precise or consistent, even if the robot's control system
believes it has commanded a precise action.

Key points on effector noise:

Uncertainty in Action: A mobile robot's action (like moving forward) can have several
outcomes due to variations in actuator performance.
Increasing Uncertainty: Each movement a robot makes can actually increase the
uncertainty about its position because the actual movement may differ from the
intended movement due to effector noise.
Compensation with Cognition and Sensory Feedback: While effector noise
introduces uncertainty, robots can use smart planning and real-time sensory feedback
to mitigate this effect and sometimes even improve their state certainty.
 Odometry Error: Robots estimate their position over time through odometry, which
involves calculating movement based on wheel rotations and the robot's kinematics.
However, effector noise can make odometry unreliable.
Environmental Factors: Inaccuracies often stem from not accounting for
environmental factors like uneven floors, wheel slippage, or unexpected interactions
with people or objects that can affect the robot's motion.

In essence, effector noise means that a robot can't always trust its actuators to perform
exactly as commanded, which introduces a layer of unpredictability into the robot's
understanding of its own movements. This can make it difficult for the robot to know
its precise location over time without using additional strategies to compensate for
these inaccuracies.

Terminology used in Map Based localization

Robot State (𝒙𝒕 ) : This represents the robot's pose or configuration at a specific time 𝑡 . For a
mobile robot moving in a two-dimensional plane, this state is typically defined by three
parameters: its position along the x and y axes, and its orientation angle 𝜃.

Control Input / Proprioceptive Input (𝒖𝒕 ) : This refers to the robot's motion or the control
commands executed between time (𝑡 − 1) and time 𝑡. This data is typically gathered from the
robot's internal proprioceptive sensors, such as wheel encoders providing odometry readings.
It is utilized during the "prediction update" (or action update) phase to estimate where the robot
has moved based on its previous state.

Sensor Measurement / Exteroceptive Input (𝒁𝒕 ): This represents the observational data
gathered from the robot's external (exteroceptive) sensors at time 𝑡. Examples include distance
readings from a laser rangefinder or ultrasonic sensor, or visual features extracted from a
camera. These actual measurements are used during the "perception update" (or
measurement update) phase to correct the robot's predicted position.

Map (M) This denotes the known model or representation of the environment in which the robot
operates. Depending on the localization technique, the map can take several forms, such as a
continuous geometric description, a grid of fine-grained cells, a topological graph, or a
collection of specific geometric features and landmarks . The robot uses this map to calculate
its expected sensor readings and compares them against its actual sensor measurements to
accurately determine its probability of being in a specific location
 • The probabilistic robot localization process consists in the iteration of prediction
(action) update and Perception (measurement) updates.
• They compute the belief state that results, when new information (e.g., encoder
values and measurement data) is incorporated into a prior belief state with
arbitrary probability density.
Probability Basics
Markov localization
• Markov localization allows for localization starting from any unknown position and
can thus recover from ambiguous situations because the robot can track multiple,
completely different possible positions.
• However, to update the probability of all positions within the whole state space at
any time requires a discrete representation of the space, such as a geometric
grid or a topological graph .
• The required memory and computational power can thus limit precision and map
size.
• Markov localization, estimate the position and orientation of a robot in its
environment, given a map of the environment and sensor data.
• Markov localization tracks the robot’s belief state using an arbitrary probability
density function to represent the robot’s position
• In practice, every known Markov localization system uses this general belief
representation by dividing the robot's configuration space into a finite, distinct
number of possible robot positions (states) on the map.
• During each update, the probability for each state (element) of the entire space is
updated.
• In real-world applications, these potential positions can range from several
hundred to millions, each with varying orientations.
Markov localization involves following steps Prediction and measurement updates.
Prediction (action) update: In this phase the robot estimates its current position (i.e.
belief) based on the knowledge of the previous position (i.e., belief) and the
odometric input. As the robot moves, the algorithm predicts the new position based
on its motion model. The motion model describes how the robot moves and accounts
for potential errors like wheel slippage or uneven terrain, introducing some
uncertainty in the robot's location.
 • The theorem of total probability is used to compute the robot’s current
belief ̅̅̅̅̅̅̅̅̅̅
𝑏𝑒𝑙(𝑥𝑡 )as a function of the previous belief 𝑏𝑒𝑙(𝑥𝑡−1 )and the
proprioceptive data (e.g., the encoder measurement or the control input) 𝑢𝑡

̅̅̅̅̅(𝒙𝒕 ) that the robot assigns to the state 𝒙𝒕 is obtained by the

As observed, the belief 𝒃𝒆𝒍
integral (or sum) of the product of two distributions: the prior assigned to 𝒙𝒕−𝟏, and the
probability that the control induces a transition from 𝒙𝒕−𝟏 to 𝒙𝒕 .
Perception Update (Measurement Update)
• After the robot moves, it uses its sensors to observe the environment.
• These sensor readings are compared to the known map, and the estimated
position is updated based on how well the readings match the map, reducing
the location's uncertainty.
• Robot corrects its previous position itself by combining it with the information
from exteroceptive sensor.

• 𝑍𝑡 - Sensor Input
• M – Map
• The normalization factor η (often termed as the normalizing constant) is used
to ensure that the probabilities sum up to 1 after the update

Where
p(𝒛𝟏 𝒙𝟏 , 𝑴)
is the probabilistic measurement model, that is, the probability
of observing the measurement data , 𝒛𝒕 given the knowledge of the map and the robot
pose. Therefore, the new belief state is simply the product between the probabilistic
measurement model and the previous belief state.
Illustration of Markov localization
• For simplicity, our environment is a
one-dimensional hallway with three
identical pillars.
1. Initialization: At the very beginning, the
robot has no idea where it is. This is
represented by a uniform distribution
across the entire state space as shown
in fig(a), meaning every position is
equally likely.
2. Perception Update: As the robot
senses its environment, it updates its
belief based on the sensor input. For
instance, if it detects a column (as seen
in the fig.(b)), it increases the
probability of being in places where the
map also shows a column. This is also
known as the "sensing" or
"measurement update" phase. Peaks
form in the probability distribution at
positions that match the sensor
reading with the map.
3. Motion Update (Prediction): After
moving, the robot updates its belief to
reflect this motion. This is shown in the
fig.(c) Due to the uncertainty in motion,
this typically spreads out the probability distribution. This is because even if the
robot knows where it was before it moved, it cannot be sure it moved exactly as
intended due to slippage, wheel errors, etc.
4. Perception Update: The robot senses the environment again and updates its belief.
As shown in the fig.(d), if it senses something expected based on its prior motion
(like another column), the peaks in the probability distribution become sharper
around the expected new location.
5. Convergence: Finally, through multiple cycles of perception and motion updates,
the robot's belief starts to converge on its actual location. In the last panel, you can
see one sharp peak, which indicates that the robot is highly confident about its
location relative to the map.
Markov Localization: Case Study1
• Markov localization is implemented in practice using a grid-space representation
of the environment.
• The state space where the robot can be is divided into a grid of fine-grained cells
of uniform size. The robot's position is approximated to one of these cells,
simplifying the continuous space into a discrete set of possible locations.
• Robot configuration parameters: The robot's configuration in planar motion is
defined by three parameters: x and y coordinates for position, and θ (theta) for
orientation. Since the robot operates in a plane, it needs to consider its orientation
as well as its position.
• For the purpose of simplification, the here one-
dimensional environment is considered.
• Let us tessellate our environment into ten
equally spaced cells.
• Suppose that the robot’s initial belief 𝑏𝑒𝑙 (𝑥0 ) is
a uniform distribution from 0 to 3 as shown in
figure(a) with the equal probability of 0.25.
Prediction update: In this phase, the robot
moves and updates its belief using the
probabilistic motion model of the control input
(odometry).
• Let us assume that the probabilistic motion
model of the odometry p(𝑥1 𝑢1 , 𝑥0 ) is the one
represented in figure b.
• As per this model between time t=0 and time
t=1, the robot may have moved either two or
three units to the right. In this example, both
movements have the same probability to occur
(p=0.5) .
• What will the robot belief be after this
movement?

•
• Starting at 𝑥0 = 0 and moving two units (𝑢1 = 2). The probability of mobile robot at
𝑥1 = 0 is 0 and 𝑥1 = 1 𝑖𝑠 0.
• The probability of mobile robot at 𝑥1 = 2 is given by:

•
• The probability of the robot being at state 𝑥1 = 3 is the sum of two possibilities:
either the robot started at 𝑥0 = 0 and moved three units (𝑢1 = 3), or it started at
𝑥0 = 1 and moved two units (𝑢1 = 2).
• 𝑝(𝑥1 = 3) = 𝑝(𝑥0 = 0)𝑝(𝑢1 = 3) + 𝑝(𝑥0 = 1)𝑝(𝑢1 = 2)

= 0.25 × 0.5 + 0.25 × 0.5 = 0.25

• The robot can be at state 𝑥1 = 4 by moving 3 units from 𝑥0 = 1 or 2 units from 𝑥0 =
2 . Both probabilities contribute equally to the final probability
• 𝑝(𝑥1 = 4) = 𝑝(𝑥0 = 1)𝑝(𝑢1 = 3) + 𝑝(𝑥0 = 2)𝑝(𝑢1 = 2)
= 0.25 × 0.5 + 0.25 × 0.5 = 0.25
• The robot can be at state 𝑥1 = 5 by moving 3 units from 𝑥0 = 2 or 2 units from 𝑥0 =
3 . Both probabilities contribute equally to the final probability
• 𝑝(𝑥1 = 5) = 𝑝(𝑥0 = 2)𝑝(𝑢1 = 3) + 𝑝(𝑥0 = 3)𝑝(𝑢1 = 2)
= 0.25 × 0.5 + 0.25 × 0.5 = 0.25
• The robot can be at state 𝑥1 = 6 by moving 3 units from 𝑥0 = 3. The resulting
probability is
• 𝑝(𝑥1 = 6) = 𝑝(𝑥0 = 3)𝑝(𝑢1 = 3)
= 0.25 × 0.5 = 0.125
Measurement update.
• Let us now assume that the robot uses its onboard rangefinder and measures the
distance z from the origin. As per sensor data distance of the robot from the origin
can be equally 5 or 6 units as shown as a statistical error model of the range sensor
in fig (d).
• The robot updates its belief state 𝑏𝑒𝑙 (𝑥1 ) by incorporating the sensor reading
using Bayes rule.
• ̅̅̅̅ (𝑥1 ) (prior to
The updated belief 𝑏𝑒𝑙 (𝑥1 ) is the product of the prior belief 𝑏𝑒𝑙
incorporating the new measurement) and the likelihood of the measurement given
the map p(z1 ∣ x1, M).

• The normalization factor η (often termed as the normalizing constant) is used to

ensure that the probabilities sum up to 1 after the update. In this case the 𝜂 =
1
≅ 5.33 to make the final result 𝑏𝑒𝑙 (𝑥1 ) normalized to one as shown in fig.(e)
0.1875

• Final Belief state: After applying the normalizing constant, the final belief bel(x1)
is obtained, which is now a properly normalized probability distribution reflecting
the robot's updated position estimate after considering the measurement.
3D grid maps

• Markov Localization can be applied using

3D grid maps, which is a spatial
representation of the environment where
the robot operates.
• In the more general planar motion case, the
grid-map is a three-dimensional array
where each cell contains the probability of
the robot to be in that cell. The cell size must
be chosen carefully.
• Each cell in the grid map holds a probability
that the robot is present in that location
given all sensor readings s1,...,sn. The axes
x, y, and 𝜃 represent the robot's position
and orientation in the plane. The origin
(0,0,0) is typically considered the starting
point for localization.
• During each prediction and measurement
steps, all the cells are updated. If the number of cells in the map is too large, the
computation can become too heavy for real-time operations.
• To address this, an adaptive cell decomposition strategy is proposed, where the
grid map dynamically changes its resolution depending on the robot's certainty
about its location. In areas where the robot has a higher certainty, the grid cells are
smaller, allowing for finer localization. In contrast, in areas with lower certainty, the
grid cells are larger, reducing the computational load. This approach balances the
computational efficiency with the need for accuracy in the robot's localization.
• Self Study Topic: Markov localization using a topological map (Sec. [Link])
(Roland Siegwart)

Advantages of Markov Localization

• Markov Localization is a robust method for robot localization. It allows the robot to
handle a range of uncertainties and errors, making it very useful in real-world
applications.
• Over time, the uncertainty about the robot's location generally decreases, making
its position estimate more precise.
Kalman Filter Localization
• The Kalman filter is a mathematical mechanism for producing an optimal estimate
of the system state based on the knowledge of the system and the measuring
device, the description of the system noise and measurement errors and the
uncertainty in the dynamics models
• Kalman filter fuses sensor signals and system knowledge in an optimal way

The Kalman filter localization algorithm, or KF localization, is a special case of Markov

localization. Instead of using an arbitrary density function, the Kalman filter uses
Gaussians to represent the robot belief 𝑏𝑒𝑙(𝑥𝑡 ), the motion model, and the
measurement model. Because a Gaussian is simply defined by its mean 𝜇𝑡 and
covariance 𝛴𝑡 , only these two parameters are updated during the prediction and
measurement phase, resulting in a very efficient algorithm in comparison to Markov
localization algorithm. However, the assumptions made by the Kalman filter limit the
choice of the initial belief 𝒃𝒆𝒍(𝒙𝟎 ), also to a Gaussian, which means that the robot
initial location must be known with a certain approximation. Hence, the robot cannot
recover its position if it gets lost. This is in contrast with the Markov localization. The
Kalman filter therefore addresses the position-tracking problem but not the global
localization or the kidnapped robot problem.
Within the Kalman filter theory the system is assumed to be linear and with white
Gaussian noise. For most mobile robot applications, the system is nonlinear. In this
cases, the Kalman filter is usually applied after linearizing the system. The extension
of Kalman filter to nonlinear systems is known as the Extended Kalman Filter (EKF)
Illustration of Kalman Filter Localization
Consider mobile robot in a one-
dimensional environment. The
robot initial belief 𝒃𝒆𝒍(𝒙𝟎 ), is
represented by a Gaussian
distribution (figure a). we
assume that at the beginning
the robot is near the first pillar.
As the robot moves to the right
(we are in the action phase), its
uncertainty increases as a result
of the convolution with the
motion model (i.e., application of
the theorem of total probability).
The resulting belief is therefore
a shifted Gaussian of increased
width, (figure b). Now, suppose
that the robot uses its
exteroceptive sensors (we are in
the perception phase) and
senses that it is near the second
pillar. The posterior probability
of the observation is shown in
(figure c). This probability
density is again a Gaussian. In order to compute the robot current belief, we must fuse
this measurement probability with the robot’s belief before the observation using the
Bayes rule. The result of this fusion is again a Gaussian shown at the bottom of
figureThe variance of the resulting belief is smaller than the variances of both the
measurement probability and the robot’s previous belief
Assumptions: The Kalman filter operates under the assumptions that the system is
linear and that overall the robot configuration, the odometric error model, and the
measurement error model are affected by white Gaussian noise.
Gaussian Distribution: Gaussian distribution is characterized by its mean 𝝁𝒕 (first
moment) and variance 𝝈𝟐𝒕 (second moment). The Probability density function (pdf) for
a normal distribution, also known as the Gaussian distribution. This function is defined
as

For vector quantities, this extends to multivariate Gaussian distributions described by

mean a vector 𝜇𝑡 and covariance matrix 𝛴𝑡 . During the prediction and measurement
updates only mean 𝜇𝑡 and covariance 𝛴𝑡 are updated. Therefore, the Kalman filter is
based on four equations: two for updating 𝜇𝑡 and 𝛴𝑡 in the prediction update, and
another two in the measurement update. In Kalman filtering the measurement update
is also commonly called correction update. The prediction and measurement update
equations of Kalman filter are based respectively on the theorem of total probability
and on the Bayes rule.
Applying the Theorem of Total Probability
Consider two independent, normally distributed random variables 𝑥1 and 𝑥2 , with
respective distributions:

These variables represent different aspects of the system or measurement errors,

each characterized by a mean (μ) and variance (𝜎 2 ). Also if y is a linear function of
these variables 𝑥1 and 𝑥2 that is,
𝑦 = 𝑓(𝑥1 , 𝑥2 ) = 𝐴𝑥1 + 𝐵𝑥2
Here, A and B are constants and y is also normally distributed. This function represents
how different system states or measurements are combined in the prediction model.
For the linear combination of x1 and x2, the resulting variable y is also normally
distributed. The mean (⟨y⟩) and variance ( 𝜎𝑦2 ) of y can be calculated as follows:

If x1 and x2 are vectors with covariance matrices 𝛴1 and 𝛴2 , the equations adapt to
handle the vector and matrix operations, resulting in:

When the function f is nonlinear, the distribution of y is not normally distributed.

However, a common practice is to approximate f as a linear function around the means
μ1 and μ2 using a first-order Taylor expansion.
Applying the Bayes Rule:
Let 𝑞 denote the robot position,
𝑝1 (𝑞) the Robot’s position belief resulting from the prediction update. It's based on
the robot's previous state and motion.
𝑝2 (𝑞) the robot’s position belief resulting from some exteroceptive sensor
measurement such as a rangefinder, providing information about the robot's position
relative to a global reference frame.
Both beliefs are assumed to be normally distributed, characterized by their means
(𝑞̂1 and 𝑞̂2 ) and variances (𝜎12 and 𝜎22 ).
The Bayes rule tells us how to compute the final distribution of the robot’s belief after
the measurement has been taken.

According to the Bayes rule, the final

distribution , after the measurement,
is proportional to the product
𝑝1 (𝑞). 𝑝2 (𝑞)
1 (𝑞−𝑞1 )2
𝑝1 (𝑞) = 𝜎 𝑒𝑥𝑝 (− )
1 √2𝜋 2𝜎1 2

1 (𝑞−𝑞2 )2
𝑝2 (𝑞) = 𝜎 𝑒𝑥𝑝 (− )
2 √2𝜋 2𝜎2 2

From the product of the two density functions, 𝑝1 (𝑞). 𝑝2 (𝑞) we obtain:

The product of these two distributions, according to Bayes' rule, gives us a new
Gaussian distribution with its own mean and variance. This product is essentially the
convolution of the two distributions' PDFs.
The mean and variance of the resulting distribution are derived through the algebraic
manipulation of the exponential terms in the probability density functions (PDFs’)
product.
The new mean (𝑞̂) and variance (𝜎 2 ) can be expressed as:

These results incorporate the new sensor measurement and adjust the belief
accordingly.
 2
In Kalman filtering the factor 𝜎2𝜎+𝜎
1
2 is commonly called Kalman gain.
1 2

The Kalman filter effectively fuses multiple sources of information (predictions and
measurements) to produce an updated estimate that is better than any estimate
based on a single source of information. This Bayesian updating process is iterative.
With each new measurement, the Kalman filter refines its estimate of the state,
becoming more accurate and confident over time.
Application to Multi-Dimensional Systems
The equations can be extended for multi-dimensional systems where vectors and
matrices replace the scalar quantities.
The mean becomes a mean vector, and the variances become covariance matrices,
but the principles remain the same.
Thus, for 𝑛 dimensional vectors, the final mean 𝑞̂ and covariance 𝑃̂ after fusion can
be written respectively as:

where 𝑞1 and 𝑞2 represent different estimates of the state of a system.

P and R are the covariances of 𝑞1 and 𝑞2 respectively
𝑃̂ is the updated estimate of the covariance matrix
In Kalman filtering these equations are usually written as

Where: 𝐾 = 𝑃(𝑃 + 𝑅)−1 is known as Kalman Gain

(𝑞2 − 𝑞1 ) is known as innovation
𝛴𝐼𝑁 = (𝑃 + 𝑅) is known as Innovation Covariance

Simultaneous Localization and Mapping (SLAM)

Autonomous map building in mobile robotics refers to the process of creating a map of the
environment by a robot without relying on pre-existing maps or human intervention. SLAM
(Simultaneous Localization and Mapping) is a technique used to achieve autonomous map
building in mobile robotics.

SLAM allows a robot to simultaneously estimate its own position (localization) and construct
a map of the environment. The robot accomplishes this by using data gathered from its
proprioceptive and exteroceptive sensors, such as odometry and sensor measurements (e.g.,
laser, ultrasonic, or camera).
Using SLAM, the robot starts with an initial estimate of its position and the environment map.
As the robot moves, it collects sensor data and updates its position estimate based on odometry
information. The robot then observes features in the environment (e.g., corners, lines, planes)
using its exteroceptive sensors, and these features are mapped in relation to the robot's
estimated position.

However, SLAM faces challenges due to noise corruption in both the robot's estimated path
and the extracted features. The uncertainty in the robot's pose increases over time due to
odometry errors, and the uncertainty in mapping observed features arises from the combination
of measurement errors and the robot's pose uncertainty.
To address these challenges and reduce uncertainty, the robot seeks to observe features whose
locations are relatively well known. These features can be landmarks that the robot has already
observed before, and their observation is referred to as loop closure detection. When a loop
closure is detected, the robot's pose uncertainty decreases, and the map is updated. As a result,
the uncertainty of other observed features and all previous robot poses also reduces.

By iteratively updating the robot's position estimate and the environment map, SLAM enables
autonomous map building in mobile robotics. It allows the robot to navigate and explore
unknown environments while simultaneously constructing a map, without relying on external
maps or human intervention.

As shown in Figure Let us assume that the robot uncertainty at its initial location is zero. From
this position, the robot observes a feature which is mapped with an uncertainty related to the
exteroceptive sensor error model (a). As the robot moves, its pose uncertainty increases under
the effect of the errors introduced by the odometry (b). At this point, the robot observes two
features and maps them with an uncertainty that results from the combination of the
measurement error with the robot pose uncertainty (c). From this, we can notice that the map
becomes correlated with the robot position estimate. Similarly, if the robot updates its position
based on an observation of an imprecisely known feature in the map, the resulting position
estimate becomes correlated with the feature location estimate. In order to reduce its
uncertainty, the robot must observe features whose location is relatively well known. These
features can, for instance, be landmarks that the robot has already observed before. In this case,
the observation is called loop closure detection. When a loop closure is detected, the robot pose
uncertainty shrinks. At the same time, the map is updated and the uncertainty of other observed
features and all previous robot poses also reduce.

The SLAM (Simultaneous Localization and Mapping) algorithm can be implemented using
various techniques and approaches. Here are the general steps involved in SLAM:

1. Initialization: The SLAM process begins with initializing the robot's position estimate and
the environment map. The initial position estimate can be based on available information
or assumptions about the robot's starting location.
2. Sensor Data Acquisition: The robot starts moving through the environment, simultaneously
collecting sensor data from its proprioceptive and exteroceptive sensors. Proprioceptive
sensors provide information about the robot's own motion, such as odometry (e.g., wheel
encoders). Exteroceptive sensors capture data about the surrounding environment, such as
laser range finders, cameras, or ultrasonic sensors.
3. Prediction (Motion Model): Using the acquired proprioceptive sensor data (e.g., odometry),
the robot predicts its next position based on its motion model. The motion model represents
the expected movement of the robot based on its control inputs, taking into account factors
such as wheel rotations and kinematics.
4. Feature Extraction: The robot analyzes the data obtained from exteroceptive sensors to
extract features from the environment. Features can include landmarks, edges, corners, or
any distinctive elements that can be reliably detected and used for mapping.
5. Data Association: In this step, the robot associates the extracted features with the
corresponding features in the map to establish correspondences. The goal is to identify
which observed features in the current frame match previously mapped features in the
environment.
6. Update (Measurement Update): The robot updates its position estimate and the map based
on the observed features and their associations. This step involves applying sensor fusion
 techniques, such as Kalman filtering or particle filtering, to incorporate the new information
and reduce uncertainty in both the robot's pose and the map.
7. Loop Closure Detection: The robot looks for instances where it revisits a previously
observed location, known as a loop closure. By detecting loop closures, the robot can refine
its pose estimate and reduce accumulated errors. Loop closures can be identified by
matching features observed in different parts of the environment.
8. Map Optimization: Once loop closures are detected, the map and the robot's pose estimates
undergo optimization. Optimization techniques, such as Graph-SLAM or Bundle
Adjustment, are used to refine the map and improve the overall consistency of the estimated
robot poses.
9. Repeat: The process of motion, feature extraction, data association, update, loop closure
detection, and map optimization continues as the robot explores the environment. With
each iteration, the robot refines its position estimate and map, progressively reducing
uncertainty.
10. Termination: The SLAM process continues until the robot completes its mapping task or
reaches a specified stopping criterion. The final output is an optimized map of the
environment and an accurate trajectory of the robot's path.