初始化
1. 初始化状态
2. 初始化状态初值的置信度，通常为设置协方差矩阵
预测
1. 根据模型预测下一个状态
2. 调整状态的置信度，通常为更新协方差矩阵
更新
1. 获取观测数据及观测数据的置信度(协方差矩阵)
2. 计算当前状态与观测状态的残差。
3. 依据观测置信度，更新当前的状态，及置信度。
符号约定
- $ x$通常表示状态。
-
$Z$ 通常表示观测向量，实际由m维向量组成，$z_i$表示第i个观测向量。 -
$u$ 表示动作。
贝叶斯滤波是g-h滤波的一类算法，也是卡尔曼滤波，粒子滤波等的基础，这里只是阐述其推导过程。
基础知识
贝叶斯公式 $$ P(x, z) = P( x|z) P(z) = P(z|x) P(x) $$
-
$P(z|x)$ 通常是预先知道的，也就是在状态为$x$的情况下，观测到$z$的概率。 -
$P(x|z)$ 是通过观测到$z$，推断实际值是$x$的概率。 -
$P(x), P(z)$ 是先验知识
由于$z$通常是我们观测到的，$P(x|z)$无关，所以也可以写作：
$$
P(x|z) = \eta P(z|x) P(x)
\ \eta = \frac {1} {P(z)}
$$
前面是对于单一观测值下，对状态的估计。而实际中会是依据多观测值估计当前状态，也就是要计算$P(x|Z)$ $$ \begin{split} P(x|Z) &= P(x|z_1, z_2, \cdots , z_m) \ &= \frac {P(x, z_1, z_2, \cdots, z_m) } {P(Z)} \ &= \frac {P(z_1| x, z_2, \cdots, z_m) P(x, z_2, \cdots, z_m)} {P(Z)} \ &= \frac{P(z_1| x, z_2, \cdots, z_m)P(z_2 | x, z_3, \cdots, z_m) P(x, z_3, \cdots ,z_m)}{P(Z)} \ &= \frac{P(z_1| x, z_2, \cdots, z_m) P(z_2 | x, z_3, \cdots , z_m) \cdots P(z_m|x) P(x)} {P(Z)} \end{split} \tag{5} $$ 通常假设各观测量之间彼此独立，也即$z_i$只与$x$有关，所以有 $$ \begin{split} P(x| Z) &= \frac {P(z_1|x) P(z_2|x) \cdots P(z_m|x) P(x)} {P(Z)} \ &=\frac {P(z_1|x) P(z_2|x) \cdots P(z_m|x) P(x)} {\prod_{i=1}^m P(z_i)} \ &= \prod_{i=1}^m \eta_i \prod_{j=1}^m P(z_j|x)P(x) \end{split} \tag{6} $$ 马尔可夫过程
贝叶斯滤波算法
问题描述
已知机器人初始状态为$x_0$，假设t-1时刻状态为$x_{t-1}$，当前输入为$u_t$，得到状态$x_t$，此时机器人观测量为$z_t$，求解某一时刻机器人的状态$x_t$。
输入：
1. 1 到t时刻的状态动作和观测值：$u_1, z_1, \cdots, u_t, z_t$
2. 观测模型：$P(z|x)$
3. 状态转移模型：$P(x_t| x_{t-1}, u_t)$
4. 系统状态先验分布：$P(x)$
输出：
算法假设
- 遵循马尔可夫假设：$x_t$仅由$x_{t-1}$和$u_t$决定，当$x_t$确定时，观测量$z_t$仅与$x_t$有关；
- 静态环境，环境不发生动态变化；
- 观测噪声和模型噪声相互独立。
推导过程如下： $$ \begin{split} Bel(x_t)&=P(x_t|x_0, u_1, z_1, \cdots, u_t, z_t) \ &= \frac {P(z_t | x_t, x_0, u_1, z_1, \cdots, u_t) P(x_t, x_0, u_1, z_1, \cdots, u_t)} {P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) P(x_t, x_0, u_1, z_1, \cdots, u_t)} {P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) \int P(x_t, x_0, u_1, z_1, \cdots, u_t, x_{t-1}) dx_{t-1} } {P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) \int P(x_t | x_0, u_1, z_1, \cdots, u_t, x_{t-1}) P(x_0, u_1, z_1,\cdots, u_t, x_{t-1}) dx_{t-1}} {P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) \int P(x_t| x_{t-1}, u_{t}) P(x_{t-1}| x_0, u_1, z_1, \cdots, u_t) P(x_0, u_1, z_1, \cdots, u_t) dx_{t-1} } {P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) P(x_0, u_1, z_1, \cdots, u_t) \int P(x_t| x_{t-1}, u_t) P(x_{t-1}| x_0, u_1, z_1, \cdots, u_t) d x_{t_1}} { P(x_0, u_1, z_1, \cdots, u_t, z_t) }\ &= \frac {P(z_t|x_t) P(x_0, u_1, z_1, \cdots, u_t) \int P(x_t| x_{t-1}, u_t) P(x_{t-1} | x_0, u_1, z_1, \cdots, z_{t-1}) dx_{t-1} } { P(x_0, u_1, z_1, \cdots, u_t, z_t) } \ &= \frac {P(z_t|x_t) P(x_0, u_1, z_1, \cdots, u_t) \int P(x_t| x_{t-1}, u_t) Bel(x_{t-1}) dx_{t-1}} { P(x_0, u_1, z_1, \cdots, u_t, z_t)} \ &= \frac {P(z_t|x_t) \int P(x_t| x_{t-1}, u_t) Bel(x_{t-1}) dx_{t-1}} {P(z_t | x_0, u_1, z_1, \cdots, u_t)} \ &= \eta_{z_t} P(z_t|x_t) \int P(x_t|x_{t-1}, u_t)Bel(x_{t-1}) dx_{t-1} \end{split} \tag{7} $$
推导过程中，
第一步是贝叶斯概率公式；
第二步运用马尔可夫假设；
第三步运用全概率公式；
第四步应用全概率公式；
第五步应用马尔可夫假设；
第六步$P(x_0, u_1, z_1, \cdots, u_t)与x_{t-1}$无关，视为常量，提取到积分外；
第七步$x_{t-1}$与$u_t$无关，运用马尔可夫假设；
第九步，全概率公式，消除$P(x_0, u_1, z_1, \cdots, u_t)$
根据递推式，有
预测： $$ \int P(x_t|x_{t-1}, u_t) Bel(x_{t-1}) dx_{t-1} \tag{8} $$ 更新： $$ \eta_{z_t} P(z_t | x_t) \tag{9} $$ 算法流程：
BayesFilter(Bel(x), d):
如果d是观测数据， 则
for all x do:
for all x do
如果d是动作，则
for all x do: