Robot Localization

Robot localization problems can differ substantially depending on how much is known about the initial pose, how the environment evolves, how actions are chosen, and how many robots are involved. Before turning to specific algorithms, it is helpful to organize these variations using a taxonomy.

We will use the term pose to refer to the robot’s position and orientation in a global coordinate frame. For a planar mobile robot, the pose is typically $x_t=(x_t,y_t,{\theta }_t)$ at time $t$, where $(x,y)$ is the planar position and $\theta$ is the heading angle.

In previous chapters, we introduced several well-known variations of the Bayes filter, including the parametric EKF and the non-parametric particle filter in Chapter 12. These algorithms propagate a belief distribution over a state ${𝒙}_t$ using a probabilistic Markov state transition model and a probabilistic measurement model. Recall that for a general state-space model, the belief at time $t$ is defined as:

where ${𝒛}_{1:t}$ are all measurements up to time $t$ and ${𝒖}_{1:t}$ are the applied controls.

In map-based localization, the state ${𝒙}_t$ represents the robot pose, and we assume that a map $𝒎$ of the environment is given. The map encodes information about the environment that constrains both how the robot can move and what it can measure. Our goal is to compute the posterior over robot poses conditioned on the map:

To incorporate the map into the filtering framework, we modify both the state transition model and the measurement model. Specifically, in Section 13.3, we introduce a map-aware state transition model $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t,𝒎)$ that captures the fact that some poses are inconsistent with the environment. Then, in Section 13.3.1, we introduce a map-aware measurement model $p({𝒛}_t\mid {𝒙}_t,𝒎)$ that reflects how local sensor readings depend on nearby structures in the map. The combination of these two models leads directly to the localization algorithms we introduce in Section 13.5 and Section 13.6.

The map influences how the robot can move. For example, a mobile robot cannot pass through walls or leave the known workspace. A purely kinematic transition model $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t)$ does not capture these constraints: it might assign nonzero probability to states that lie inside obstacles or outside the mapped area.

To incorporate the map, we consider the map-aware transition model $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t,𝒎)$. Directly modeling this distribution for arbitrary maps is difficult, so we adopt an approximation that reuses the original transition model and a separate, map-induced prior. Specifically, we define $p({𝒙}_t\mid 𝒎)$ as the probability of a pose given only the map. This term characterizes how consistent a pose is with the environment. For example, $p({𝒙}_t\mid 𝒎)=0$ for a pose inside a wall or outside the mapped region.

Using Bayes’ rule, we can approximate the map-aware transition model as:

where $\eta$ is a normalization constant that ensures the left-hand side integrates to one. Equation 13.2 can be derived step-by-step by starting from Bayes’ rule:

Since $p(𝒎\mid {𝒙}_{t-1},{𝒖}_t)$ does not depend on ${𝒙}_t$, we can absorb it into a constant ${\eta }^{\prime }$:

Next, we assume that the influence of the past on the map is captured entirely through the current pose ${𝒙}_t$:

Intuitively, once we know the current pose, the specific path taken to get there does not provide additional information about the static map.¹¹1This approximation becomes more accurate as the time step between $t-1$ and $t$ shrinks, so that the robot moves only a short distance between successive poses. With this assumption:

Applying Bayes’ rule again to $p(𝒎\mid {𝒙}_t)$ gives:

Substituting into the previous expression and absorbing constant factors that do not depend on ${𝒙}_t$ into a new normalization constant $\eta$ yields:

which is Equation (Equation 13.2).

In this approximation, $p({𝒙}_t\mid 𝒎)$ is the probability of a state given only the map and captures the geometric consistency of that state with respect to the environment^††margin: For example, $p({𝒙}_t\mid 𝒎)=0$ for a state ${𝒙}_t$ that lies inside a wall or outside the known map. . The factor $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t)$ encodes the motion model, and $p({𝒙}_t\mid 𝒎)$ acts as a map-based correction that suppresses physically implausible states.

From a computational perspective, Equation (Equation 13.2) is attractive because it allows us to reuse existing implementations of the motion model and incorporate the map through a relatively inexpensive correction:

• •

  We first generate a kinematic prediction of the new pose using $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t)$, which depends only on the robot model and control.

• •

  We then reweight or prune these predictions using $p({𝒙}_t\mid 𝒎)$, which often reduces to simple operations such as checking whether the pose lies in free space (for occupancy grids) or near a corridor (for semantic maps).

Thus, the map-aware transition model modifies the likelihood of proposed poses without requiring a complete redesign of the underlying motion model.

The first map-based localization algorithm we introduce is Markov localization. Markov localization applies the Bayes filter from Chapter 11 to the map-aware models introduced above. The belief over robot pose at time $t$ is:

