Introduction

Simultaneous Localization and Mapping is exactly what it sounds like. Consider a mobile robot moving through some 2D or 3D environment. It takes relative measurements of its location relative to its sensor data. The issue the robot faces is that it does not have access to its map nor its location, but it would like to simultaneously construct a map of its environment and localize itself within that map.

Definition

Given the dynamics $$\begin{align*} x_{t+1}&=f(x_{t},u_{t},w_{t})\\ m_{t+1}&=m_{t} \\ \mathbf{y}_{t}:&=\{ y_{t}^{1},\dots,y_{t}^{N} \} \\ y^{i}_{t}&=g(x_{t},m^{i}_{t},v_{t})\\ \end{align*}$$where $x_{t+1}$ is our state at time $t+1$, $m_{t+1}$ is our map at time $t+1$, and $\mathbf{y}_{t}$ is our set of observations at time $t$ and $y_{t}^{i}$ is our observation of the $i^{th}$ element in our map at time $t$. Mathematically, we can model our robot as a POMDP so we consider a 6 tuple: $(\mathbb{S},\mathbb{U},\mathbb{Z}, \mathcal{T},\mathcal{Q},c)$ where

• $\mathbb{S}:=\mathbb{X}\times \mathbb{M}$ is the joint state space where $\mathbb{X}$ is the Pose of the robot and $\mathbb{M}$ is the space of landmarks.

• $\mathbb{U}$ is the action space.

• $\mathbb{Y}$ is the observation space. This represents our sensor measurements.

• $\mathcal{T}:\mathbb{X}\times \mathbb{U}\to [0,1]$ is the transition kernel i.e. $$\begin{align*} \mathcal{T}(A\mid x_{t},u_{t})&=P(x_{t+1}\in A\mid x_{t},u_{t})\quad&x_{t},x_{t+1},A\in\mathbb{X}, u_{t}\in\mathbb{U} \end{align*}$$also referred to as the motion model.

• $Q:\mathbb{S}\to [0,1]$ is the conditional likelihood of the observation i.e. $$\begin{align*} Q(A\mid s_{t})=P(y_{t}\in A\mid s_{t})=P(y_{t}\in A\mid x_{t}, m_{t})\quad s_{t}\in\mathbb{S},x_{t}\in\mathbb{X},m_{t}\in\mathbb{M},y_{t},A\in\mathbb{Y} \end{align*}$$

• $c:\mathbb{S}\times \mathbb{U}\to \mathbb{R}$ is the cost function.

  ---

  We can reduce this into a Belief MDP $(\mathcal{P}(\mathbb{S}),\mathbb{U},\eta,\tilde{c})$ where Let us look at constructing the filter process: $$\begin{align*} \pi_{t}(s_{t}):&=P(s_{t}\mid y_{[0,t]}, u_{[0,t-1]})\\ &=\frac{P(s_{t},y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]})}{P(y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]})}\\\\ &=\frac{P(s_{t},y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]})}{\int\limits _{\mathbb{S}}P(s_{t},y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t} }\\\\ &=\frac{\int\limits _{\mathbb{S}}P(s_{t},s_{t-1},y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t-1} }{\int\limits _{\mathbb{S}}\int\limits _{\mathbb{S}}P(s_{t},s_{t-1},y_{t},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t} \, ds_{t-1} }\\\\ &=\frac{\int\limits _{\mathbb{S}} P(y_{t}\mid s_{t})P(s_{t},s_{t-1},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t-1} }{\int\limits _{\mathbb{S}}\int\limits _{\mathbb{S}}P(y_{t}\mid s_{t})P(s_{t},s_{t-1},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t} \, ds_{t-1} }\\ \\ &=\frac{\int\limits _{\mathbb{S}} P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})P(s_{t-1},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t-1} }{\int\limits _{\mathbb{S}}\int\limits _{\mathbb{S}}P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})P(s_{t-1},u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]}) \, ds_{t} \, ds_{t-1} }\\ \\ &=\frac{\int\limits _{\mathbb{S}} P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})P(u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]})\pi_{t-1}(s_{t-1}) \, ds_{t-1} }{\int\limits _{\mathbb{S}}\int\limits _{\mathbb{S}}P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})P(u_{t-1}\mid y_{[0,t-1]}, u_{[0,t-2]})\pi_{t-1}(s_{t-1}) \, ds_{t} \, ds_{t-1} }\\ \\ &=\frac{\int\limits _{\mathbb{S}} P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})\pi_{t-1}(s_{t-1}) \, ds_{t-1} }{\int\limits _{\mathbb{S}}\int\limits _{\mathbb{S}}P(y_{t}\mid s_{t})P(s_{t}\mid s_{t-1},u_{t-1})\pi_{t-1}(s_{t-1}) \, ds_{t} \, ds_{t-1} }\\ \\ &=\frac{\int\limits _{\mathbb{X}\times \mathbb{M}} P(y_{t}\mid s_{t})P(x_{t}\mid x_{t-1},u_{t-1})P(m_{t}\mid m_{t-1})\pi_{t-1}(x_{t-1},m_{t-1}) \, d(x_{t-1}\times m_{t-1}) }{\int\limits _{\mathbb{X}\times \mathbb{M}}\int\limits _{\mathbb{S}} P(y_{t}\mid s_{t})P(x_{t}\mid x_{t-1},u_{t-1})P(m_{t}\mid m_{t-1})\pi_{t-1}(s_{t-1}) \, ds_{t} \, ds_{t-1} }\\ \\ \end{align*}$$