【易懂教程】我是如何快速理解卡尔曼滤波Kalman Filter(KF)算法的(1)？

推荐开源项目：简单的SLAM与机器人教程与编程实践-github

牛逼的算法往往都是来源于一个很简单的思想所演化而来，如果你不了解这个思想和演化过程那就谈不上了解那个牛逼的算法——@Ai酱

直观理解

首先卡尔曼滤波要解决的问题是什么？我以机器人估计自己离障碍物距离为例
答：首先机器人已知“上个时刻机器人离障碍物距离”，“传感器测量的机器人离障碍物的距离（我们称它为观测值，比如雷达直接测量机器人离障碍物距离7m）”和“自己当前时刻的速度”这三个数据。而根据“上个时刻机器人离障碍物距离”和“自己当前时刻的速度”可以估算出当前机器人离障碍物的距离（我们称它为估计值）。比如：上一秒离障碍物10m，速度是4m/s，那么现在这秒估计就离障碍物距离是6m。那么问题来了，机器人离障碍物的距离现在既有个观测值7m，又有个估计值6m。到底相信哪个？单纯相信观测值万一那传感器坏了呢？单纯相信估计值那么万一上个时刻的距离估计值或者速度不准呢？所以，我们要根据观测值和估计值的准确度来得到最终机器人离障碍物的距离估计值。准确度高的就最终结果比重高，准确度低就占比低。如果雷达测量的那个7m准确度是90%，根据速度估计出的那个6m准确度是80%，那么最终的距离估计结果就是$result=(1-\frac{0.9}{0.8+0.9})\ast 6+\frac{0.9}{0.8+0.9}\ast 7=6.52$米.
事实上$\frac{0.9}{0.8+0.9}$这个就是所谓的卡尔曼增益。
直观理解讲完了，以上。

但是上面的90%与80%这两个准确度是怎么计算的？卡尔曼这个人到底是怎么发明卡尔曼滤波算法的？卡尔曼发明算法的思路历程是什么？卡尔曼滤波算法到底与贝叶斯滤波算法有什么联系?我想这些肯定是你脑海中挥之不去的阴影，仅仅从直观理解无法解答这些问题。

卡尔曼滤波(Kalman filter)算法与贝叶斯滤波(bayes filter)算法之间的联系是什么？

答：贝叶斯滤波是一种思想，它告诉了我们怎么在知道观测值与控制命令这两种值的可信度时如何计算最终估计出的值的可信度。但是贝叶斯滤波并没有告诉我们怎么计算观测值的可信度，怎么对控制模型（发出控制命令后机器人是按照怎样的模型执行）进行建模。而卡尔曼滤波算法就是对贝叶斯滤波算法的一个具体实现。卡尔曼滤波认为观测值可信度模型是一个正态分布，控制模型是一个正态分布。

所以卡尔曼滤波算法是源自于对贝叶斯滤波算法的进一步演化，我希望在学本文前你能对贝叶斯滤波有了解。如果你还不清楚贝叶斯滤波怎么做的？可以看看这篇文章【易懂教程】我是如何十分钟理解与推导贝叶斯滤波(Bayes Filter)算法？

回顾一下贝叶斯滤波算法：
在贝叶斯滤波中我们知道了：$P(x_t|z_t,u_t,x_{t-1})=\eta P(z_t|x_t)P(x_t|u_t,x_{t-1})$。
其中$P(x_t|z_t,u_t,x_{t-1})$是在已知融合观测值$z_t$和控制命令$u_t$以及上个状态$x_{t-1}$的情况下当前状态为$x_t$的概率,$P(z_t|x_t)$是需要我们设计的观测模型（在当前状态为$x_t$的情况下观测值是$z_t$的概率），$P(x_t|u_t,x_{t-1})$这个是需要我们设计的控制模型（在上个状态是$x_{t-1}$并且控制命令是$u_t$的情况下当前状态是$x_t$的概率）。

为何贝叶斯滤波算法要求一个概率值？

我们求$P(x_t|z_t,u_t,x_{t-1})$它的作用就是看当前状态是$x_t$的概率有多大，选择概率最大的那个状态取值作为状态。（比如当前机器人离障碍物10m的概率是80%，离障碍物距离为10.3m的概率是89%，我当然认为机器人离障碍物距离是10.3m）。当然一般不是只求一个值而是求一个概率分布。什么是概率分布？就是很多个数和它们出现的概率就组成了概率分布。也就是说不止一个数的概率而是很多个数的概率。

卡尔曼滤波对贝叶斯滤波做了什么？

而前面我们也提到了卡尔曼滤波就是贝叶斯滤波这个算法的实现方式之一。所以卡尔曼滤波算法也自然设计了自己对观测模型和控制模型的一个实现。卡尔曼滤波算法做的唯一的工作就是设计了$P(z_t|x_t)$和$P(x_t|u_t,x_{t-1})$这两个东西的计算模型（所以只要你设计了一种具体的模型假设那么你也发明了一种算法~）。什么叫做模型假设？就是套一个函数上去，比如假设线性模型那就套一个直线上去，假设是正态分布就把正态分布函数套上去。所谓的卡尔曼增益之类的东西只不过是用卡尔曼滤波算法所假设的控制模型与观测模型套到$P(x_t|z_t,u_t,x_{t-1})=\eta P(z_t|x_t)P(x_t|u_t,x_{t-1})$这个公式经过各种化简演化后得到的结果。

