卡尔曼滤波的详细推导与再认识

前言

在跟随杨亮老师学习SLAM的过程中，重新学习了一下数据融合的基本算法，卡尔曼滤波算法，之前一直是单纯地根据别人给出的公式，进行套用，而且一些基本的参数，都不能很清楚的理解。这次趁课程学习的机会，自己终于把这些基础细节都理清了，并详细地推导了一遍卡尔曼滤波。作为一种学习过程的记录，码在这里和大家分享。

（图片来源于网络，感谢）

贝叶斯估计

大家在看卡尔曼滤波文章时，总会看到好多关于贝叶斯概率公式的事情，但是又不太理解，最后文章中给出的卡尔曼滤波公式里面，明明没有直接用到贝叶斯概率的公式，因此会对这个贝叶斯估计产生困惑，至少我是如此。而现在才明白，其实，贝叶斯概率公式正是卡尔曼滤波的起点。

首先，读者可能对这个概念有点模糊，贝叶斯的概率估计是干什么用而提出的呢？

贝叶斯估计的提出，是为了解决“逆向概率”问题的。
举个例子，某产品在机器正常时的生产的合格率为0.9，在机器异常时的生产的合格率为0.6。机器的故障率为5%=0.05；
正向概率就是统计得出的机器正常运转时的产品合概率为0.9，机器异常时生产产品的合格率为0.6，这些都是根据已有的生产产品情况统计出来，正常生产1000件，900件合格，机器异常时生产100件，60件合格。
而逆向概率是。通过我的产品，反向预测我的生产机器的状况，即我生产的产品合格的情况下，生产的时候是正常的概率：
计算：求解产品合格情况下，机器正常的概率，既是求在产品合格时，机器正常所占的比例，所以，所求就是等于机器合格且正常的情况除以所有合格的情况，具体计算过程如下，表述的灵感来源感谢。

上述公式就是我们相要引入的贝叶斯公式，它表征了先验估计与后验估计之间的关系。
上述公式的一些小注解：
P(合格)：这就是合格的先验概率；
P(合格|正常):这是合格的后验概率，表示在机器正常的情况下（或者说机器正常了之后），零件合格的概率。
P（合格，正常）=P(合格|正常)*P(合格)，这是利用全概率公式。证明的话，很简单，把P（合格）左除过去做分母，就可以从定义上理解了。

贝叶斯估计在联合概率下的拓展

在联合概率（就是P（合格，正常）这样同时发生）的情况下，贝叶斯公式可以拓展为：

利用条件该来进行推导，证明推导的过程如下：

上式是普遍的贝叶斯联合分布公式，在SLAM这种实际问题中，这些x,y,z参数都有实际的意义，参数也有了具体的变化。x表示移动小车的位姿，y变为z，表示传感器的观测量，b表示移动小车给予的控制量（速度、角速度等）。

贝叶斯联合概率公式到卡尔曼滤波的推导

上式P（x|z,b）是基于观测量和控制量来对自身位姿进行的估计，也就是我们相要达到的目标，想时刻知道我们在空间中的精确位姿。而我们无法直接这样进行估计，我们因为z,b都和x位姿有关系，他们之间有着未知的耦合关系，难以把他们理解成独立分布而求解，所以，为了将我们的概率估计公式，重新变成可以计算，我们就利用了上述的公式，将基于观测量和控制量来估计位姿可能会怎样，重新理解成右式，在我现在的控制量的情况下，怎样的位姿状态，会使我得到的测量值，最有可能变成当前的测量值。
对下式进行进一步理解：

P（z|x,b）：因为观测量z,控制b之间是独立的，b存在与否，都不会对z造成影响，所以可以理解P（z|x）;
P (z|b)：同样的，因为观测量z和控制量b之间是独立的，互不影响的，所以其就等于P（z），而我们不关心z的分布，我们想求解的是，怎样的x，才能使x最有可能成为现在的观测量z。，多以，将其作为一个系数理解。
所以，左式可以理解为右式。而右式就是卡尔曼滤波的核心。

实际的测量、控制、位姿估计中，都会有测量偏差，我们首先假设一个较为理想的情况， 就是我们所有的噪声偏差都是均值为0的高斯白噪声，基于x对z的估计，是有均值和方差的高斯分布，这个不是假设，而是在高频统计下，他们一般就是高斯分布，基于b对x的估计也是一样的。
因此(4)式可以化成他们的概率密度函数形式，目的也是为了求得x使得（4）取得最大值。

上式就是两个独立分布的正太分布的概率密度函数，因此可以对上式进行化简，将其合并为一个融合两个分布的融合分布，字母 f (fusion，表征融合后的概率分布的均值和方差)。


（6）式的结论是通过求导得到的，就是讲两个正太分布的概率密度函数的融合，下面就来具体利用导数和曲率公式推导下。

因为融合后的分布依然是正态分布，而正太分布是一个开口向下的曲线，所以曲线的上极值点，就是我们相要求解的点。而上极值点，就是该函数一阶导数等于0的点，y’(x)=0。容易从（6）式推理出，x的变化，只反映e的指数的大小上，因而求导时只求解指数部分即可（记为h(x)）。

