# Bayes Filter

## 1 非线性动态系统的状态空间模型

\begin{aligned} \boldsymbol{x_k} &= f(\boldsymbol{x_{k-1}} , \boldsymbol{u_{k}} ) + \boldsymbol{w_{k}} \\ \boldsymbol{z_k} &= h(\boldsymbol{x_{k}}) + \boldsymbol{v_k} \end{aligned}

https://en.wikipedia.org/wiki/State-space_representation#Nonlinear_systems
Modern Control Systems, 12th, by Richard C. Dorf, Robert H. Bishop, Chapter 3 State Variable Models

## 2 Bayes filter

### 2.1 概率基本知识

$p(x,y) = p(X=x \; \text{and} \; Y=y)$

$p(x \mid y) = \frac{p(x,y)}{p(y)}$

\begin{aligned} p(x) & = \sum_y p(x \mid y) p(y) \quad \quad \text{(discrete case)} \\ p(x) & = \int p(x \mid y) p(y) \, \mathrm{d}y \quad \quad \text{(continuous case)} \end{aligned}

$p(x \mid y) = \frac{p(y \mid x )p(x)}{p(y)}$

$p(x \mid y) = \eta \, p(y \mid x )p(x)$

#### 2.1.1 条件独立(Conditional Independence)

$p(x,y \mid z) = p(x \mid z) p (y \mid z)$

\begin{aligned} p(x \mid z) & = p(x \mid z, y) \\ p(y \mid z) & = p(y \mid z, x) \\ \end{aligned}

$\begin{gathered} p(x,y \mid z) = p(x \mid z) p(y \mid z) \mathrel{\rlap{\hskip .5em/}}\Longrightarrow p(x,y) = p(x)p(y) \\ p(x,y) = p(x)p(y) \mathrel{\rlap{\hskip .5em/}}\Longrightarrow p(x,y \mid z) = p(x \mid z) p(y \mid z) \end{gathered}$

### 2.2 贝叶斯滤波

#### 2.2.1 记法说明

$$t_1$$ 到 $$t_2$$ 时刻的所有状态： $$x_{t_1:t_2} = x_{t_1}, x_{t_1 + 1}, x_{t_1 + 2}, \cdots, x_{t_2}$$
$$t_1$$ 到 $$t_2$$ 时刻的所有测量值： $$z_{t_1:t_2} = z_{t_1}, z_{t_1 + 1}, z_{t_1 + 2}, \cdots, z_{t_2}$$
$$t_1$$ 到 $$t_2$$ 时刻的所有控制量： $$u_{t_1:t_2} = u_{t_1}, u_{t_1 + 1}, u_{t_1 + 2}, \cdots, u_{t_2}$$

### 2.3 贝叶斯滤波框架介绍

(1) 测量值（或称观察值） $$z_{t}$$ 和对系统的控制量 $$u_{t}$$
(2) 传感器的模型(Measurements Model) $$p(z_t \mid x_t)$$
(3) 系统控制量的模型(Motion Model) $$p(x_t \mid u_t, x_{t-1})$$
(4) 初始时系统状态，即先验概率 $$p(x_0)$$

$bel(x_t) := p(x_t \mid u_{1:t}, z_{1:t})$

#### 2.3.1 马尔可夫假设（即“状态完全”假设）

A state $$x_t$$ will be called complete if it is the best predictor of the future. Put differently, completeness entails that knowledge of past states, measurements, or controls carry no additional information that would help us to predict the future more accurately. It it important to notice that our definition of completeness does not require the future to be a deterministic function of the state. Temporal processes that meet these conditions are commonly known as Markov chains.

\begin{aligned} p(x_t \mid x_{0:t-1}, z_{1:t-1}, u_{1:t}) &= p(x_t \mid x_{t-1}, u_t) \\ p(z_t \mid x_{0:t}, z_{1:t-1}, u_{1:t}) &= p(z_t \mid x_t) \end{aligned}

### 2.4 贝叶斯滤波推导

\begin{aligned} \overline{bel}(x_t) & = p(x_{t} \mid u_{1:t}, z_{1:t-1}) \\ & \stackrel{\text{Total Prob.}}{=} \int p(x_t \mid x_{t-1}, u_{1:t}, z_{1:t-1}) p(x_{t-1} \mid u_{1:t}, z_{1:t-1}) \, \mathrm{d} x_{t-1} \end{aligned}

\begin{aligned} \overline{bel}(x_t) & = \int p(x_t \mid x_{t-1}, u_{1:t}, z_{1:t-1}) p(x_{t-1} \mid u_{1:t}, z_{1:t-1}) \, \mathrm{d} x_{t-1} \\ & \stackrel{\text{Markov}}{=} \int p(x_t \mid x_{t-1}, u_t) p(x_{t-1} \mid u_{1:t}, z_{1:t-1}) \, \mathrm{d} x_{t-1} \\ & = \int p(x_t \mid x_{t-1}, u_t) p(x_{t-1} \mid u_{1:t-1}, z_{1:t-1}) \, \mathrm{d} x_{t-1} \\ & = \int p(x_t \mid x_{t-1}, u_t) bel(x_{t-1}) \, \mathrm{d} x_{t-1} \end{aligned}

\begin{aligned} bel(x_t) & = p(x_{t} \mid u_{1:t}, z_{1:t}) \\ & = \frac{p(z_t \mid x_t, u_{1:t}, z_{1:t-1})p(x_t \mid u_{1:t}, z_{1:t-1})}{p(z_t \mid u_{1:t}, z_{1:t-1})} \\ & = \eta \, p(z_t \mid x_t, u_{1:t}, z_{1:t-1})p(x_t \mid u_{1:t}, z_{1:t-1}) \end{aligned}

\begin{aligned} bel(x_t) & = \eta \, p(z_t \mid x_t, u_{1:t}, z_{1:t-1})p(x_t \mid u_{1:t}, z_{1:t-1}) \\ & \stackrel{\text{Markov}}{=} \eta \, p(z_t \mid x_t) p(x_t \mid u_{1:t}, z_{1:t-1}) \\ & = \eta \, p(z_t \mid x_t) \overline{bel}(x_t) \end{aligned}

${\color{red}{bel(x_t)}} = \eta \, p(z_t \mid x_t) \int p(x_t \mid x_{t-1}, u_t) {\color{red}{bel(x_{t-1})}} \, \mathrm{d} x_{t-1}$

(1) 传感器模型 $$p(z_t \mid x_t)$$
(2) 系统控制量模型 $$p(x_t \mid u_t, x_{t-1})$$
(3) 系统的初始状态概率分布 $$bel(x_0) = p(x_0)$$

### 2.5 贝叶斯滤波系列算法

• Kalman filters
• Extended Kalman filters
• Particle filters

## 3 贝叶斯滤波实例

\begin{aligned} bel(X_0 = \text{open}) &= 0.5 \\ bel(X_0 = \text{closed}) &= 0.5 \end{aligned}

\begin{aligned} p(Z_t = \mathrm{sense\_open} \mid X_t = \mathrm{is\_open}) &= 0.6 \\ p(Z_t = \mathrm{sense\_closed} \mid X_t = \mathrm{is\_open}) &= 0.4 \\ p(Z_t = \mathrm{sense\_open} \mid X_t = \mathrm{is\_closed}) &= 0.2 \\ p(Z_t = \mathrm{sense\_closed} \mid X_t = \mathrm{is\_closed}) &= 0.8 \\ \end{aligned}

