RBPF的滤波过程

RBPF（Rao-Blackwellized Particle Filter）是一种基于粒子滤波（Particle Filter）的贝叶斯滤波方法，其滤波过程如下：

初始化粒子集合（particles）：根据先验知识，生成一组初始粒子，每个粒子代表了一个可能的系统状态。
预测（prediction）：根据系统的动态模型，对每个粒子进行状态预测，得到新的粒子集合。这个过程相当于对粒子进行一次状态转移。
权重计算（weighting）：对于每个新粒子，根据其代表的状态与观测数据的匹配程度，计算其权重。这个过程相当于对粒子进行一次似然度计算。
重采样（resampling）：根据粒子的权重，对粒子集合进行重采样，使得权重大的粒子被多次采样，权重小的粒子被少次采样。这个过程相当于对粒子进行一次选择、替换和复制。
条件化（conditioning）：对于重采样后的粒子集合，根据其状态计算滤波结果。这个过程相当于对重采样后的粒子集合进行一次条件期望计算。
返回滤波结果。

在RBPF中，重采样和条件化的过程被分离开来，其中重采样部分使用普通的粒子滤波算法，而条件化部分则使用了Rao-Blackwellization技术，通过对粒子的局部状态进行精确计算，提高了滤波的效率