Nonparametric Filters

| |

直方图滤波

直方图滤波(Histogram filter) 将状态空间分解成有限的区域,并用一个单一的概率值表示每一个区域的累积后验。应用于有限空间时,该滤波被称为离散贝叶斯滤波(discrete Bayes filter) ; 应用于连续空间时,通常称之为直方图滤波。 离散贝叶斯滤波应用于有限状态空间,这里随机变量 $X$ 可以取有限多个值,有限状态空间在机器人学中应用很广泛。

离散贝叶斯算法离散贝叶斯算法
在连续状态空间,可以考虑将连续空间离散化,这被称为直方图滤波。直方图滤波将连续状态空间分解成有限位,或者区域:

$$ \begin{align} dom (X_t) = x_{1,t} \cup x_{2,t} \cup \cdots \cup x_{k,t} \\ \forall i \ne k \text{,有} x_{i,t} \cap x_{k,t} = \varnothing \notag \end{align} $$

其中 $X_t$ 为描述机器人状态在时刻 $t$ 的随机变量,函数 $dom (X_t)$ 表示状态空间,它是 $X_t$ 所以可以取值的域。
离散贝叶斯滤波为每一个区域 $x_{k,t}$ 分配一个概率 $p_{k,t}$,在每一个区域里,离散贝叶斯滤波不会再携带更多的关于置信度分布的信息,此时后验概率是一个分段常数的概率密度函数:

$$ \begin{align} p(x_t) = \frac{p_{k,t}}{\vert x_{k,t} \vert} \end{align} $$

如果状态空间的确是离散的,则条件概率 $p(x_{k,t} \vert u_t , x_{i,t-1})$ 和 $p(z_t \vert x_{k,t})$ 就是已知的,在连续状态空间中,通常根据每个状态给定定义密度 $p(x_{t} \vert u_t , x_{t-1})$ 和 $p(z_t \vert x_{t})$ ,例如在每一个很小的区域取一个均值:

$$ \begin{align} \hat x_{k,t} = \vert x_{k,t} \vert ^{-1} & \int _{x_{k,t}} x_{t} d x_t \\ & \Downarrow \notag \\ p(z_t \vert x_{k,t}) & \approx p(z_t \vert \hat x_{k,t}) \\ p(x_{k,t} \vert u_t , x_{i,t-1}) &\approx \eta \vert x_{k,t} \vert p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1}) \end{align} $$

下面证明上面的(4)式:

$$ \begin{align} \begin{split} p(z_t \vert x_{k,t}) &= \frac{p(z_t ,x_{k,t})}{p(x_{k,t})} \\ & = \frac{\int_{x_{k,t}} p(z_t ,x_t) d x_t}{ \int_{x_{k,t}} p(x_t) d x_t} \\ & = \frac{\int_{x_{k,t}} p(z_t |x_t) \textcolor{blue}{p(x_t)} d x_t}{ \int_{x_{k,t}} \textcolor{blue}{p(x_t)} d x_t} \\ & =\frac{\int_{x_{k,t}} p(z_t |x_t) \textcolor{blue}{\frac{p_{k,t}} {\vert x_{k,t} \vert}} d x_t}{ \int_{x_{k,t}} \textcolor{blue}{\frac{p_{k,t}} {\vert x_{k,t} \vert}} d x_t} \\ & = \frac{\textcolor{blue}{\frac{p_{k,t}}{\vert x_{k,t} \vert}} \int_{x_{k,t}} p(z_t |x_t) d x_t }{\textcolor{blue}{\frac{p_{k,t}}{\vert x_{k,t} \vert}} \int_{x_{k,t}} 1 d x_t } \\ & = \frac{\int_{x_{k,t}} p(z_t |x_t) d x_t }{\int_{x_{k,t}} 1 d x_t } \\ &= |x_{k,t}|^{-1} \int_{x_{k,t}} \textcolor{green}{p(z_t |x_t)} d x_t \\ & \Downarrow 对 x_t \isin x_{k,t},使\textcolor{green}{p(z_t \vert \hat x_{k,t}) }\approx p(z_t |x_t) \\ p(z_t \vert x_{k,t}) & \approx |x_{k,t}|^{-1} \int_{x_{k,t}} \textcolor{green}{p(z_t \vert \hat x_{k,t}) } d x_t \\ & = |x_{k,t}|^{-1} \textcolor{green}{p(z_t \vert \hat x_{k,t}) } \int_{x_{k,t}} 1 d x_t \\ & = |x_{k,t}|^{-1} \textcolor{green}{p(z_t \vert \hat x_{k,t}) } |x_{k,t}|\\ &= \textcolor{green}{p(z_t \vert \hat x_{k,t}) } \end{split} \end{align} $$

继续:

