Recursive State Estimation

The perception pipelines of the scene understanding chapter tell you where the objects of interest are in the image coordinate frame. They stop short of what humans call understanding. Giving each object a symbolic identity is one step toward it, because it lets an embodied agent reason about the scene with the symbolic methods the AI community has developed over many decades. Where probabilistic reasoning sits in an embodied agent. The Bayesian inference section treated two random variables, captured by a joint distribution $p(x, y)$, and inferred the posterior of one given the other. This section extends that picture in two ways:

• It introduces a state $s$ that gathers many random variables, and considers dynamical systems whose state evolves over time, often with nonlinear dynamics, as is common in robotics.
• It makes the time index $t$ explicit, so the evolution of the state can be drawn as a graphical model.

The perception subsystem turns sensor data into noisy estimates, such as object detections. The noise comes both from the algorithms and from imperfect sensors. The model that maps the true state to these estimates is the measurement model, also called the sensor model. Together with the transition model, which describes how the state changes, it is all you need to apply Bayes’ rule recursively. The result is the recursive state estimator, better known as the Bayes filter. It lets the agent maintain an internal belief about the current state of the environment.

​

The Bayes filter

Consider an embodied agent, such as a robot, that moves in an environment. The agent’s belief and its interactions with the environment. 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$ carries everything about the past that matters for 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. It is less restrictive than it looks, 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, measurements and controls. The figure uses $x$ for states and $u$ for actions; the text uses $s$ and $a$. 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.

The algorithm takes the previous belief, the latest action and the latest measurement, and returns the new belief. The normalizer $\eta$ makes $\mathtt{bel}(s_t)$ sum (or integrate) to one over all $s_t$. $$\begin{array}{rl} 1 & \quad \textbf{function}\ \texttt{bayes\_filter}(\mathtt{bel}(s_{t-1}),\, a_t,\, z_t) \\ 2 & \quad \quad \textbf{for all}\ s_t\ \textbf{do} \\ 3 & \quad \qquad \widehat{\mathtt{bel}}(s_t) = \int p(s_t \mid a_t, s_{t-1})\, \mathtt{bel}(s_{t-1})\, ds_{t-1} \qquad \text{(prediction)} \\ 4 & \quad \qquad \mathtt{bel}(s_t) = \eta\, p(z_t \mid s_t)\, \widehat{\mathtt{bel}}(s_t) \qquad \text{(measurement update)} \\ 5 & \quad \quad \textbf{end for} \\ 6 & \quad \quad \textbf{return}\ \mathtt{bel}(s_t) \end{array}$$ The Bayes filter. For a discrete state, the integral in line 3 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 a LIDAR may mistake the returns from that clutter for 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 $z_1 = \mathtt{sense\_open}$ now reweighs the predicted belief: $$\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}$$ The example is small, but it shows how the Bayes filter combines action and perception in one framework. The action is optional. A phone localizes itself from radio signals with the same recursion, even though it is carried rather than commanded. Its prediction step then uses a motion model without a control input. Key references: (Burgard & Thrun, n.d.)

​

References

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

---