Bayes Filter

随机变量 \(X\) 和 \(Y\) 的联合分布记为：
\[p(x,y) = p(X=x \; \text{and} \; Y=y)\]

如果 \(p(y) > 0\) ，我们称已经知道随机变量 \(Y=y\) 时，随机变量 \(X=x\) 的概率为条件概率，其定义为：
\[p(x \mid y) = \frac{p(x,y)}{p(y)}\]

全概率公式(Law of Total Probability)为：
\[\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)}\]
由于当 \(x\) 取不同值时，上式的分母 \(p(y)\) 是不会变的，所以常常记 \(\eta = \frac{1}{p(y)}\) ，这样贝叶斯公式也可以写为：
\[p(x \mid y) = \eta \, p(y \mid x )p(x)\]

贝叶斯滤波(Beyes filter, or Recursive Bayesian estimation)为非线性动态系统的状态估计问题提供了一种基于概率分布形式的解决方案。 贝叶斯滤波的基本原理是：首先用系统模型预测出状态的概率密度，然后用最近的测量值对其进行修正得到“后验概率密度”。

在贝叶斯滤波框架中，下面这些是已知的（可以认为它们是系统参数）：
(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)\)

贝叶斯滤波就是 求已知系统控制量（即 \(u_{1:t}\) ）和状态测量值（即 \(z_{1:t}\) ）时系统的状态，即后验概率： \(p(x_t \mid u_{1:t}, z_{1:t})\) ，一般我们把这个后验概率称为 Belief ，记为：
\[bel(x_t) := p(x_t \mid u_{1:t}, z_{1:t})\]

为了简化 \(bel(x_t)\) 的求解，我们假设系统满足马尔可夫性，请看下文。

本节主要参考：Probabilistic Robotics, by Sebastian Thrun, 2.4.3 Mathematical Derivation of the Bayes Filter

贝叶斯滤波推导，可以分为两步。
第一步，预测过程，即由 \(p(x_{t-1} \mid u_{1:t-1}, z_{1:t-1})\) （也就是 \(bel(x_{t-1})\) ）推导出 \(p(x_{t} \mid u_{1:t}, z_{1:t-1})\) 。
我们把 \(p(x_{t} \mid u_{1:t}, z_{1:t-1})\) 记作 \(\overline{bel}(x_t)\) 。由全概率公式(Law of Total Probability)可得：
\[\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}\]
由马尔可夫假设，有： \(p(x_t \mid x_{t-1}, u_{1:t}, z_{1:t-1}) = p(x_t \mid x_{t-1}, u_t)\)
观察上式中的 \(p(x_{t-1} \mid u_{1:t}, z_{1:t-1})\) ，由于 \(u_t\) 显然不会对过去的状态 \(x_{t-1}\) 产生影响，所以可以去掉它（因为如果 \(A,B\) 相互独立，有 \(P(A \mid B) = P(A)\) ），从而有： \(p(x_{t-1} \mid u_{1:t}, z_{1:t-1}) = p(x_{t-1} \mid u_{1:t-1}, z_{1:t-1})\)
综上，有：
\[\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}\]

第二步，更新过程，即由 \(p(x_{t} \mid u_{1:t}, z_{1:t-1})\) 推导出 \(p(x_{t} \mid u_{1:t}, z_{1:t})\) （也就是 \(bel(x_t)\) ）。
由贝叶斯公式可以推导出 \(p(x \mid y,z) = \frac{p(y \mid x, z)p(x \mid z)}{p(y \mid z)}\) （这个式子称为 Bayes Rule with Background Knowledge），利用这个结论可得：
\[\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}\]
其中， \(\eta\) 可看做是归一化常数，它当 \(x_t\) 取不同可能状态时是不变的，所以保证 \(x_t\) 取所有可能状态时 \(bel(x_t)\) 相加为 1 即可。
由马尔可夫假设，有： \(p(z_t \mid x_t, u_{1:t}, z_{1:t-1}) = p(z_t \mid x_t)\) ，所以：
\[\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}\]
具有极大后验概率密度 \(bel(x_t)\) 的状态可以作为系统状态的估计值。

从上面的公式知， 贝叶斯滤波需要知道三个概率分布：
(1) 传感器模型 \(p(z_t \mid x_t)\)
(2) 系统控制量模型 \(p(x_t \mid u_t, x_{t-1})\)
(3) 系统的初始状态概率分布 \(bel(x_0) = p(x_0)\)

贝叶斯滤波算法可简单描述为：

贝叶斯滤波需要进行积分运算，除了一些特殊的系统模型（如线性高斯系统，有限状态的离散系统）之外，对于一般的非线性、非高斯系统，贝叶斯滤波很难得到后验概率的封闭解析式。因此，现有的非线性滤波器多采用近似的计算方法解决积分问题，以此来获取估计的次优解。在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下，基于一阶或二阶 Taylor 级数展开的扩展 Kalman 滤波得到广泛应用。在一般情况下，逼近概率密度函数比逼近非线性函数容易实现。据此，Julier 与 Uhlmann 提出一种 Unscented Kalman 滤波器(UKF)，通过选定的 sigma 点来精确估计随机变量经非线性变换后的均值和方差，从而更好的近似状态的概率密度函数，其理论估计精度优于扩展 Kalman 滤波(EKF)。获取次优解的另外一中方案便是基于蒙特卡洛模拟的粒子滤波器。

摘自：http://wenku.baidu.com/view/88896d2b453610661ed9f4b4.html

贝叶斯滤波算法是一个大的框架，一系列算法都可以认为是贝叶斯滤波算法的具体实现。
如下面这些都是贝叶斯滤波算法：

• Kalman filters
  
• Extended Kalman filters
  
• Particle filters
  

贝叶斯滤波是一个大的框架，它可以用来作状态估计（状态估计是一个非常广的概念，如速度估计，位置估计等等）。
机器人的 Localization 算法（已知地图，估计自己的位置）基本上都是贝叶斯滤波或其变种。

定义 \(l_t(x) := \log \frac{bel_t(x)}{1 - bel_t(x)}\) ，Binary Bayes Filter 算法如图 5 所示。

其中， \(p(x)\) 是状态 \(x\) 的先验概率，初始时 \(l_0(x) = \log \frac{p(x)}{1 - p(x)}\) 。

说明：Binary Bayes Filter 算法中使用了 \(p(x \mid z_t)\) （它被称为 Inverse Measurement Model），而没有使用贝叶斯滤波中的 \(p(z_t \mid x)\) 。

如果状态相对简单时， \(p(x \mid z_t)\) 比 \(p(z_t \mid x)\) 更加容易计算。

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.

在前面推导 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(z_t \mid x) = \frac{p(x \mid z_t)p(z_t)}{p(x)}\) ，所以上式进一步可化为：
\[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})}\]

利用前面两式，对 \(l_t(x)\) 进行推导，有：
\[\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}\]

证毕。