【机器人状态估计】粒子滤波算法介绍

概率定位算法

问题分类：位姿追踪、局部定位、全局定位；静态、动态环境定位；单一机器人定位、多机器人定位。

贝叶斯滤波框架：
定位置信度与运动模型卷积，两次独立估计值的整合比单一估计值使系统状态确定性更高。

粒子滤波

基本思路

1. 随机产生M个粒子（如M=1000），每个粒子表示状态变量的（随机）数值，如位置、航向等，粒子有权重，即重要性因子，初始时每个粒子的权重都相等
2. 预测（运动方程）每个粒子下一时刻的”位置“
3. 根据测量更新粒子的权重，与测量更接近的粒子具有更高的权重
4. 重采样粒子，去除权重低的粒子，并用高权重的粒子去复制替代，粒子的权重被重新均分
5. 统计粒子集合的加权均值和方差以得到系统的状态估计

粒子滤波实现自动驾驶多传感器融合定位

1. 使用运动方程从上一时刻完成更新的粒子预测状态
2. 传感器测量与从预测的状态估计的测量做差, 称为innovation, 重新确定第i个粒子的权重
   $$w_t^i=p(z_t|x_t^i,m)$$

$$\begin{gathered} \delta z=h(x,m)-z \\ {w}_t^i \end{gathered}$$