and the map $𝒎$ is treated as known and fixed.

Using the map-aware transition and measurement models $p({𝒙}_t\mid {𝒙}_{t-1},{𝒖}_t,𝒎)$ and $p({𝒛}_t\mid {𝒙}_t,𝒎)$, the Bayes filter recursion becomes:

where $\overline{\text{bel}}({𝒙}_t)$ denotes the predicted belief and $\eta$ is a normalization constant chosen so that $\int \text{bel}({𝒙}_t)𝑑{𝒙}_t=1$. We summarize this recursion in Algorithm 1.

Conceptually, Markov localization has the same structure as the Bayes filter from Chapter 11: a prediction step that propagates the belief through the motion model, followed by a correction step that incorporates the latest measurement. The only difference is that both the prediction and correction now depend explicitly on the map $𝒎$.

In its most general form, however, Markov localization is not directly computationally tractable. The integral over all possible poses and the need to maintain an arbitrary belief function are prohibitive in high-dimensional or continuous state spaces. As in Chapter 12, the key to practicality is to choose a representation for the belief that is expressive enough for the problem at hand, yet structured enough to admit efficient computation.

In Section 13.5, we adopt a unimodal Gaussian belief and derive an EKF-based localization algorithm, which is well suited for pose tracking with moderate uncertainty. In Section 13.6, we adopt a particle-based representation and derive Monte Carlo Localization, capable of handling global localization and multimodal beliefs.

We now develop an EKF-based realization of Markov localization. The EKF localization algorithm assumes that the belief over the robot’s pose can be approximated by a single Gaussian:

and applies the EKF prediction and update equations from Chapter 12 using map-aware models. This Gaussian structure significantly improves computational efficiency relative to the full Markov localization algorithm, at the cost of not being able to represent multiple well-separated pose hypotheses.²²2As in Chapter 12, this unimodality assumption is well suited to pose tracking but generally insufficient for global localization.

We assume the same nonlinear state transition model as in the referenced section:

where ${\mathit{ϵ}}_t\sim 𝒩(\mathrm{𝟎},Q_t)$ is zero-mean Gaussian process noise. The corresponding state transition Jacobian is:

where ${𝝁}_{t-1}$ is the mean of the previous belief $\text{bel}({𝒙}_{t-1})$.

The main difference between the general EKF and EKF localization is the presence of a feature-based map and a measurement model that relates robot poses to observed landmarks. We assume we have a map $𝒎$ of $N$ point landmarks:

where each landmark $m_j$ is given by its two-dimensional location $(m_{j,x},m_{j,y})$ in the global coordinate frame. At time $t$ the robot obtains a set of landmark measurements:

where each ${𝒛}_t^i$ is associated (implicitly or explicitly) with one landmark.

Given a pose ${𝒙}_t$ and the index $j$ of the corresponding landmark, the measurement model is:

where ${𝜹}_t\sim 𝒩(\mathrm{𝟎},R_t)$ models zero-mean Gaussian sensor noise, and $h(\cdot )$ encodes the expected range, bearing, or other features to landmark $m_j$. For each measurement, the Jacobian of the measurement model with respect to the state is:

where ${\overline{𝝁}}_t$ is the predicted mean from the EKF prediction step, and $c_t^i$ is the index of the map feature associated with measurement $i$.

A new challenge in feature-based localization is data association. Given a set of measurements ${𝒛}_t$, which landmark does each measurement correspond to? We denote the correspondence for measurement $i$ at time $t$ by $c_t^i\in \{1,\mathrm{\dots },N+1\}$, where $c_t^i=j$ means that measurement $i$ corresponds to landmark $m_j$, and $c_t^i=N+1$ indicates that measurement $i$ does not correspond to any known landmark^††margin: For example, due to a spurious detection. .

We first consider the simpler case where we assume the correspondences $c_t^i$ are known.

The second practical realization of Markov localization we consider is Monte Carlo localization (MCL). This algorithm leverages the non-parametric particle filter framework from Chapter 12 and is particularly well suited to global pose localization and the kidnapped robot problem.^††margin: We can also use MCL to solve the kidnapped robot problem by injecting new randomly sampled particles at each step so that the filter remains sensitive to unexpected measurements.

Like the particle filter, MCL represents the belief distribution $\text{bel}({𝒙}_t)$ by a set of $K$ particles:

where each particle ${𝒙}_t^{[k]}$ represents a hypothesis about the true state ${𝒙}_t$. Regions of the state space with more particles correspond to higher belief.

At each step of the algorithm, we:

1. 1.

   propagate particles forward using the map-aware state transition model, and

2. 2.

   weight and resample the particles using the map-aware measurement model.