\begin{aligned} p(X_t = \mathrm{is\_open} \mid U_t = \text{push}, X_{t-1} = \mathrm{is\_open}) &= 1 \\ p(X_t = \mathrm{is\_closed} \mid U_t = \text{push}, X_{t-1} = \mathrm{is\_open}) &= 0 \\ p(X_t = \mathrm{is\_open} \mid U_t = \text{push}, X_{t-1} = \mathrm{is\_closed}) &= 0.8 \\ p(X_t = \mathrm{is\_closed} \mid U_t = \text{push}, X_{t-1} = \mathrm{is\_closed}) &= 0.2 \\ p(X_t = \mathrm{is\_open} \mid U_t = \mathrm{do\_nothing}, X_{t-1} = \mathrm{is\_open}) &= 1 \\ p(X_t = \mathrm{is\_closed} \mid U_t = \mathrm{do\_nothing}, X_{t-1} = \mathrm{is\_open}) &= 0 \\ p(X_t = \mathrm{is\_open} \mid U_t = \mathrm{do\_nothing}, X_{t-1} = \mathrm{is\_closed}) &= 0 \\ p(X_t = \mathrm{is\_closed} \mid U_t = \mathrm{do\_nothing}, X_{t-1} = \mathrm{is\_closed}) &= 1 \\ \end{aligned}

\begin{aligned} \overline{bel}(x_1) & = \int p(x_1 \mid x_0, u_1) bel(x_0) \, \mathrm{d} x_0 \\ & = \sum_{x_0} p(x_1 \mid x_0, u_1) bel(x_0) \\ & = p(x_1 \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_open}) \, bel(X_0 = \mathrm{is\_open}) \\ & \quad + p(x_1 \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_closed}) \, bel(X_0 = \mathrm{is\_closed}) \\ \end{aligned}

\begin{aligned} \overline{bel}(X_1 = \mathrm{is\_open}) & = p(X_1 = \mathrm{is\_open} \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_open}) \, bel(X_0 = \mathrm{is\_open}) \\ & \quad + p(X_1 = \mathrm{is\_open} \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_closed}) \, bel(X_0 = \mathrm{is\_closed}) \\ & = 1 \cdot 0.5 + 0 \cdot 0.5 = 0.5 \\ \end{aligned}

\begin{aligned} \overline{bel}(X_1 = \mathrm{is\_closed}) & = p(X_1 = \mathrm{is\_closed} \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_open}) \, bel(X_0 = \mathrm{is\_open}) \\ & \quad + p(X_1 = \mathrm{is\_closed} \mid U_1 = \mathrm{do\_nothing}, X_0 = \mathrm{is\_closed}) \, bel(X_0 = \mathrm{is\_closed}) \\ & = 0 \cdot 0.5 + 1 \cdot 0.5 = 0.5 \\ \end{aligned}

$bel(x_1) = \eta \, p(Z_1 = \mathrm{sense\_open} \mid x_1) \overline{bel}(x_1)$

\begin{aligned} bel(X_1 = \mathrm{is\_open} ) &= \eta \, p(Z_1 = \mathrm{sense\_open} \mid X_1 = \mathrm{is\_open}) \overline{bel}(X_1 = \mathrm{is\_open})\\ & = \eta \, 0.6 \cdot 0.5 = \eta \, 0.3 \end{aligned}

\begin{aligned} bel(X_1 = \mathrm{is\_closed} ) &= \eta \, p(Z_1 = \mathrm{sense\_open} \mid X_1 = \mathrm{is\_closed}) \overline{bel}(X_1 = \mathrm{is\_closed})\\ & = \eta \, 0.2 \cdot 0.5 = \eta \, 0.1 \end{aligned}

$\eta = (0.3 + 0.1)^{-1} = 2.5$

\begin{aligned} bel(X_1 = \mathrm{is\_open} ) = \eta \, 0.3 = 0.75 \\ bel(X_1 = \mathrm{is\_closed} ) = \eta \, 0.1 = 0.25 \end{aligned}

\begin{aligned} \overline{bel}(X_2 = \mathrm{is\_open}) = 1 \cdot 0.75 + 0.8 \cdot 0.25 = 0.95 \\ \overline{bel}(X_2 = \mathrm{is\_closed}) = 0 \cdot 0.75 + 0.2 \cdot 0.25 = 0.05 \\ \end{aligned}

\begin{aligned} bel(X_2 = \mathrm{is\_open}) = \eta \, 0.6 \cdot 0.95 \approx 0.983 \\ bel(X_2 = \mathrm{is\_closed}) = \eta \, 0.2 \cdot 0.05 \approx 0.017 \\ \end{aligned}