$$ \begin{align} \begin{split} p(x_{k,t} \vert u_t , x_{i,t-1}) &= \frac{p(x_{k,t} ,x_{i,t-1} | u_t)}{p(x_{i,t-1} | u_t)} \\ &=\frac{\int_{x_{k,t} } \int _{x_{i,t-1} } \textcolor{blue}{p(x_t,x_{t-1} | u_t)} d x_t d x_{t-1} }{ \int _{x_{i,t-1} } p (x_{t-1}|u_t) d x_{t-1}} \\ & = \frac{\int_{x_{k,t} } \int _{x_{i,t-1} } \textcolor{blue}{p(x_t | u_t,x_{t-1})p(x_{t-1} | u_t)} d x_t d x_{t-1} }{ \int _{x_{i,t-1} } p (x_{t-1}|u_t) d x_{t-1}} \\ & \Downarrow \text{马尔可夫假设} \\ &= \frac{\int_{x_{k,t} } \int _{x_{i,t-1} } p(x_t | u_t,x_{t-1})\textcolor{orange}{p(x_{t-1})} d x_t d x_{t-1} }{ \int _{x_{i,t-1} } \textcolor{orange}{p (x_{t-1})} d x_{t-1}} \\ &= \frac{\int_{x_{k,t} } \int _{x_{i,t-1} } p(x_t | u_t,x_{t-1})\textcolor{orange}{\frac{p_{i,t-1}}{\vert x_{i,t-1} \vert}} d x_t d x_{t-1} }{ \int _{x_{i,t-1} } \textcolor{orange}{\frac{p_{i,t-1}}{\vert x_{i,t-1} \vert} } d x_{t-1}} \\ &=\frac{\int_{x_{k,t} } \int _{x_{i,t-1} } p(x_t | u_t,x_{t-1}) d x_t d x_{t-1} }{ \int _{x_{i,t-1} } 1 d x_{t-1}} \\ &=|x_{i,t-1}|^{-1} \int_{x_{k,t} } \int _{x_{i,t-1} } \textcolor{blue}{p(x_t | u_t,x_{t-1})} d x_t d x_{t-1} \\ & \Downarrow \text{取} p(x_{t} \vert u_t , x_{t-1}) \approx p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1}) \\ & \approx \eta |x_{i,t-1}|^{-1} \int_{x_{k,t} } \int _{x_{i,t-1} } \textcolor{blue}{p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1})} d x_t d x_{t-1} \\ & = \eta |x_{i,t-1}|^{-1} \textcolor{blue}{p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1})} \int_{x_{k,t} } \int _{x_{i,t-1} } 1 d x_t d x_{t-1} \\ & = \eta \textcolor{green}{|x_{i,t-1}|^{-1}} \textcolor{blue}{p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1})} |x_{k,t}| \textcolor{green}{ |x_{i,t-1}|} \\ & = \eta |x_{k,t}| \textcolor{blue}{p(\hat x_{k,t} \vert u_t , \hat x_{i,t-1})} \end{split} \end{align} $$

如果所有区域大小相等(意味着 $|x_{k,t}|$ 对所有 $k$ 都是相同的),可以简单地忽略因子 $|x_{k,t}|$ ,将它合并入归一化因子中。

静态二值贝叶斯滤波

如果一个机器人从传感器测量的序列中估计环境的一个固定的二值数,那么这些问题通过二值贝叶斯滤波(binary Bayes filter) 来阐述。例如,一个机器人可能想知道门是开着的还是关着的,并认为在检测期间门的状态不改变。使用静态二值贝叶斯滤波的另一个例子是占用栅格地图(occupancy grid maps)。 当状态静止时,置信度就仅是测量的函数:

$$ \begin{align} bel_t(x) = p(x|z_{1:t},u_{1:t}) = p(x|z_{1:t}) \end{align} $$

二值状态用 $x$ 和 $\neg x$ 来表示,有 $bel_t(\neg x) = 1- bel_t (x)$ ,注意到这里的状态 $x$ 与时间无关。 这个问题仍然可以用上面的离散贝叶斯滤波来处理,但是,这种情况下的置信度通常由一个概率比的对数来计算,以此避免概率接近0或1所引起的截断问题。
状态 $x$ 的概率定义为此事件的概率除以事件不发生的概率:

$$ \begin{align} \frac{p(x)}{p(\neg x)}= \frac{p(x)}{1- p(x)} \\ l(x) := \log \frac{p(x)}{1-p(x)} \end{align} $$

上式 $l(x)$ 称为“概率对数”,取值为 $-\infin \sim \infin $ 算法流程图如下:

具有反向测量模型的概率对数形式的二值贝叶斯滤波具有反向测量模型的概率对数形式的二值贝叶斯滤波
这里的二值贝叶斯滤波利用了一个反向测量模型(inverse measurement model) $p(x|z_t)$ 代替前向模型 $p(z_t|x)$ ,有:

$$ \begin{align} bel_t (x) = 1-\frac{1}{1+\exp\{l_t\}} \end{align} $$