之后令h’(x)=0，求解得出x的值，即为融合后的xf。

那么融合后的方差怎么求呢？我们依然从正太分布的图中可以看出，我们所求带的极值点正式正太分布的均值（由分布的对称性也可以进行理解），同样的，该极值点出的曲率也是最大的，因而可以从曲率的角度，来求解协方差。

又因为在此极值点处，一阶导数等于0，所以分母可以去掉，相当于，直接求解函数h(x)的二阶导数。

因此，我们通过一阶导数和曲率公式，将融合后的均值与方差都算出来了，也就意味着，我们知道了，在融合后的概率函数的均值处，满足我们的最优估计的需求。我们将均值与方差总结出来，并进一步变形，可以得到：

将均值和协方差整理为为上式的形式，就是为了重新理解均值与方差在融合了测量与估计后的意义。

对均值而言：就是在上一步预测的基础上，加上一个带有权重的测量值差。权重用方差来表征，而方差就是测量和预测的不确定度的关系，测量m所用的传感器方差小，测量精度高，则权重δm的值小，则系统的权重会变大，相当于更相信测量值m，反之亦然。从现实角度来讲，也很合理，谁的精度高，我相信谁。
对方差而言：分析也是类似的，谁的方差小，谁对方差的影响就越大，谁对融合后的方差缩紧效果就越明显。
而接下来，在结合实际的状态量，将x变成n维1列向量，就可以将其变换到卡尔曼滤波算法了。测量值m比如是通过激光、视觉等测量的，得到的测量值就会有对应的表示形式如（r,θ），（u,v）等，为保证能够与状态量x进行计算，因此需要通过变换矩阵H，将真实空间的测量值映射到类似x的形式。

上式的协方差矩阵理论上应该写成左转置右乘的形式，但为接下来的推导方便，先这样描述着。之后我们令上述（11）式中所说的协方差权重系数式，记为一个变量K，然后化简一下（11）式。

上述的权重系数K就是卡尔曼滤波中的卡尔曼增益K的由来之后的x以及δ2，就是卡尔曼滤波中的迭代的量，上述就是卡尔曼滤波算的核心，实现了将两种分布的数据进行融合，得到了一个方差更小的融合后的值。之后，在卡尔曼滤波算法的更新迭代部分中，就是利用上述的推导完成的。

卡尔曼滤波算法

卡尔曼滤波算法的目的是融合不同的观测类型的数据，把不同种类的数据进行融合，然后得到一个更好的，更符合测量实际的数据。卡尔曼滤波算法的核心就是预测、融合与迭代。
下面我们通过卡尔曼的普遍公式，来进行进一步阐述上一章节中提出的公式（13）。
首先：基于上一时刻的数据，预测当前时刻的数据，并更新对应的斜方差矩阵：

这个变换矩阵F就包含着式（5）中，基于控制量b对自身的估计，例如，F在移动小车方面，可以映射加速度、速度对位置状态的影响。

其次，表征预测（估计）量与测量值之间的误差关系:


到这里，这些公式就是在做公式（5）中的y函数所表示的意思，考虑了当前状态，预测量和测量值之间的，将他们分别进行融合，目的是求解怎样的x,才能在已经发生状态变换b和已经过去状态xk-1的情况下，使得当前的测量值xk与测量值z之间的误差函数最小。也就是我们一开始所列的条件概率的方程（式（5））。
接下来的就是带入这些求得的参数进行求解，而求解的参数与公式就是我们在上述的式（13）中所表述的，这也就是为什么贝叶斯与卡尔曼滤波如此紧密结合的原因，卡尔曼滤波只是贝叶斯公式的一种具体表达的形式：

求解完K时刻的状态量后，继续将当前的协方差信息进行更新，以进行下一次的预测与更新，卡尔曼滤波实现这种基于上一时刻的位姿、当前时刻的状态控制以及当前姿态的测量值，进行位姿估计。

卡尔曼滤波到扩展卡尔曼滤波的扩展

一般来说，卡尔曼滤波适用于线性方程，而扩展卡尔曼滤波可以适用于非线性的状态方程。这样叙述的原因就在于对状态方程而言，卡尔曼滤波可以用状态变换矩阵Fk直接将上一时刻的状态映射到当前状态，而对于一些非线性的函数，我们无法用矩阵进行描述，因为矩阵的本质就是线性映射。所以，我们需要用函数重新改写我们的状态方程。

相应地，计算测量值与预测值得差值的方程，也要进行类似改写：

那大家可能会产生疑问，这样用函数表示，会使系统求解困难，怎么办呢？采用一阶线性近似，用对应函数的雅克比矩阵，带入协方差公式继续计算。计算的过程就和卡尔曼滤波十分相似了。

结论

之前一直搞不懂贝叶斯在卡尔曼滤波中的作用，这次在杨亮老师的教学视频点播下，系统地梳理了一遍贝叶斯估计与卡尔曼滤波的关系，自己很受启发，推导过程也非常详细，至少自己理解的更深入了些。在表述上，没有完全把读者当做完全的新手，因而新人的可读性会很差，但耐下心来，会有收获的。