## 4 Binary Bayes Filters With Static State

$bel_t(\boldsymbol{x}) = p(\boldsymbol{x} \mid \boldsymbol{z}_{1:t}, \boldsymbol{u}_{1:t}) = p(\boldsymbol{x} \mid \boldsymbol{z}_{1:t})$

$l(x) := \log \frac{p(x)}{p(\lnot x)} = \log \frac{p(x)}{1- p(x)}$

### 4.1 Binary Bayes Filter算法

Inverse models are often used in situations where measurements are more complex than the binary state. An example of such a situation is the problem of estimating whether or not a door is closed, from camera images. Here the state is extremely simple, but the space of all measurements is huge. It is easier to devise a function that calculates a probability of a door being closed from a camera image, than describing the distribution over all camera images that show a closed door.

### 4.2 Binary Bayes Filter算法推导

\begin{aligned} bel(x_t) & = p(x_{t} \mid u_{1:t}, z_{1:t}) \\ & = \frac{p(z_t \mid x_t, u_{1:t}, z_{1:t-1})p(x_t \mid u_{1:t}, z_{1:t-1})}{p(z_t \mid u_{1:t}, z_{1:t-1})} \\ & = \frac{p(z_t \mid x_t, u_{1:t}, z_{1:t-1})p(x_t \mid u_{1:t}, z_{1:t-1})}{p(z_t \mid u_{1:t}, z_{1:t-1})} \\ \end{aligned}

\begin{aligned} p(x \mid z_{1:t}) & = \frac{p(z_t \mid x, z_{1:t-1})p(x \mid z_{1:t-1})}{p(z_t \mid z_{1:t-1})} \\ & = \frac{p(z_t \mid x)p(x \mid z_{1:t-1})}{p(z_t \mid z_{1:t-1})} \\ \end{aligned}

$p(x \mid z_{1:t}) = \frac{p(x \mid z_t)p(z_t) p(x \mid z_{1:t-1})}{p(x) p(z_t \mid z_{1:t-1})}$

$p(\lnot x \mid z_{1:t}) = \frac{p(\lnot x \mid z_t)p(z_t) p(\lnot x \mid z_{1:t-1})}{p(\lnot x) p(z_t \mid z_{1:t-1})}$

\begin{aligned} l_t(x) & := \log \frac{bel_t(x)}{1 - bel_t(x)} \\ & = \log \left( \frac{p(x \mid z_{1:t})}{p(\lnot x \mid z_{1:t})} \right) \\ & = \log \left( \frac{\frac{p(x \mid z_t)p(z_t) p(x \mid z_{1:t-1})}{p(x) p(z_t \mid z_{1:t-1})}}{\frac{p(\lnot x \mid z_t)p(z_t) p(\lnot x \mid z_{1:t-1})}{p(\lnot x) p(z_t \mid z_{1:t-1})}} \right) \\ & = \log \left( \frac{p(x \mid z_t)}{p(\lnot x \mid z_t)} \cdot \frac{p(x \mid z_{1:t-1})}{p(\lnot x \mid z_{1:t-1})} \cdot \frac{p(\lnot x)}{p(x)} \right) \\ & = \log \left( \frac{p(x \mid z_t)}{1 - p(x \mid z_t)} \cdot \frac{p(x \mid z_{1:t-1})}{1 - p(x \mid z_{1:t-1})} \cdot \frac{1 - p(x)}{p(x)} \right) \\ & = \log \frac{p(x \mid z_t)}{1 - p(x \mid z_t)} + \log \frac{p(x \mid z_{1:t-1})}{1 - p(x \mid z_{1:t-1})} + \log \frac{1 - p(x)}{p(x)} \\ & = \log \frac{p(x \mid z_t)}{1 - p(x \mid z_t)} + l_{t-1}(x) - \log \frac{p(x)}{1 - p(x)} \\ \end{aligned}

Created: <2016-06-11 Sat 00:00>

Last updated: <2018-02-10 Sat 19:47>

Creator: Emacs 25.3.1 (Org mode 9.1.4)