slam 滤波

一、卡尔曼滤波

二、误差卡尔曼滤波

原文地址：Error-State Kalman Filter理解与梗概推导 - 知乎 (zhihu.com)

三、扩展卡尔曼滤波

        卡尔曼滤波与扩展卡尔曼滤波公式对比如下。

        扩展卡尔曼滤波，用一阶泰勒展开，来对非线性系统进行线性化。Fk和Hk都为雅克比矩阵。

        详细的讲解，请参考如下博文扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）_ekf滤波实例-CSDN博客

四、无迹卡尔曼滤波

        UKF生成了一些点，来近似非线性。由这些点来决定实际x和P的取值范围。
        UT变换：假设一个非线性系统y = f ( x )，其中x 为n 维状态向量，并已知其平均值为x ‾ ，方差为P x ，则可以经过UT变换构造2 n + 1 个Sigma点x i  ，同时构造x i 相应的权值W i ，进而得到y 的统计特性（均值和方差）。 有点类似于概率论里已知x的均值和方差，求y = f ( x ) )的y 的均值和方差。

UKF的算法步骤如下：

         以上的内容都来自于如下博客。

        无迹（损）卡尔曼滤波（UKF）理论讲解与实例_无迹卡尔曼滤波估算例子-CSDN博客

        此博客讲的比较详细，且以slam实际例子进行了示范。

五、粒子滤波

        5.1 算法思想

        粒子滤波(Particle Filter)不需要对状态空间进行区间划分。粒子滤波算法采用很多粒子对置信度进行近似,每个粒子都是对t时刻机器人实际状态的一个猜测。越接近t时刻的Ground Truth状态描述的粒子，权重越高。粒子更新的过程类似于达尔文的自然选择(Natural Selection)机制，与当前Sensor测量状态越匹配的粒子，有更大的机会生存下来，与Sensor测量结果不符的粒子会被淘汰掉，最终粒子都会集中在正确的状态附近。

        5.2 自动驾驶实例

粒子滤波(Particle Filter)的主要步骤如下:

1）Initialisation Step：在初始化步骤中，根据GPS坐标输入估算位置，估算位置是存在噪声的，但是可以提供一个范围约束。

2）Prediction Step：在Prediction过程中，对所有粒子(Particles)增加车辆的控制输入(速度、角速度等)，预测所有粒子的下一步位置。

3）Update Step：在Update过程中，根据地图中的Landmark位置和对应的测量距离来更新所有粒子(Particles)的权重。

4）Resample Step：根据粒子(Particles)的权重，对所有粒子(Particles)进行重采样，权重越高的粒子有更大的概率生存下来，权重越小的例子生存下来的概率就越低，从而达到优胜劣汰的目的。

原文地址：自动驾驶定位算法(十三)-粒子滤波(Particle Filter) - 知乎 (zhihu.com)