{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"colab_type": "text",
"id": "view-in-github",
"tags": [
"no-tex"
]
},
"source": [
""
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "JoW4C_OkOMhe",
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install -q -U gtbook\n"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "10-snNDwOSuC",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"import numpy as np\n",
"from numpy.random import default_rng\n",
"rng = default_rng()\n",
"\n",
"import plotly.express as px\n",
"import plotly.graph_objects as go\n",
"try:\n",
" import google.colab\n",
"except:\n",
" import plotly.io as pio\n",
" pio.renderers.default = \"png\"\n",
"\n",
"import gtsam\n",
"from gtbook import logistics\n",
"from gtbook.display import show\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {
"id": "8m7rpauo_4qD"
},
"source": [
"# Localization\n",
"\n",
"> We introduce three variations of Bayes filtering to solve the robot localization problem: Markov localization, and Monte Carlo localization, and Kalman filtering.\n",
"\n",
"
"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Localization** is the process of estimating the robot's position using sensor data\n",
"and the history of executed actions.\n",
"In this chapter, the robot's position at time $k$ is denoted by $x_k$, so the localization problem\n",
"is to estimate $x_k$ at each time instant using the robot's history of sensor observations,\n",
"$z_1, \\dots z_k$, and commanded actions $u_1, \\dots , u_{k-1}$.\n",
"\n",
"As we have seen in Chapter 3, when we are able to exploit the Markov property, we need\n",
"not consider the entire sensor and action history at each time $k$; intstead, we can iteratively\n",
"compute a probability distribution for the state $x_k$ using only our belief about the state $x_{k-1}$,\n",
"the action $u_{k-1}$, and the sensor observation $z_k$.\n",
"This kind of iterative, online updating is referred to as filtering.\n",
"The input to the filter is the belief at time $k-1$, the action $u_{k-1}$, and the observation $z_k$.\n",
"The output of the filter is the new belief, a probability distribution for $x_k$.\n",
"Note that when taking this approach we abandon computing the full joint distribution\n",
"$P(X_1, \\dots X_k)$, and content ourselves to compute only the conditional distribution\n",
"for $X_k$ at time $k$.\n",
"In this chapter, the filters that we develop will be derived from Bayes theorem,\n",
"and the result is known as **Bayes filtering**.\n",
"\n",
"We begin the section with a general introduction to Bayes filters, and then develop\n",
"three specific algorithms, Markov localization, and Monte Carlo localization, and Kalman filtering.\n",
"These three algorithms reflect trade-offs in computational complexity versus accuracy and expressive\n",
"power. In particular, Markov localization relies on a grid-based representation that\n",
"can require significant computer memory as well as significant computation in the update\n",
"process.\n",
"Monte Carlo localization addresses each of these concerns by using a sampling-based approach,\n",
"at the expense of accuracy.\n",
"Kalman filters, on the other hand, provide an exact and optimal solution to the localization\n",
"problem, by only in the special case when the robot can be described as a linear system\n",
"and all uncertainties in motion and observation are Gaussian in nature."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## A Running Example\n",
"\n",
"When introducing concepts related to localization, it will be useful to \n",
"have a short, ground truth trajectory for the robot that we can use to assess the quality of\n",
"solutions.\n",
"For this purpose,\n",
"consider a trajectory that starts at the bottom left of the map,\n",
"moves to the right, then up between the middle two shelves:"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"
Variable | value |
---|---|
x1 | 10 6 |
x2 | 15 6 |
x3 | 20 6 |
x4 | 25 6 |
x5 | 30 6 |
x6 | 35 6 |
x7 | 40 6 |
x8 | 45 6 |
x9 | 50 6 |
x10 | 50 11 |
x11 | 50 16 |
x12 | 50 21 |
x13 | 50 26 |
x14 | 50 31 |