卡尔曼滤波KF、扩展的卡尔曼滤波EKF

写在前面

本文参考《概率机器人》第三章

首先说明一下KF和EKF能用来做什么，目前从我的角度来看，是因为研究slam问题来学习卡尔曼滤波。从直观上讲，他能解决一些复杂的状态估计问题。从移动机器人的角度来讲，由于机器人的运动由控制器控制会带来控制偏差，机器人的感知会因为传感器的测量带来偏差，这就给机器人系统带来了不确定性。KF和EKF就可以针对机器人系统的之中不确定性，为机器人的运动和观测引入高斯噪声模型，可以让机器人系统在诸多不确定性中依然能很好的运行。接下来详细介绍KF和EKF的理论部分基础公式。

注：本文只针对涉及到的起源与结论公式讲清楚问题，关于细节的KF和EKF的数学证明和详细推导不做过多说明。在工程使用中应该可以满足需求，如关注其数学理论推导请参考《概率机器人》原文。

KF

由于机器人系统中存在着诸多不确定性，真实世界中的机器人并不能通过算法来准确无误的达到预定的位置，所以对于机器人的状态，可以定义一个区间，在这个状态区间内，机器人处于区间内的状态的概率通常认为其服从高斯分布。
下面定义机器人的运动方程：

式中x为机器人的状态，矩阵A，B分别为状态转移矩阵和控制矩阵，u为控制向量，后面一项为高斯随机向量，表示机器人运动控制的不确定性，其中高斯噪声参数为u,σ。近一步可以得到装态x的后验分布概率密度函数为：

与上述运动方程的逻辑一样，定义观测方程：

上式中C为观测矩阵，最后一项为高斯噪声项，则得到观测方程的后验分布概率密度函数：

假设机器人初始的状态也是服从高斯分布的：

这样对于卡尔曼滤波的数学逻辑就定义好了。在开始介绍KF的流程之前先说明一下公式，关于多维的高斯函数，一定要清楚他的意义。
根据上述的运动、观测方程我们可以知道，机器人状态是存在不确定性的。KF的做法就是，根据运动去估计状态，根据观测再去更新状态。具体的算法流程如下：

其中K是由观测矩阵和观测噪声的协方差计算的卡尔曼增益，体现了根据观测更新状态的做法，其具体的推导感兴趣的话参考原文。下面图片将展示一个简单的卡尔曼滤波示例：

EKF

认真观察的话，会发现在上述的机器人运动、观测方程中，他们的表达式都是线性的，所以在机器人上一个状态服从高斯分布，经过一个线性变化后，新的状态又服从一个新的高斯分布，这样就得到了新时刻的状态估计。但是在实际机器人的运动中，几乎不存在线性的运动方程和观测方程，大多都是非线性的，那么上一时刻状态服从的高斯分布经过一个非线性的变换会怎么样？通过下面第一二幅图的比较，高斯分布经过线性变换之后是一个新的高斯分布，经过一个非线性变换之后，其已经不是一个高斯分布。

这样的话就说明在现实世界中KF是不能解决实际的机器人状态估计问题，怎么办呢？一种重要的思想就是线性化（线性近似）。
现在我们重新定义机器人的运动方程和观测方程：

这里的g和z都是非线性的。对于这两个非线性函数，可以通过泰勒展开线性化，泰勒展开可以由g的值根和斜率构造一个函数g的近似函数。其斜率用偏导数表示：

对于非线性函数g在μ t-1经过泰勒展开可以得到：

那么上式就是一个线性变换，近一步就可以得到t时刻状态x的后验分布概率密度函数：

根据上述运动方程的泰勒展开线性化思想可以得到观测方程的泰勒展开：

其中斜率：

后验分布概率密度函数：

经过线性化之后，就可以像KF一样进行状态的估计和更新了。下面给出EKF的流程：

结语

通过上述的讨论已经了解了KF和EKF在理论上解决了什么问题，以及为什么要提出EKF（非线性问题线性化）。最后通过原文中的一张图，来说明KF和EKF在算法流程和计算上的对比：

注：其实自己在之前已经学习了卡尔曼滤波的原理，但是由于被雅可比矩阵搞晕了，结果系统的看了所有公式之后，什么协方差矩阵，什么雅可比矩阵，也就那么回事，甚至我想做过多的解释都写不出来。