Recursive State Estimation

This section develops the Bayes filter, the recursive state estimator introduced in the chapter overview, and works it through on a simple example.

​

The Bayes filter

Consider an embodied agent, such as a robot, that moves in an environment. The agent never sees the state $s_t$. It receives measurements $z_t$, takes actions $a_t$, and keeps a belief $\mathtt{bel}(s_t)$ about the state. The state of the environment contains dynamic variables, such as the agent’s pose (position and orientation, 6D in general), its velocity and the poses of other objects. It also contains static variables, such as the locations of obstacles and walls. Most practical state estimation algorithms make the Markov assumption: the current state $s_t$ contains all the information about the past that is relevant for predicting the future. Given $s_{t-1}$ and the latest action, earlier states add no information about $s_t$. This assumption is what makes the algorithms tractable. The assumption also covers processes that depend on more than one past state, because you are free to choose what the state contains. If the next state depends on the two most recent states (a second-order Markov process), define the augmented state $\tilde{s}_t = [s_t, s_{t-1}]$. The process is then first-order Markov in $\tilde{s}_t$. The dynamic Bayesian network below encodes the Markov assumption. Dynamic Bayesian network of states $s$, actions $a$ and measurements $z$. Shaded nodes are observed. Write $z_{t_1:t_2}$ for the measurements acquired from time $t_1$ to time $t_2$, and similarly $s_{t_1:t_2}$ and $a_{t_1:t_2}$. The graph then factorizes as follows: $$\begin{aligned} p(s_t \mid s_{0:t-1}, z_{1:t-1}, a_{1:t}) &= p(s_t \mid s_{t-1}, a_t) \\ p(z_t \mid s_{0:t}, z_{1:t-1}, a_{1:t}) &= p(z_t \mid s_t) \end{aligned}$$ The state transition model $p(s_t \mid s_{t-1}, a_t)$ gives the distribution of the new state after the agent executes the control action $a_t$. The measurement model $p(z_t \mid s_t)$ gives the distribution of the measurement for each state. Both are usually taken to be the same at every time step. By convention, the agent first executes the action $a_1$ and then takes the measurement $z_1$. The belief is the posterior distribution over the state $s_t$, conditioned on all past measurements and actions: $$\mathtt{bel}(s_t) = p(s_t \mid z_{1:t}, a_{1:t})$$ It is also useful to define the belief just after the action $a_t$, but before the measurement $z_t$ is taken into account: $$\widehat{\mathtt{bel}}(s_t) = p(s_t \mid z_{1:t-1}, a_{1:t})$$ The Bayes filter is a recursive algorithm with two steps:

1. The prediction step computes $\widehat{\mathtt{bel}}(s_t)$ from the previous belief $\mathtt{bel}(s_{t-1})$ and the transition model.
2. The measurement update step weighs $\widehat{\mathtt{bel}}(s_t)$ by the probability $p(z_t \mid s_t)$ of the measurement that was actually observed.

One step of the filter takes the previous belief $\mathtt{bel}(s_{t-1})$, the action $a_t$ and the measurement $z_t$, and returns the new belief $\mathtt{bel}(s_t)$. The normalizer $\eta$ makes $\mathtt{bel}(s_t)$ sum (or integrate) to one over all $s_t$. $$\begin{array}{lll} 1: & \textbf{for all } s_t \textbf{ do} & \\ 2: & \qquad \widehat{\mathtt{bel}}(s_t) = \int p(s_t \mid a_t, s_{t-1})\, \mathtt{bel}(s_{t-1})\, ds_{t-1} & \text{prediction} \\ 3: & \qquad \mathtt{bel}(s_t) = \eta\, p(z_t \mid s_t)\, \widehat{\mathtt{bel}}(s_t) & \text{measurement update} \\ 4: & \textbf{end for} & \\ 5: & \textbf{return } \mathtt{bel}(s_t) & \end{array}$$ The Bayes filter. For a discrete state, the integral in line 2 is a sum over $s_{t-1}$.

​

Door state estimation

This example is from Thrun, Burgard and Fox, Probabilistic Robotics (MIT Press, 2005), section 2.4.2. A robot with a camera has to estimate whether a door is open or closed. The door has two possible states, open and closed. The robot does not know the initial state, so its initial belief is uniform: $$\begin{aligned} \mathtt{bel}(s_0 = \mathtt{open}) &= 0.5 \\ \mathtt{bel}(s_0 = \mathtt{closed}) &= 0.5 \end{aligned}$$

​

Measurement model

