卡尔曼滤波算法总结

自己学习整理卡尔曼滤波算法，从放弃到精通

kaerman 滤波算法

卡尔曼滤波是非常经典的预测追踪算法，是结合线性系统动态方程的维纳滤波，其实质是线性最小均方差估计器，能够在系统存在噪声和干扰的情况下进行系统状态的最优估计，广泛使用在导航、制导、控制相关领域。

使用范围及作用

一般的滤波算法是频域滤波，而卡尔曼滤波是时域滤波。
不要求系统的信号和噪声都是平稳的，但默认估计噪声和测量噪声均为白噪声，这样其均值和方差能够更好控制。但该系统仅适用于线性系统，并可以对该系统的走向做出有根据的预测。
在连续变化的系统中效果更佳理想，由于其只对前一个状态量进行保存，不需保留其他数据，占用内存小，速度快，适合处理实时问题和嵌入式系统。

卡尔曼滤波算法核心思想

核心思想：根据本时刻的“测量值”和上一时刻对本时刻的“预测值”和“误差”进行加权平均，得到本时刻的最优量，并对下一时刻的值进行预测。
预测+实测+修正的过程；
误差(噪声)即为预测误差（上时刻对本时刻的期望）和测量误差(该时刻的实际测量值)，其独立存在，不受测量数据的影响。
上一时刻的预测值和本时刻的测量值均为正态分布，将两次正态分布进行融合，能够更为准确的得到运动位置。
卡尔曼滤波是首先对系统构建隐马尔可夫模型。隐马尔可夫模型的状态一般是无法直接观测到的，需要分为状态转移方程和观测方程，之后利用贝叶斯定理进行实现。 我们先根据以往的经验预估一个"先验概率"P(A)，也就是卡尔曼的后验估计值，然后加入新的信息（实验结果B），即测量值，这样有了新的信息后，我们对事件A的预测就更加准确。

预测过程

图中灰色部分表示上一时刻的估计值，通过上一时刻的估计值预测得到本时刻的预测值如橙色部分，蓝色部分为本时刻的实际测量值，通过对预测值和测量值进行加权平均的到该时刻的最优值。

卡尔曼滤波方程

kalman滤波的理论框架是全概率法则和贝叶斯法则，在设定中假设预测和感知均有误差，且均服从正态分布，且预测过程(预测未来)和感知过程(修正当下)采用不同的概率更新策略，具体采取的策略如下所示：
预测过程符合全概率法则，是卷积过程，即采用概率分布相加；
感知过程符合贝叶斯法则，是乘积过程，即采用概率分布相乘；
预测：
1.预测状态方程
目的:根据系统状态变量k-1时刻的最优值以及系统输入计算得到系统的预测值。
方程：

F为状态转移矩阵，当为一维数据时，值为1；B为控制矩阵，U为当前状态的控制增益,一般情况下并不存在。
2.预测协方差方程
目的：根据k-1时刻的系统协方差预测k时刻系统协方差。
方程：

Q为系统过程的协方差；

更新：
1.卡尔曼增益方程(贝叶斯滤波)
目的：根据协方差矩阵的预测计算卡尔曼增益，使得最优估计值的方差最小。
方程：

H为观测矩阵，当为p为一维矩阵时，其值为1；R为测量噪声的协方差矩阵，其数值与测量仪器相关。
贝叶斯公式：P(A∣B)=P(B)P(A)×P(B∣A)
先验概率 P ( X ：仅仅依赖主观上的经验，事先根据已有的进行推断
后验概率 P ( X ∣ Z )：是在相关证据或者背景给定并纳入考虑以后的条件概率
似然 P ( Z ∣ X )：已知结果区推测固有性质的可能性
卡尔曼增益公式：卡尔曼增益=估计误差/（估计误差+测量误差）
Q越小，K值越小，预测噪音的不确定性变小，越相信估计值；Q越大，K值越大，预测噪音的不确定性变大，越相信观测值。
2.更新最优值方程（卡尔曼滤波的输出）
目的：根据状态变量的预测值和实际系统的测量值计算得到k时刻状态变量的最优值。
方程：

3.更新协方差方程
目的：为了求解k时刻的协方差矩阵（得到k+时刻的卡尔曼输出值做准备）。
方程：

I为单位矩阵。

卡尔曼滤波的使用

1. 选择状态量、观测量
2. 构建方程
3. 初始化参数（Q,R, P0和X^0）
4. 代入公式迭代
5. 调节超参数

卡尔曼增益

卡尔曼增益=估计误差Q/（估计误差Q+测量误差R）

根据如图拓扑图可以看出，影响k值的大小主要为q和r值对其的影响。因此对q和r值的影响决定了系统更加信任预测值还是测量值。

Q和R的调节

Q表示过程矩阵的协方差，是状态转移矩阵与实际过程之间的误差。调参思路：在某些稳定的过程中可以假定它是固定的，通过寻找最优的Q值使滤波器获得更好的性能，Q一般为对角阵，且对角线上的值很小，使得可以快速收敛；在对高速行驶中的车辆系统的Q值进行估算时，使用最大加速度对Q值进行确定。
R表示测量噪声协方差，是一个数值，与仪器相关的一个特性。R值越小，收敛越快。可以通过实验手段寻找合适的值。
从小到大调整Ｑ值使系统收敛速度正常，从大到小调整Ｒ值使输出结果接近真实。

依据对预测值和观测值的信任程度，对K中的Q和R的参数进行调节.
Q为过程噪声的协方差，若运动模型不准确，增大Q的值；
R 为观测噪声的协方差，根据传感器的准确，则此时R值较小；
R固定，Q越大，代表越信任测量值，Q无穷代表只用测量值；反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测。

手工调参：
如果系统模型比较简单的话，

1. 限制P的值，防止P趋近去0；
2. 适当增加Q的值，这样P的值也相应增加
3. 给P乘以一个适当的因此，这样确保K值在一个恰当的范围内

离线学习
如果模型比较复杂，或者精度要求高，可以使用离线学习的方式，可以使用kalman smoother。该办法为卡尔曼滤波算法的个别情况：一般的卡尔曼滤波是利用过去的估计值和现在的测量值来估计将来的状态，而smoother是在已经获得的数据包括未来状态的情况下，估计过去时间下的状态。但该方法主要用于不需要实时处理的情况。

参数取值方法？

如何判断调试效果变好？

由于Q和R均为协方差矩阵，其对角线元素开根号就是标准差“1σ”，若卡尔曼增益正确，则误差点的66%应该落在“±1σ”界限之内。在知道真值的情况下，看估计值是不是更精确了；如果有随机噪声的存在，使用蒙特卡罗判据进行判定。

对于移动中的小车可以使用加速度的方差对Q进行估计；将传感器的准确值进行转换就可以得到R的范围。

实例说明

卡尔曼滤波优缺点

优点：采用物理意义较为直观的时域状态空间；仅需要前后两步的数据、数据的存储量小；使用比较简单的递推算法，便于在计算机上实现；不仅适用于平稳过程，还可以推广到非平稳随机过程的情况。在目标跟踪应用中，使用卡尔曼滤波器对系统进行预测，可以有效地解决目标移动过程中出现遮挡导致目标丢失的情况。
缺点：不能够应用于非线性系统；

衍生卡尔曼滤波

卡尔曼方程推导过程