高翔【自动驾驶与机器人中的SLAM技术】学习笔记（六）卡尔曼滤波器二：图解卡尔曼滤波器；卡尔曼滤波器公式理解；面试答法；

上一篇卡尔曼滤波器一中，从整体上认识了，卡尔曼滤波器整体是在做一件什么事。

知道了，协方差就可以理解为偏差，或者误差。

这一篇主要讲卡尔曼滤波器中的公式，理解公式，就能知道如何实现卡尔曼滤波器。

上一篇：卡尔曼滤波器在做一件什么事，这一篇，卡尔曼滤波器怎么做。

---

博文一： 

为了搞懂卡尔曼我按照吃饼原理：读了大量的博文。首先让我有点理解的感觉源自：百度百科中的例子。这个可能有点反常识，但确实是百度百科，让我有点理解的感觉。

为了弄懂（理解一点），我连百度百科都没放过。但是人家写的确实不错。

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

几个重点：

• 线性系统
• 系统状态最优估计
• 最优估计也可以看做是滤波过程：滤除系统中的噪声和干扰的影响。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。 

 性质：

①卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。

②任何一组观测数据都无助于消除x(t)的确定性。增益K(t)也同样地与观测数据无关。

③当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计，也就是最小方差估计。

案例：

卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。

人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对目标当前位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

测量值有噪声，通过滤波设法去掉噪声的影响。

受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。

使估计值尽可能准确地接近真实值，这就是最优估计。

真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。

对于理解卡尔曼滤波器最有帮助的是它文中举得例子：

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的（假设），也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设预测值是23度，同时该值的高斯噪声的偏差是5度（5是这样得