【滤波算法】KF及其衍生
1. KF家族（和优化的比较）
卡尔曼滤波器是1958年卡尔曼等人提出的对系统状态进行最优估计的算法。随后基于此衍生了各种变体算法，比较常用的有扩展卡尔曼滤波EKF、迭代扩展卡尔曼滤波IEKF、误差状态卡尔曼滤波ESKF、无损卡尔曼滤波UKF、粒子滤波PF等等。述算法的详细描述以及它们相对于KF算法的改进，以及它们的优缺点和适用范围总结。
- 扩展卡尔曼滤波（EKF）：
EKF是一种基于泰勒级数展开的近似线性化方法，适用于状态变量和测量变量之间存在非线性关系的情况。具体来说，EKF通过将非线性函数在当前状态的一阶泰勒级数展开，将问题转化为一个线性卡尔曼滤波问题。这样可以利用卡尔曼滤波的优势来处理具有非线性关系的系统。
相对于KF算法的改进：EKF能够处理非线性系统，而KF只适用于线性系统。
优点：计算简单、易于实现；能够处理非线性系统，对多数非线性问题的解决效果很好。
缺点：由于该方法是基于一次泰勒级数展开进行近似的，因此对于高度非线性的系统，可能会导致精度下降。
适用范围：适用于小幅度非线性问题，在一定程度上也可用于中等幅度非线性问题。
- 迭代扩展卡尔曼滤波（IEKF）：
IEKF是一种对EKF进行迭代求解的方法，通过多次迭代使得估计结果更加准确，但是计算量相应地增加了。它采用的是迭代线性化技术，即在每个迭代步骤中，都使用当前状态下的最优估计来近似非线性函数，并更新系统的状态和协方差矩阵。
相对于KF算法的改进：能够处理非线性系统，精度比EKF更高。
优点：相对于EKF，精度更高。
缺点：计算量大，需要反复迭代。
适用范围：适用于中等幅度非线性问题，在一定程度上也可用于较大幅度非线性问题。
- 误差状态卡尔曼滤波（ESKF）：
ESKF是一种将状态向量分解成状态值和状态偏差两部分，并分别进行滤波的方法，能够有效地避免非线性问题带来的精度下降。具体来说，ESKF先将状态向量表示为真实状态值与状态偏差的和，然后对两者进行分别滤波，最终根据滤波结果计算出估计状态。ESKF通常需要先进行一次EKF或UKF来初始化状态向量的协方差矩阵。
相对于KF算法的改进：能够处理非线性系统，精度比EKF更高。
优点：相对于EKF，精度更高；能够处理非线性系统，并且不需要进行近似，计算效率较高。
缺点：需要进行初始化，而且初值的选取对结果有影响。
适用范围：适用于小到中等幅度非线性问题。
- 无损卡尔曼滤波（UKF）：
UKF是一种基于权值采样的非参数滤波方法，不需要对非线性函数进行任何近似，因此适用于高度非线性的系统。UKF通过对状态向量进行采样，然后利用这些样本点来逼近系统的状态分布，并根据重要性权值对这些采样点进行加权，以得到对系统状态的估计结果。相比EKF和IEKF等方法，UKF不需要对非线性函数进行泰勒级数展开或迭代近似，能够更好地处理高度非线性的系统。
相对于KF算法的改进：能够处理非线性系统，精度比EKF更高。
优点：能够处理高度非线性的系统，并且不需要对非线性函数进行任何近似；相比于EKF和IEKF等方法，精度更高。
缺点：计算量大，需要大量的随机采样；由于采样数目有限，可能会导致估计误差较大。
适用范围：适用于高度非线性的问题，但是需要注意计算效率和求解精度之间的平衡。
- 粒子滤波（PF）：
PF是一种基于粒子的蒙特卡罗方法，通过从状态空间中抽样一组代表性的粒子，并根据重要性权值对它们进行加权，来估计系统的状态。具体来说，PF首先根据先验概率分布从状态空间中抽取一定数量的粒子，然后利用粒子在状态转移方程和观测方程下的演化来更新粒子的状态，并计算每个粒子的权值，最终根据这些权值来计算系统状态的估计结果。
相对于KF算法的改进：能够处理非线性系统，精度比EKF更高。
优点：能够处理非线性系统，并且不需要进行近似；能够有效地利用先验信息和观测数据，提高估计精度。
缺点：计算量大，需要大量的粒子采样；由于采样数目有限，可能会导致估计误差较大。
适用范围：适用于高度非线性的问题，但是需要注意求解精度和计算效率之间的平衡。
- 非线性优化：非线性优化是将当前时刻所有状态构建代价函数，进行迭代优化（剃度下降：牛顿法、高斯牛顿法、列文伯格-马夸尔特法等），每次迭代会把上一次迭代的结果作为新的工作点进行优化。因此可以考虑到整个系统所有约束，精度高，但是速度就慢，因此也有提出一些加速方法，例如：滑动窗口等。
异同：优化和滤波都是解决非线性最小二乘问题，只是方式不同，对精度和速度的取舍不同，也有证明IEKF和高斯牛顿针对同一个问题的求解能力是一样的，可以参考文章:(Bell, Bradley M., and Frederick W. Cathey. “The iterated Kalman filter update as a Gauss-Newton method.” IEEE Transactions on Automatic Control 38.2 (1993): 294-297.)
下面是对上述算法的总结表格：
| 算法名称 | 优点 | 缺点 | 适用范围 |
|---|---|---|---|
| EKF | 计算简单、易于实现；能够处理小幅度非线性问题 | 对高度非线性问题精度可能下降 | 小幅度非线性问题 |
| IEKF | 相比于EKF，精度更高 | 计算量大，需要反复迭代 | 中等幅度非线性问题 |
| ESKF | 相比于EKF，精度更高；能够处理非线性系统，计算效率较高 | 需要进行初始化，初值选取对结果影响较大 | 小到中等幅度非线性问题 |
| UKF | 能够处理高度非线性的系统，并且不需要进行近似；相比于EKF和IEKF等方法，精度更高 | 计算量大，采样数目有限，可能会导致估计误差较大 | 高度非线性问题 |
| PF | 能够处理非线性系统，并且不需要进行近似；能够有效地利用先验信息和观测数据，提高估计精度 | 计算量大，需要大量的粒子采样，采样数目有限，可能会导致估计误差较大 | 高度非线性问题 |
2. 算法实例
IMU+GPS
ESKF GPS+IMU