卡尔曼滤波器2——数据融合（笔记篇 + 代码实现）

前言

本文是观看DR_CAN老师的视频后，简单总结了一下的笔记，并根据思路写了示例代码；这里主要讲使用卡尔曼滤波器进行数据融合。

视频地址：https://www.bilibili.com/video/BV12D4y1S7fU

数据融合 Date Fusion

这里从一个例子开始，用“两个称”来称同一个物体，得到两个结果；第一个称结果是30g，第二个称结构是32g。

由于两个称都不准，存在误差的。第一个称的标准差是2g；第二个称的标准差是4g。而且都符合正态分布，也称为高斯分布。

数学形式记为：

第一个称  

第二个称  

 下面看看，两个称的输出的概率分布情况；第一个称服从正态分布，标准差是2g；所以他在28g到32g之间的概率是68.4%。

 第二个称也服从正态分布，标准差是4g；所以他在28g到36g之间的概率是68.4%。相对于第一个称，由于标准差更大，所以看起来，更矮一些更胖一些。

现在需要用这两个称的结果，去“估计真实值”，如何估算呢？

感觉上是在这两个称结果之间，而且第一个称标准差更小，真实结果更靠近第一个称的结果。

数学上是找到一个最优值，于是采用卡尔曼算法。设估计值为，则：     

其中 K 是卡尔曼增益；它的范围在0到1，即[0,1]。

目的 是求 K 使得估计值的标准最少，也就是使得估计值的方差最小。

设： 估计值 ，估计值标准差，估计值的方差Var()。

估计值的方差 =

备注：红色和蓝色两部分相互独立，可以单独拿出来；

如果要求估计值方差的最小值，需要对k求导，然后令它等于0，就可以求得极值了：

上面求得 估计值的方差，进行求导得到：

即： 

整理后得到： 

好啦，现在求出k，这个k值能使得估计值方差的最小

然后把数据代入，

第一个称    解释：称得是30g，标准差是2g。

第二个称    解释：称得是32g，标准差是4g。

先求k值 卡尔曼增益，

然后计算估计值 ，

最后更新一下估计值的方差，

 于是估计值的标准差 等于1.79。

Python版的伪代码：

 
'''
卡尔曼滤波——数据融合
'''
 
# 卡尔曼增益 = 数据1的误差 除以 (数据1的误差 + 数据2的误差)
# 误差对应方差!!
def kalman_gain(e1, e2):
    return e1/(e1 + e2)
 
# 估计值 = 数据1的估计值 + 系数*(数据2测量值 - 数据1的估计值)
def now_estimated_value(X1, K, X2):
    return X1 + K(X2 - X1)
 
 
# 更新估计误差 = （1 - 卡尔曼增益）* 数据1的估计误差 + 卡尔曼增益* 数据2的估计误差
def now_estimated_error(K, e1, e2):
    return (1 - K)*e1 + k*e2
 
 
# 循环体
K = kalman_gain(e1, e2)
X_k = now_estimated_value(X1, K, X2)
e_EST = now_estimated_error(K, e1, e2)