卡尔曼滤波是如何实现贝叶斯滤波的？

我以机器人状态估计为例。

卡尔曼滤波对机器人的观测与控制做出的假设

1. 首先它认为机器人的状态是线性变化的。什么叫做机器人的状态是线性变化的？假设第$x_t$时刻我们机器人的状态是指机器人离目标点的距离$x_t$，（事实上机器人的状态可以指很多东西比如机器人的姿态，速度等等）。那么现在机器人状态是线性变化的这句话的意思是指机器人是匀速直线运动。即机器人离目标点的距离可以这么表示：$x_t=a_t{x}_{t-1}+u_t+w_t$，其中$a_t$是常数，$u_t$是控制命令（即让机器人这段时间移动多远），$w_t$是噪声。同时卡尔曼滤波算法还假设观测值$z_t$与机器人实际离目标的距离$x_t$也是线性的。即可以这么表示$z_t=c_t{x}_t+v_t$。其中$c_t$是常数，$v_k$是噪声。（观测值就是指的是传感器的测量值）
   总结一下就是：
   $x_t=a_t{x}_{t-1}+u_t+w_t$
   $z_t=c_t{x}_t+v_t$

2. 卡尔曼滤波对贝叶斯滤波中的观测模型$P(z_t|x_t)$建模和控制模型建模$P(x_t|u_t,x_{t-1})$所做出的假设：
   卡尔曼滤波算法认为所有的变量和噪声都是服从正态分布（高斯分布）。即$x_t,x_{t-1},u_t,w_t,v_t$都是服从正态分布（高斯分布）。所以$P(z_t|x_t)$和$P(x_t|u_t,x_{t-1})$都是服从高斯分布。**为何卡尔曼滤波要假设这些都是高斯分布？**原因有两个：一是世界上很多噪声和数据都是服从高斯分布，二是我我们假设了$x_t$与其他变量是线性关系，而刚刚好高斯分布进行线性变换后还是高斯分布。这个性质让我们求$x_t$的概率分布$P(x_t|z_t,u_t,x_{t-1})$变得非常容易。为何变得容易？这是因为高斯分布只要你确定了均值和方差那就知道了概率分布函数，所以我们只用专注根据其他变量的均值和方差来求$x_t$的均值和方差即可。

卡尔曼滤波到底要解决一个怎样的问题？已知哪些量，最终想求什么结果？

在卡尔曼滤波中我们已知$x_{t-1},w_t,v_t$的概率分布，即我们知道$P(x_{t-1}),P(w_t),P(v_t)$。并且卡尔曼滤波算法还认为$x_t=a_t{x}_{t-1}+u_t+w_t$，$z_t=c_t{x}_t+v_t$。根据这些信息我们要求$P(x_t|z_t,u_t,x_{t-1})$。注意：卡尔曼滤波最终结果是求一个概率分布。也就是说是求概率值$P(x_t|...)$，而不是求当前机器人状态的最优估计$x_t^{\ast }$。很多人就总是误以为直接求$x_t^{\ast }$事实上并不是。它是先求概率分布，然后将计算的概率分布的均值${\bar{x}}_t$把它作为当前机器人状态最优的估计值$x_t^{\ast }$。
在前面提到了$x_{t-1},w_t,v_t$的概率分布是正态分布。所以记$P(x_{t-1})=N(x_{t-1}^{\ast },...),P(w_t)=N(0,R_t),P(v_k)=N(0,Q_t)$。其中$N(0,R_t)$表示均值是0，方差是$R_t$的高斯分布,注意$x_{t-1}^{\ast }$是上个时刻的最优估计。我们在前面知道$x_t=a_t{x}_{t-1}+u_t+w_t$，现在等式的右边各个变量的概率分布都是已知（都是高斯分布），所以我们可以根据右边的概率分布算左边的。在前面我提到了$x_t$也是服从高斯分布（因为右边都是高斯分布，这些高斯分布进行线性运算还是高斯分布），而高斯分布只要确定了均值和方差那就知道了概率分布函数。因此我们只用关注$x_t$它的概率分布的均值和方差的计算（并且$x_t$的均值${\bar{x}}_t$就是机器人状态的最优估计$x_t^{\ast }$）。

从算法发明者的角度看卡尔曼滤波的推导思路到底怎样的？

现在我们已知：
$x_t=a_t{x}_{t-1}+u_t+w_t$
$z_t=c_t{x}_t+v_t$
$P(x_{t-1})=N(x_{t-1}^{\ast },...),P(w_t)=N(0,R_t),P(v_k)=N(0,Q_t)$

我们的目标是求贝叶斯公式：
$P(x_t|z_t,u_t,x_{t-1})=P(z_t|x_t)P(x_t|u_t,x_{t-1})$