This is summarized in Algorithm 4, which is nearly identical to the particle filter algorithm in the referenced algorithm, the only difference being that the map $𝒎$ now appears in the probabilistic state transition and measurement models.

In global localization, MCL typically starts from a broad prior, such as having particles spread over the entire map. As the robot moves and gathers measurements, inconsistent hypotheses are down-weighted and vanish during resampling, while particles near the true pose accumulate. This makes MCL a powerful tool for environments with strong ambiguities, repeated structures, or very uncertain initial conditions.

In this chapter, we examined the fundamental concepts and algorithms underlying robot localization—the process of estimating a robot’s pose relative to a known map of the environment. We began with a taxonomy of localization problems, distinguishing between pose tracking and global localization, static and dynamic environments, active and passive strategies, and single-robot versus multi-robot settings. This taxonomy provides a conceptual checklist for matching real-world scenarios to appropriate algorithmic tools.

Building on the Bayesian estimation framework introduced earlier, we extended the state transition and measurement models to explicitly incorporate map information. This led to the Markov localization algorithm, a general Bayesian filter that serves as a conceptual foundation for map-aware localization.

We then explored two important realizations of this framework: EKF localization, which represents the belief as a unimodal Gaussian and is effective for pose tracking with moderate uncertainty; and MCL, which uses a particle-based, non-parametric representation suitable for multi-hypothesis and global localization problems. We discussed data association—the challenge of determining correspondences between sensor measurements and known map features—and techniques such as maximum-likelihood matching and validation gates to improve robustness.

Together, these methods form the core of modern localization systems, enabling robots to estimate their position and orientation using noisy sensors, partial maps, and uncertain environments. In later chapters, these ideas will reappear in more complex settings such as simultaneous localization and mapping (SLAM), where the robot must estimate both its pose and the map at the same time.

The starter code for the exercises provided below is available online through GitHub. To get started, download the code by running in a terminal window:

We denote Problems requiring hand-written solutions and coding in Python with and , respectively.

In this problem, you will implement an extended Kalman filter (EKF) for robot localization in an environment where the robot can collect relative position measurements to a set of four landmarks. We will consider a robot with a discrete-time dynamics model ${𝒙}_{t+1}=f({𝒙}_t,{𝒖}_t)+{\mathit{ϵ}}_t$ defined by:

and the goal is to estimate the unknown robot pose, ${𝒙}_t={[x_t,y_t,{\theta }_t]}^{\top }$. The noise vector ${\mathit{ϵ}}_t={[{ϵ}_t^x,{ϵ}_t^y,{ϵ}_t^{\theta }]}^{\top }$ is a random variable with a zero mean Gaussian distribution ${\mathit{ϵ}}_t\sim 𝒩(\mathrm{𝟎},Q)$, where $Q=0.1\mathrm{\Delta }{t}^{2}I$. In this problem, we assume the global ground truth positions of the four landmarks are known. The landmarks are stationary objects in the environment, and their combined state vector is:

As the robot navigates through its environment, it receives noisy measurements of the positions of four landmarks in the environment relative to the robot’s current pose. The measurement for landmark $i$ is the relative position with the measurement model:

where the measurements have an associated measurement noise with ${𝜹}_t\sim 𝒩(\mathrm{𝟎},R)$, where $R=0.25I$. The full measurement vector of all landmarks is:

In the file ch13/exercises/ekf_localization.ipynb, complete the following:

1. 1.

   Implement the functions robot_dynamics and robot_measurement to match the dynamics and measurement models described above.

2. 2.

   Implement the function dynamics_jacobian to compute the dynamics Jacobian $F_t={\nabla }_{𝒙}f({𝒙}_t,{𝒖}_t)$.

3. 3.

   Implement the function measurement_jacobian to compute the measurement model Jacobian $H_t={\nabla }_{𝒙}h({𝒙}_t,𝒎)$ for the model that computes the full measurement vector ${𝒛}_t$.

4. 4.

   Implement the function ekf_localization_update to implement the EKF localization update described in Algorithm 2. Note that you won’t need to explicitly have a for loop in this function for each measurement since we have defined the update to be vectorized for all measurements at once.

5. 5.

   Run the provided code to see how the algorithm performs for the simulated robot.

In this problem, we consider the same problem setup defined in Problem 1, where we are localizing a robot given relative position measurements to a set of four known landmarks. In the file ch13/exercises/particle_filter_localization.ipynb, complete the following:

1. 1.

   Implement the functions robot_dynamics and robot_measurement to match the dynamics and measurement models described above. Note: you can reuse your solution from the previous problem.

2. 2.

   Implement the particle filter algorithm update function particle_filter_update as described in Algorithm 4.

3. 3.

   Run the provided code to see how the algorithm performs for the simulated robot.