No real sensor is perfect. Assume the measurement model is: The asymmetry is realistic. A closed door is easier to recognize than an open one. Through an open door, a camera sees the clutter of the room behind it, and the LIDAR returns from that clutter can be classified as a closed door.

​

Transition model

The robot can use its manipulator to push the door, or it can do nothing. The transition model is: Recall the convention: the robot first acts and then senses. Reversing the order gives the same equations with shifted indices. At $t = 1$, the robot does nothing and then senses that the door is open. The two steps of the Bayes filter are as follows.

​

Prediction at time 1

The state is discrete, so the integral in the prediction step becomes a sum. In the second line, $p(s_1 \mid \mathtt{nothing}, \mathtt{open})$ is short for $p(s_1 \mid a_1 = \mathtt{nothing}, s_0 = \mathtt{open})$. $$\begin{aligned} \widehat{\mathtt{bel}}(s_1) &= \sum_{s_0} p(s_1 \mid a_1, s_0)\, \mathtt{bel}(s_0) \\ &= p(s_1 \mid \mathtt{nothing}, \mathtt{open})\, \mathtt{bel}(s_0 = \mathtt{open}) \\ &\quad + p(s_1 \mid \mathtt{nothing}, \mathtt{closed})\, \mathtt{bel}(s_0 = \mathtt{closed}) \end{aligned}$$ Evaluating it for both values of $s_1$: $$\begin{aligned} \widehat{\mathtt{bel}}(s_1 = \mathtt{open}) &= 1 \times 0.5 + 0 \times 0.5 = 0.5 \\ \widehat{\mathtt{bel}}(s_1 = \mathtt{closed}) &= 0 \times 0.5 + 1 \times 0.5 = 0.5 \end{aligned}$$ The predicted belief equals the prior. Doing nothing does not change the door, and the door does not change by itself.

​

Measurement update at time 1

The measurement update now reweighs the predicted belief by the likelihood of $z_1 = \mathtt{sense\_open}$: $$\mathtt{bel}(s_1) = \eta\, p(z_1 = \mathtt{sense\_open} \mid s_1)\, \widehat{\mathtt{bel}}(s_1)$$ For the two possible states: $$\begin{aligned} \mathtt{bel}(s_1 = \mathtt{open}) &= \eta\, p(z_1 = \mathtt{sense\_open} \mid s_1 = \mathtt{open})\, \widehat{\mathtt{bel}}(s_1 = \mathtt{open}) \\ &= \eta \times 0.6 \times 0.5 = 0.3\,\eta \\[6pt] \mathtt{bel}(s_1 = \mathtt{closed}) &= \eta\, p(z_1 = \mathtt{sense\_open} \mid s_1 = \mathtt{closed})\, \widehat{\mathtt{bel}}(s_1 = \mathtt{closed}) \\ &= \eta \times 0.2 \times 0.5 = 0.1\,\eta \end{aligned}$$ The two beliefs must sum to one, so $\eta = 1/(0.3 + 0.1) = 2.5$, and $$\begin{aligned} \mathtt{bel}(s_1 = \mathtt{open}) &= 0.75 \\ \mathtt{bel}(s_1 = \mathtt{closed}) &= 0.25 \end{aligned}$$

​

Prediction at time 2

At $t = 2$, the robot pushes the door and again senses that it is open. The prediction step gives $$\begin{aligned} \widehat{\mathtt{bel}}(s_2 = \mathtt{open}) &= 1 \times 0.75 + 0.8 \times 0.25 = 0.95 \\ \widehat{\mathtt{bel}}(s_2 = \mathtt{closed}) &= 0 \times 0.75 + 0.2 \times 0.25 = 0.05 \end{aligned}$$

​

Measurement update at time 2

$$\begin{aligned} \mathtt{bel}(s_2 = \mathtt{open}) &= \eta \times 0.6 \times 0.95 = 0.57\,\eta \\ \mathtt{bel}(s_2 = \mathtt{closed}) &= \eta \times 0.2 \times 0.05 = 0.01\,\eta \end{aligned}$$ With $\eta = 1/(0.57 + 0.01) \approx 1.724$, the posterior belief at $t = 2$ is $$\begin{aligned} \mathtt{bel}(s_2 = \mathtt{open}) &\approx 0.983 \\ \mathtt{bel}(s_2 = \mathtt{closed}) &\approx 0.017 \end{aligned}$$ In the door example each step has an action and a measurement. The filter also works without explicit actions such as when a mobile device localizes itself from radio signals. In that case its location estimator prediction step uses an implicit motion model. Key references: (Burgard & Thrun, n.d.)

​

References

• Burgard, W., Thrun, S. (n.d.). Probabilistic Robotics.

---