【opencv实践】easy卡尔曼滤波中：结合一个小例子的数学推导

我们上一篇文章：

【opencv实践】easy卡尔曼滤波上：浅谈

介绍了卡尔曼滤波的由来和原理，我们在这儿一句话总结一下，大家如果有不懂的可以去看上篇文章：

卡尔曼滤波就是将模型预测的数据和观测数据进行融合，来过滤掉观测误差，进而得到更精确的数据。

以一个小车在平面上走为例子，小车的程序让它以初速度为V0，加速度为a的运动方式向前运动，但由于各种误差和原因（比如有风，或者上坡等），我们的小车并不严格按照这个模型走，存在噪声（并且假设高斯分布）。

并且我们不断的用传感器来测量小车的位置，但传感器也存在高斯分布的噪声。

卡尔曼滤波主要有五大公式，在这个例子的基础上，我们依次来看一下。

第一个公式：

• Xk：当前状态描述矩阵
• Fk：状态转移矩阵
• Bk：控制矩阵
• Uk：控制向量

对于小车来说，状态描述就是它的一维的位置x，以及运动速度v：

状态转移矩阵就是从上一时刻到下一时刻之间的过渡：

我们将Fk和Xk-1相乘，就可以得到：

显而易见，就是在小车匀速运动下的状态变化了。

但我们的小车是匀加速运动的，我们将加速度视为控制向量Uk，因为加速度是由踩油门啊之类的产生的(姑且这么理解吧)。

因为我们只有一个控制变量，及加速度a，所以

控制矩阵Bk就是将控制向量转换，将控制变量体现到状态Xk上：

到此为止，我们用第一个公式推导出了小车从上一个时刻到当前时刻状态的理想变化。

第二个公式：

• Pk：描述状态量之间的互相关性，是Xk的协方差矩阵，也就是Cov(Xk)
• Fk：状态转移矩阵
• Qk：外界噪声的协方差

Pk是Xk的协方差，我们第一个公式更新了状态向量Xk，第二个公式就是更新Xk的协方差Pk，并加上一个外界噪声Qk。

因为我们用的是卡尔曼滤波，我们目标对象的状态都是呈现高斯分布，所以我们有两个需要关注的变量：均值以及协方差。所以我们需要在更新了Xk，也就包含均值和方差之后，还需要更新协方差，来关注各状态量之间的关联程度。

第三个公式：

• K：卡尔曼增益矩阵
• Hk：传感器数据
• Pk ：传感器测量的噪声的协方差

第三个公式的意义就是求出K，而K是为了在公式四五中使用，单独拎出来并没有什么直观上的意义。

我们知道，卡尔曼滤波就是将传感器的观测数据和模型的预测数据相融合，而公式一二做的就是给出模型的预测数据，公式三四五做的就是将预测数据和观测数据相融合来得到一个新的，更准确的数据。

在这两者之间有观测数据的采集这一步，但这不属于卡尔曼滤波算法要做的。

那怎么融合呢？我们的预测数据和观测数据都是高斯分布，两者大概呈下图关系：

而显然两者的重叠部分就是最优的最可能的分布了，如何得到中间这一块呢？就是将两个高斯分布相乘。

两者相乘之后（也就是融合之后），得到的大概如下图：

为了简便，我们用一个k来简化一下：

这个k，就是我们公式三求的卡尔曼增益了！（一维情况下）

而后面的两个公式，就是我们公式四五的一维表达了，不过因为我们实际问题有可能是多维的，所以我们实际的公式用矩阵表示，看着可能更复杂一些。

第四个公式：

• Xk' ：数据融合之后的状态分布
• Xk：模型预测的Xk状态分布
• Zk：Xk的均值
• HkXk：传感器观测数据的均值

第五个公式：

更新状态描述量的互相关性。

到此，我们的五个公式就完成了一次循环，在应用中我们就是迭代进行一步步滤波和预测的。

说是公式推导，其实就是强行给大家介绍了一波这五个公式，因为推导的话，我写不出自己的东西，因为我也是看别人的教程学的，真要写大概率就是纯抄了，意义不大。

大家觉得没看懂的想看数学推导的，可以看这篇《图说卡尔曼滤波》,外国一位大牛写的不过已经被翻译成中文了，强烈推荐。

https://zhuanlan.zhihu.com/p/39912633?utm_source=wechat_session&utm_medium=social&utm_oi=1024373037663211520