根据概率乘法和加法运算规则所以有：
$P(x_t|x_{t-1},u_t)=a_{t}P(x_{t-1})+P(u_t)+P(w_t)$
$P(z_t|x_t)=c_{t}P(x_t)+P(v_t)$

所以很容易得到贝叶斯公式$P(x_t|z_t,u_t,x_{t-1})=\eta P(z_t|x_t)P(x_t|u_t,x_{t-1})$的右边都是已知量了。所以我们已经可以求得$P(x_t|z_t,u_t,x_{t-1})$，然后只用将它的均值${\bar{x}}_t$作为当前时刻机器人状态的最优估计$x_t^{\ast }$。

卡尔曼滤波已经讲完了。但是你又会疑惑为何别的书上或者博客都会讲卡尔曼增益，更新，预测之类的东西。事实上他们只不过是将前面那些东西代入到$P(x_t|z_t,u_t,x_{t-1})=P(z_t|x_t)P(x_t|u_t,x_{t-1})$，然后化简。最终得到一个简洁的计算$P(x_t|z_t,u_t,x_{t-1})$的均值的表达式。

如何化简推导得到卡尔曼增益？以及如何推导得到$x_t^{\ast }$的递推表达式

在前面我们提到了最优的机器人状态估计$x_t^{\ast }$就是$P(x_t|z_t,u_t,x_{t-1})$这个概率分布函数的均值。注意：我们最关系的东西是怎么求均值。
现在我们已知：
$P(x_t|x_{t-1},u_t)=a_{t}P(x_{t-1})+P(u_t)+P(w_t)$
$P(z_t|x_t)=c_{t}P(x_t)+P(v_t)$
想求：
$P(x_t|z_t,u_t,x_{t-1})=P(z_t|x_t)P(x_t|u_t,x_{t-1})$
式子中所有的概率分布都是正态分布，在概率论里面我们学过正态分布的线性和得到的仍然是一个正态分布（我们暂时只推导二维的卡尔曼滤波，多维其实就是多了个向量运算而已），并且所得到的正态分布的均值也是其他几个正态分布的均值的线性和,新正态分布的方差是系数平方再求和。举个例子：正态分布$N(1,3.2)$的均值是1，正态分布$N(3,4.1)$的均值是3.他们两进行线性和得到新正态分布$2\ast N(1,3.2)+5\ast N(3,4.1)$的均值是$2\ast 1+5\ast 3=17$，方差是$2^2\ast 3.2+5^2\ast 4.1=115.3$。即新的正态分布是$N(17,115.3)$。

所以我们同样可以根据上面的方法求$P(x_t|x_{t-1},u_t)$这个正态分布的均值$x_t^{\ast }$和方差，这个均值$x_t^{\ast }$就是我们对机器人状态的最优估计。
我们先将正态分布具体的表达式代入已知的那几个等式
$P(x_t|x_{t-1},u_t)=a_{t}P(x_{t-1})+P(u_t)+P(w_t)=a_t\ast N(x_{t-1}^{\ast },...)+N({\bar{u}}_t,...)+N(0,R_t)=N(a_t\ast x_{t-1}+{\bar{u}}_t,...)$，注意$x_{t-1}^{\ast }$是上个时刻的对$x_{t-1}$最优估计。
$P(z_t|x_t)=c_{t}P(x_t)+P(v_t)=c_t\ast N(x_t^{\ast },...)+N(0,Q_t)=N(c_t\ast x_t^{\ast },...)$。
$P(x_t|z_t,u_t,x_{t-1})=P(z_t|x_t)P(x_t|u_t,x_{t-1})=N(c_t\ast x_t^{\ast },...)N(a_t\ast x_{t-1}^{\ast }+{\bar{u}}_t,...)$。
两个正态分布相加虽然可以直接得到他们的均值也是相加，但是两个正态分布相乘却没有这么简单的性质，这需要我们用定义法去做。

我们知道正态分布式这么写的$...\ast e^{\frac{(x-\bar{x}{)}^2}{s^2}}$，其中$\bar{x}$是均值,$s^2$是方差。现在我们想求$P(x_t|z_t,u_t,x_{t-1})$这个东西的均值。我们已经是知道它是正态分布所以它的形式肯定也是$...\ast e^{\frac{(x-\bar{x}{)}^2}{s^2}}$这种。所以我们只用关注e的指数是怎么变化的。
$$\begin{gathered} P(x_t|z_t,u_t,x_{t-1})=P(z_t|x_t)P(x_t|u_t,x_{t-1})=N(c_t\ast x_t^{\ast },...)N(a_t\ast x_{t-1}^{\ast }+{\bar{u}}_t,...) \\ =...e^{[(z_t-c_t\ast x_t^{\ast }{)}^2+(x_t-a_t\ast x_{t-1}^{\ast }-{\bar{u}}_t{)}^2]} \end{gathered}$$

你的赞与关注是支持我知识分享的唯一动力。

关于卡尔曼滤波编程实践可以看这篇文章：卡尔曼滤波编程实践