证明:

$$ \begin{align} \begin{split} p(x|z_{1:t}) &= \frac{p(z_t | x,z_{1:t-1})p(x|z_{1:t-1})}{p(z_t |z_{1:t-1})} = \frac{\textcolor{blue}{p(z_t|x)}p(x|z_{1:t-1})}{p(z_t |z_{1:t-1})} \\ &= \frac{\textcolor{blue}{\frac{p(x|z_t)p(z_t)}{p(x)}}p(x|z_{1:t-1})}{p(z_t |z_{1:t-1})} \\ &=\frac{p(x|z_t) p(z_t) p(x|z_{1:t-1})}{p(x) p(z_t |z_{1:t-1})} \\ \end{split} \end{align} $$

类似的,对事件 $\neg x$ 有:

$$ \begin{align} p(\neg x|z_{1:t}) = \frac{p(\neg x|z_t) p(z_t) p(\neg x|z_{1:t-1})}{p(\neg x) p(z_t |z_{1:t-1})} \end{align} $$

用式(13)除以式(12)有:

$$ \begin{align} \begin{split} \frac{p(x|z_{1:t})}{p(\neg x|z_{1:t})} &= \frac{p(x|z_t) p(z_t) p(x|z_{1:t-1}) p(\neg x) p(z_t |z_{1:t-1})}{p(x) p(z_t |z_{1:t-1}) p(\neg x|z_t) p(z_t) p(\neg x|z_{1:t-1})}\\ &= \frac{p(x|z_t) p(x|z_{1:t-1}) p(\neg x) }{p(x) p(\neg x|z_t) p(\neg x|z_{1:t-1})} \\ &= \frac{p(x|z_t) p(x|z_{1:t-1}) (1-p(x)) }{p(x) (1-p(x|z_t)) (1-p(x|z_{1:t-1}))} \\ &= \frac{\textcolor{blue}{p(x|z_t)} \quad \textcolor{green}{p(x|z_{1:t-1})} \quad (1-p(x)) }{\textcolor{blue}{(1-p(x|z_t))} \textcolor{green}{(1-p(x|z_{1:t-1})) } p(x) } \end{split} \end{align} $$

由上面的式(10)有:

$$ \begin{align} \begin{split} l_t (x) &=\log \frac{\textcolor{blue}{p(x|z_t)} }{\textcolor{blue}{(1-p(x|z_t))} } + \log \frac{\textcolor{green}{p(x|z_{1:t-1})}}{\textcolor{green}{(1-p(x|z_{1:t-1})) } } + \log \frac{ (1-p(x))}{p(x)} \\ &=\log \frac{\textcolor{blue}{p(x|z_t)} } {\textcolor{blue}{(1-p(x|z_t))} } - \log \frac{p(x)}{1-p(x)} + l_{t-1}(x) \end{split} \end{align} $$

其中, $p(x)$ 是x的先验概率,上式中每次测量更新需要先验的求和,一般先验定义为处理传感器测量前的初始置信度的概率对数:

$$ \begin{align} l_0 (x) =\log \frac{p(x)}{1-p(x)} \end{align} $$

粒子滤波

粒子滤波(particle filter) 是贝叶斯滤波的另一种非参数实现。
粒子滤波中,后验分布的样本叫作粒子(particles) , 有

$$ \begin{align} \chi_t = x_t^{[1]},x_t^{[2]},\cdots ,x_t^{[M]} \end{align} $$

每一个粒子 $x_t^{[m]} (1 \leq m \leq M)$ 是状态在时刻 $t$ 的一个具体的实例,一个粒子是根据真实世界状态在时刻 $t$ 的一种可能假设,其中 $M$ 代表粒子集 $\chi_t$ 的粒子数量,$M$一般较大,例如$M=1000$,有时 $M$是$t$或其他与置信度 $bel(x_t)$ 相关的其他数量的函数。粒子滤波的直观感觉就是用一系列粒子 $\chi_t$ 来近似置信度 $bel(x_t)$ ,理想状态下,假设 $x_t$包含在粒子集合 $\chi_t$ 中的可能性与其贝叶斯滤波的后验分布 $bel(x_t)$ 成比例:

$$ \begin{align} x_t^{[m]} \sim p(x_t|z_{1:t},u_{1:t}) \end{align} $$

当 $M→ \infin$ 时,属性是完全满足的。
粒子滤波也是递归的,算法流程如下:

粒子滤波.png粒子滤波.png

输入粒子集$\chi_{t-1}$和最新的动作(控制)$z_t$ 和最新的测量 $z_t$ ,算法先构造一个暂时的粒子集 $\overline{\chi}$ 来表示置信度 $\overline{bel}(x_t)$ ,随后,它将这些粒子转换为粒子集 $\chi_t$ 用它近似后验分布 $bel(x_t)$ 。

未完待续……