卡尔曼滤波KF-EKF-UKF-PF-如何通过数据来估计自身状态

卡尔曼滤波核心：数据融合算法，将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据融合在一起，得到一个更精确的目的测量值。

 

卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小，能够利用前一时刻的状态（和可能的测量值）来得到当前时刻下的状态的最优估计。

实现过程：使用上一次的最优结果预测当前的值（先验估计），同时使用观测值（传感器的结果）修正当前值(先验估计）。

1.预测：先通过上一次的最优估计得到本次先验估计，同时更新先验估计的协防差矩阵。

2.更新：通过先验的协防差矩阵计算本次卡尔曼增益，然后卡尔曼增益x(观察值-先验估计）+先验估计得到最优估计，然后更新最优估计的协防差矩阵。

举例来理解：

1.先验估计--就是通过运动方程建立的状态空间方程

其中先验估计就是根据公式得到的：

2.计算先验估计协防差--其中先验估计协防差关系为

 将先验估计带入得到先验估计的协防差，同时噪声wt的协防差为Q

 

3.通过卫星测量值

 

4.状态更新--修正估计

核心就是根据卡尔曼增益对最优估计值和测量值进行加权操作

 5.更新卡尔曼增益--R为噪声

卡尔曼增益的实质：就是最优估计值的误差/（最优估计误差+测量误差R）

6.更新后的后验估计协防差

 二.调节超参数

 传感器精度高，更信任观测值，R减小，K越大，更信任观测值

运动模型很完美，无摩擦，R增大，Q增大，K越小，更信任预测值

Q--过程噪声的方差 ，在理想光滑的场景下，Q可以适当小一点，粗糙摩擦力大则Q可以大一点

R--观测噪声的方差，观测噪声由传感器决定的，传感器精度高R可以小一点。

 卡尔曼滤波的应用--陀螺仪滤波

 注：H为系统观测参数[1，0]

 

 

KF是线性滤波器

EKF扩展卡尔曼滤波--就是将非线性部分线性化---泰勒展开

 正态分布的随机变量通过非线性系统后就不再是正态的，于是需要在上一次的后验估计处线性化

于是线性化使得A为雅克比矩阵 得到线性化方程为：

非线性与线性的比较：预测直接采用非线性，Q更改，R也更改，后验的预测值也要改，

其中A和H为雅克比矩阵

EKF面临的一个重要问题是，当一个高斯分布经过非线性变换后，如何用另一个高斯分布近似它？按照它现在的做法，存在以下的局限性：（注意是滤波器自己的局限性，还没谈在SLAM问题里的局限性）。

1. 即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
2. 系统本身线性化过程中，丢掉了高阶项。
3. 线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的A，H，也不是最好的。
4. 在估计非线性输出的均值时，EKF算的是的形式。这个结果几乎不会是输出分布的真正期望值。协方差也是同理。

希望维持高斯分布假设，可以这样子改：

1. 为了克服第3条工作点的问题，我们以EKF估计的结果为工作点，重新计算一遍EKF，直到这个工作点变化够小。是为迭代EKF（Iterated EKF, IEKF）。
2. 为了克服第4条，我们除了计算，再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。

小结

总体来说，EKF的特点是易于使用，适用性广而且计算效率比较高。但他的局限性在于只适用于局部线性化，旨在估计非线性强度不是很高的模型。另一个缺陷在于它没有考虑状态方程线性化过程中出现不确定性的情况。且由于需要对雅可比矩阵求导，非线性项在量测和状态方程里必须可导（不过这一点我觉得很容易满足不算劣势）。

总之，对于非线性问题，EKF除了计算量大，还有线性误差的影响，所以这里引入UKF，相比之下UKF在非线性强度上就没有太多局限。

UKF 无迹卡尔曼滤波-基于无迹变换利用概率分布逼近非线性函数

主要解决一个高斯分布经过非线性变换后，怎么用另一个高斯分布近似它

 UT变换--计算2n+1个sigma点和相应的权值来计算非线性函数y的统计特征：

 举例如下：

 

1.第一步，获取sigma点集：

 2.利用上面的Xsigma,带入模型中预测k时刻计算k+1时刻的状态：

3.通过权值对sigma点进行预测，计算均值与协方差

 4.根据预测值再做一次UT变换，等到新的sigma点集：

 5.计算观测均值和方差

 6.计算Kalman增益：

 7.状态和方差更新：

 小结

我们可以看出来UKF没有采用线性化过程，这使得许多人认为它的适用性更广，能够对不可导的函数进行应用，然而实际在许多分段函数模型比如censored measurement model (审查量测模型)等模型中，sigma的选点会导致不连续附近输出的均值和协方差产生偏差，同时初始统计结果也不再适用，进一步会导致结果的发散和有偏。这也就是为什么现在诞生了很多加窗滤波和强跟踪滤波的原因。

PF粒子滤波

UKF用少数点来近似状态分布。但它依然在卡尔曼滤波对系统的线性、高斯假设下。当系统的线性程度很差的时候，卡尔曼滤波的效果也不会太好。而且卡尔曼滤波很难实现全局定位。所以它主要用于位置追踪。粒子滤波属于无参数滤波，它没有具体的得到后验结果的函数，其思路是用粒子集近似系统状态的分布。所以它可以近似任何系统状态分布。这样的特性也为它处理全局定位和机器人绑架问题奠定了基础。
粒子滤波的主要步骤如下：

1.初始状态：用大量粒子模拟X(t)，粒子在空间内均匀分布
2.预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；更新粒子集的预测状态。

3.校正阶段：用观测模型对预测粒子进行评价，越接近于真实状态的粒子，其权重越大
4.重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；使粒子集向权重大的粒子靠拢。
5.滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。用粒子集的均值状态量来表征机器人的状态。

粒子滤波在动态追踪实时估计等领域没有尊严，因为它其实是一种蒙特卡洛方法对采样求数值解的过程。这个方法可以求解一般无法通过解析方法求解的方程积分。在这种方法中，首先在随机分布的基础上生产样本基于统计值的经典计算方法，然后根据平均样本值估算分布参数的基础上生成样本代替本来的样本。因此在理想情况下，极限无穷大的样本数，离散变量的期望一定可以重现连续变量的真实期望。通常，因为大量样本的计算，计算复杂度太高，不可能直接从变量的概率密度分布中获取样本，在这种情况下就采用重要性采样技术，可以生产近似的密度分布从而达到减少计算量的目的。但是，又由于样本是从重要性分布中生成的，会出misrepresentation，因此需要通过把足够的权重分配给各个样本来克服这种misrepresentation。但这又无形增加了计算量。（拆了东墙补西墙的感觉）

粒子过滤使用顺序重要性采样SIS，它是重要性采样的递归变体。其操作基于以下事实：一个计算可以在每个时间步k中以递归方式计算状态xk和权重值的重要性分布。算法SIS 使用一组加权粒子，以给出滤波分布 。这种方法的缺点是多数权重等于零，这种情况经常发生。这是所谓的退化问题，它阻碍了粒子过滤的实际实施。解决该问题的一种尝试是对重要性重要性采样SIR进行重新排序。实际上，消除了具有非常低权重（低概率）的粒子，并复制了具有高权重的粒子. (重采样的概念）

算法流程：

 PF无论计算精度和计算速度都不可与UKF和EKF相比，它更像是为了解决非高斯滤波等假设局限性受制而采取的备选方案

在很多实验中，UKF并未表现出理论上优于EKF的计算效率，那么简单来说，别拿sigma点不当小点心，每进行一次sigma点的采样，UKF都需要大量时间去处理所有sigma点，假如每个点都进行Runge-Kutta integration的话，一个3阶的矩阵每次迭代需要进行15个点的Runge-Kutta integrations。相比之下EKF只进行了一次。（谁说求一阶导就慢了！哼！EKF表示不服。）

从精度的角度来看，所有高精度都是通过增加计算量来换来的，如果UKF通过加权减少Sigma点的方法来降低计算负载，那么精度在一定程度上会低于一阶泰勒展开的EKF线性化。除了EKF和UKF之间的时间复杂性问题外，我们还需要检查它们的理论性能。从以往的一些研究中中，我们知道UKF可以将状态估计和误差协方差预测到4阶精度，而EKF只能预测状态估计的2阶和误差协方差的4阶。但是，只有在状态误差分布中的峰度和高阶矩很明显的情况下，UKF才能进行更准确的估计。在我们的应用中，四元数分量协方差的大小显着小于统一性（在大多数情况下约为），这意味着峰度和更高阶矩非常小。这一事实说明了为什么UKF的性能不比EKF好。

另外，采样率也是另外一个拉小UKF与EKF差距的因素，对许多动态模型（有论文提到四元数动态）随着采样间隔的缩短，模型愈发趋近准线性化，那么越小的步长，积分步长把（四元数）传播到单位球面的偏差就越小。因此最小化了线性化误差。

最后，也是最根本的在选取EKF和UKF最直观的因素，UKF不用进行雅可比矩阵计算，但是，许多模型的求导是极为简单的，在最根本的地方UKF没有提供更优的解决方案。这也使得，状态模型的雅可比计算的简单性允许我们在计算EKF和UKF用相同的方法计算过程误差协方差。

总结，UKF并不是万能的，也不是一定比EKF优秀，很多时候需要根据情况选择特定的滤波，希望大家在学习过程中摒弃直观感受，深挖论文，考据原理。相信总有人的结果能支持你的理论。

1. 卡尔曼滤波是递归的线性高斯系统最优估计。
2. EKF将NLNG系统在工作点附近近似为LG进行处理。
3. IEKF对工作点进行迭代。
4. UKF没有线性化近似，而是把sigma point进行非线性变换后再用高斯近似。依然在高斯线性的约束下。
5. PF为无参数滤波，去掉高斯假设，以粒子作为采样点来描述分布。
6. 优化方式同时考虑所有帧间约束，迭代线性化求解。

粒子滤波（PF）原理详解_cofisher的博客-CSDN博客_pf滤波器