腿足机器人之五- 粒子滤波

上一篇博客使用的是高斯分布结合贝叶斯准则来估计机器人状态，本篇是基于直方图和粒子滤波器这两种无参滤波器估计机器人状态。

直方图方法将状态空间分解成有限多个区域，并用直方图表示后验概率。直方图为每个区域分配一个单独的累积概率；可以将其视为对连续密度函数的逐段常数近似。第二种技术通过有限多个样本来表示后验概率。由此产生的滤波器被称为粒子滤波器，在某些机器人问题中获得了极大的流行。

直方图滤波器和粒子滤波器，都不对后验密度做强烈的参数假设。因而非常适合表示复杂的多模态信念（后验状态概率估计）。因此，当机器人需要应对全局不确定性的阶段，以及面对产生多个独立假设的困难数据关联问题时，它们通常是首选方法。

直方图滤波

直方图滤波器的离散形式称为离散贝叶斯滤波器，其算法流程如下：

其中$u_t$是控制信息，$z_t$是传感器观测值， { P k , t − 1 } \{P_{k,t-1}\} {Pk,t−1​}，其中的大括号表示这个概率是一个集合，$t-1$表示的是时刻，k表示的是状态$x$处在k时，$p_{k,t-1}$表示t-1时刻，机器人处于状态k的概率，然后大扩号表示这是要遍历机器人所有可能状态的集合。

第3行根据t时刻控制信息$u_t$和前一时刻状态$x_i$估计t时刻处于状态k的概率${\overline{p}}_{k,t}$，这是一个先验概率，第4行根据传感器观测值修正先验概率，得到状态的后验估计$p_{k,t}$

上面的离散的可能状态如何求得？分为静态和动态法，静态分解技术依赖于一个预先选定的固定分解方案，这种方案在分解过程开始前就已经确定，且不考虑被近似的后验概率分布的具体形状。动态分解技术会根据后验分布的具体形状来适应性地调整分解结构。这类技术能更有效地针对状态空间中后验概率较大的区域提供更高的分辨率，而在概率较低的区域采用更粗糙的分解，从而节省计算资源并提高效率。递归地分解状态空间并调整分解的精细度的密度树就是一种动态法。

粒子滤波

在粒子滤波方法中，后验概率是一些离散的点，称为粒子（particles），其可以表示为：
$${\mathcal{X}}_t:=x_t^{[1]},x_t^{[2]},\cdots ,x_t^{[M]}$$

每一个粒子$x_t^{[m]}$都是t时刻状态的一个可能分布。通常粒子的状态总数M是一个很大的数，如M=1000，在一些实现中M也是时间或者和后验概率有联系的其他变量的函数。

粒子滤波器的核心思想是用一组粒子 $({\mathcal{X}}_t)$近似后验概率分布 $(\text{bel}(x_t))$。理想情况下，某个状态假设 $(x_t)$ 被包含在粒子集 $({\mathcal{X}}_t)$ 中的概率应与其在贝叶斯滤波后验中的概率成正比：
$$x_t^{[m]}\sim p(x_t\mid z_{1:t},u_{1:t})$$
粒子滤波的算法流程如下：

第2行~6行创建一个临时的粒子集 $(\overline{{\mathcal{X}}_t})$，这个集合中的粒子是从先前粒子集${\mathcal{X}}_{t-1}$对称处理得到。
第3行要遍历M个粒子；
第4行采样，即对于每一个粒子 $(x_t^{[m]})$，首先基于过渡模型 $(p(x_t|u_t,x_{t-1}^{[m]}))$从前一状态 $(x_{t-1}^{[m]})$ 生成一个新的粒子样本。这表示根据控制输入 ( $u_t$ ) 和之前的状态，预测当前可能的状态。
第5行权重更新，每个新采样的粒子 $(x_t^{[m]})$ 的权重 $(w_t^{[m]})$ 由观测模型 $(p(z_t|x_t^{[m]}))$ 计算得出。这个权重反映了给定当前观察 $(z_t)$ 下，粒子 $(x_t^{[m]})$ 的合理性或“拟合度”。
第8~11行是粒子滤波的重采样部分，这一过程在算法实现中非常关键，用于更新粒子集以更好地代表后验概率分布。每个粒子的重要性权重决定了其被重新抽取的概率。这些权重是根据每个粒子相对于观测数据的似然性来分配的，即反映了每个粒子如何适应观测数据的程度。