参考自B站MATLAB中国及《视觉SLAM十四讲》第9章9.1
1.卡尔曼滤波器–最优状态估计
假如有一个自动驾驶汽车比赛，要求参赛汽车根据GPS测量定位分别在100种地形上行驶1公里，在每种地形上都尽量停靠在1公里终点处。计算100次的平均最终位置，取位置方差最小且平均位置最接近1公里的队伍获胜。
GPS的定位是比较粗糙的，误差较大，为了赢得比赛，期望获得尽量准确的位置估计。一个汽车系统可以简化成如下形式，输入是油门，输出是位置。
上图所示的汽车系统有很多状态量，其模型可以表示为：
只保留速度作为输入，输出为位置后的简化模型为：
上式中：为速度，
为预测量，
为实测量。
理想情况下，测量值等于预测值，所以，所以
。为了让汽车尽可能靠近终点停放，它必须尽可能准确。
实际场景还需考虑噪声，对于测量量，使用GPS测量时难免会有测量误差，记为
。同样对于预测变量
，因
与速度有关，速度有可能受风速等的影响，必须考虑这个过程噪声，记为
。虽然不能准确的确定噪声的具体值，但可以知道噪声值的概率分布服从高斯分布。
以上是实际的车辆动力学模型。我们不知道这个模型，只能根据假设的模型进行预测和计算。
这里如何根据带有噪声的测量量和有噪声的预测量来尽可能准确的估计汽车的真实位置，正是kalman滤波可以做的事情。
如上图所示，估计位置在时间
的方差小，预测位置的不确定性小。在
时刻，预测值
的不确定性变大，测量值
的不确定性相对较小。将实测量的概率分布相乘得到新的高斯分布，取平均值作为汽车在
处的位置更准确。
预测量与测量量概率密度函数的乘法与如下所示的离散Kalman滤波方程有关，
卡尔曼滤波器是为随机系统设计的状态观测器。
上式的第一部分表示为，它取决于
处的输出
和
处的输入
，如下图所示：
部分取决于前一时刻的位置估计和当前速度，因此也称为先验估计。借助
，
可以表示为：
从上式可以看出，第二部分使用的是实测量。 滤波方程由预测值和基于测量量的更新值组成。
称为
Kalman 增益系数，称为后验估计。如下所示：
题外话：关于先验估计与后验估计，可以根据字面意思理解。先验估计顾名思义就是借助先前的经验来估计，譬如1枚硬币，在扔出去之前根据经验就可以估计出来出现正面的概率是1/2。后验估计，就是要通过测量试验后才能进行的估计，譬如已知一个箱子里装了红黑两种颜色的球，但是不知两种球的比率，学过概率后都知道可以从箱子中取样，样本中红黑球的比率可当作箱子中红黑球的比率，这种通过试验来进行的估计就是后验估计。
回到汽车的例子，汽车位置估计的卡尔曼方程的先验估计和更新部分可以表示为
请记住，Q和R分别是过程噪声和测量噪声的方差，P表示预测值的协方差，对应于单变量系统的方差。卡尔曼增益用于调整状态估计中预测值与测量值的比例。当预测值更准确时，希望
的比例更大，当测量准确时，希望
的比例更大。
观察卡尔曼增益的表达式，当测量误差趋近于0时，
趋近0，可以得到估计值等于测量值，测量值的概率密度函数将变成1个脉冲值，其对应的位置就是最优估计。
再考虑当趋近于0时，此时卡尔曼增益也等于0，预测值完全取决于先验估计，与测量值无关。
卡尔曼滤波不需要所有的先验信息进行状态估计，只需要对时刻的预测和估计进行持续更新。
从前面的描述可以看出，测量值是以为基础的，增加更多的传感器，如
等，可以提高测量值的精度，获得更准确的位置估计。观察卡尔曼方程，添加传感器时，只需增加
、
、
的维度即可。因此卡尔曼滤波器也称为传感器融合算法。
2.非线性系统
第一部分介绍的简化汽车模型的卡尔曼方程对应一个线性系统。在实践中，非线性系统更为常见。如果汽车模型考虑摩擦，则预测值与速度之间的关系将变为非线性。相应的测量函数也可能与预测值呈非线性关系。
在线性系统中，服从高斯分布的预测值和测量值仍然服从高斯分布：
对于非线性系统，估计值将不再服从高斯分布，这将导致卡尔曼滤波器可能不收敛。
2.1扩展卡尔曼滤波器EKF
这里引出用于非线性系统的扩展卡尔曼滤波器(Extened Kalman Filter,EKF)，EKF将非线性函数在时刻估算状态的平均值附近进行线性化，
使用EKF时有如下缺点：
- 1）导数复杂时，雅可比矩阵的解析解不易求得
- 2）使用数值解时，雅可比计算量较大
- 3）EKF仅适用于可微系统模型，系统模型不可微分时无法求雅可比矩阵，不能使用EKF
- 4）对于高度非线性系统，EKF的结果不是最优的
- 5）对于系统某些不能线性近似的状态，EKF表现较差
2.2 无迹卡尔曼滤波器UKF
由于EKF中使用了非线性函数的线性近似，存在诸多问题，而无迹卡尔曼滤波器(Unscented Kalman Filter，UKF)通过近似预测量和测量量的概率分布而非近似非线性函数来进行状态估计。无迹卡尔曼滤波器在原概率分布上选择一组最小的采样点，使采样点的均值和方差与原概率分布相同，然后将每个采样点代入非线性函数进行计算得到输出点的均值和方差。根据输出点来计算高斯分布。
2.3 粒子滤波器
粒子滤波器(Particle Filter,PF),粒子滤波器近似任意分布，而不像UKF仅局限于高斯分布，为了估计任意的概率分布，粒子滤波器所需的采样点远远大于UKF。
3.总结
REF
1.https://www.bilibili.com/video/BV1V5411V72J?p=6&spm_id_from=pageDriver
2.https://book.douban.com/subject/27028215/
文章出处登录后可见！
