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

矫情吗；* 2022-09-07 15:58 215阅读 0赞

## 前言 ##

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

视频地址：[https://www.bilibili.com/video/BV12D4y1S7fU][https_www.bilibili.com_video_BV12D4y1S7fU]

## 数据融合 Date Fusion ##

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

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

数学形式记为：

> 第一个称  ![Z\_\{1\} = 30g;\\sigma \_\{1\}=2g][Z_1_ _ 30g_sigma _1_2g]
> 
> 第二个称  ![Z\_\{2\} = 32g;\\sigma \_\{2\}=4g][Z_2_ _ 32g_sigma _2_4g]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxMjA0NDY0_size_16_color_FFFFFF_t_70][]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxMjA0NDY0_size_16_color_FFFFFF_t_70 1][]

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

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

数学上是找到一个**最优值**，于是采用卡尔曼算法。设估计值为![z \\hat\{\}][z _hat]，则：

>                                                    ![z\\hat\{\} = Z\_\{\_\{1\}\} + K( Z\_\{\_\{2\}\} - Z\_\{\_\{1\}\} )][z_hat_ _ Z_1_ _ K_ Z_2_ - Z_1_]

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

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

设： 估计值![z \\hat\{\}][z _hat] ，估计值标准差![\\sigma \_\{z\}][sigma _z]，估计值的方差Var(![z \\hat\{\}][z _hat])。

估计值的方差 ![\\sigma \_\{\_\{z\}\}^\{2\}][sigma _z_2]=

![= Var(Z\_\{\_\{1\}\} + K( Z\_\{\_\{2\}\} - Z\_\{\_\{1\}\} ))][Var_Z_1_ _ K_ Z_2_ - Z_1_]

![= Var(Z\_\{\_\{1\}\} + KZ\_\{\_\{2\}\} - KZ\_\{\_\{1\}\} )][Var_Z_1_ _ KZ_2_ - KZ_1_]

![= Var(\{\\color\{Red\} (1-K)Z\_\{\_\{1\}\}\} + \{\\color\{Blue\} KZ\_\{\_\{2\}\}\} )][Var_color_Red_ _1-K_Z_1_ _ _color_Blue_ KZ_2_]

![= Var((1-K)Z\_\{\_\{1\}\}) +Var(KZ\_\{\_\{2\}\} )][Var_1-K_Z_1_ _Var_KZ_2_]

![= (1-K)^\{2\} \*Var(Z\_\{\_\{1\}\}) +K^\{2\}\*Var(Z\_\{\_\{2\}\} )][_1-K_2_ _Var_Z_1_ _K_2_Var_Z_2_]

**![= \{\\color\{Teal\} \{\\color\{Teal\} (1-K)^\{2\} \*\\sigma \_\{1\}^\{2\} +K^\{2\}\*\\sigma \_\{2\}^\{2\}\}\}][_color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2]**

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

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

![\\frac\{d\\sigma \_\{z\}^\{2\}\}\{dk\} = 0][frac_d_sigma _z_2_dk_ _ 0]

上面求得 估计值的方差![\\sigma \_\{z\}^\{2\}= \{\\color\{Teal\} \{\\color\{Teal\} (1-K)^\{2\} \*\\sigma \_\{1\}^\{2\} +K^\{2\}\*\\sigma \_\{2\}^\{2\}\}\}][sigma _z_2_ _color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2]，进行求导得到：

![\-2(1-k)\\sigma \_\{1\}^\{2\} + 2k\\sigma \_\{2\}^\{2\} = 0][-2_1-k_sigma _1_2_ _ 2k_sigma _2_2_ _ 0]

即： ![\-\\sigma \_\{1\}^\{2\} + k\\sigma \_\{1\}^\{2\} + k\\sigma \_\{2\}^\{2\} =0][-_sigma _1_2_ _ k_sigma _1_2_ _ k_sigma _2_2_ _0]

整理后得到： ![k=\\frac\{\\sigma \_\{1\}^\{2\}\}\{\\sigma \_\{1\}^\{2\} + \\sigma \_\{2\}^\{2\}\}][k_frac_sigma _1_2_sigma _1_2_ _ _sigma _2_2]

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

然后把数据代入，

> 第一个称  ![Z\_\{1\} = 30g;\\sigma \_\{1\}=2g][Z_1_ _ 30g_sigma _1_2g]  解释：称得是30g，标准差是2g。
> 
> 第二个称  ![Z\_\{2\} = 32g;\\sigma \_\{2\}=4g][Z_2_ _ 32g_sigma _2_4g]  解释：称得是32g，标准差是4g。

先求k值 卡尔曼增益，![k= \\frac\{2^\{2\}\}\{2^\{2\} + 4^\{2\}\} =0.2][k_ _frac_2_2_2_2_ _ 4_2_ _0.2]

然后计算估计值 ![z \\hat\{\}][z _hat]，![z\\hat\{\} = Z\_\{\_\{1\}\} + K( Z\_\{\_\{2\}\} - Z\_\{\_\{1\}\} ) = 30+0.2(32-30)=30.4][z_hat_ _ Z_1_ _ K_ Z_2_ - Z_1_ _ _ 30_0.2_32-30_30.4]

最后更新一下估计值的方差![\\sigma \_\{\_\{z\}\}^\{2\}][sigma _z_2]，![\\sigma \_\{\_\{z\}\}^\{2\}=\{\\color\{Teal\} \{\\color\{Teal\} (1-K)^\{2\} \*\\sigma \_\{1\}^\{2\} +K^\{2\}\*\\sigma \_\{2\}^\{2\}\}\}=(1-0.2)^\{2\}2^\{2\}+0.2^\{2\}4^\{2\}=3.2][sigma _z_2_color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2_1-0.2_2_2_2_0.2_2_4_2_3.2]

于是估计值的标准差![\\sigma \_\{\_\{z\}\}^\{\}][sigma _z 1] 等于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)

本文只提供参考和学习，谢谢。

[https_www.bilibili.com_video_BV12D4y1S7fU]: https://www.bilibili.com/video/BV12D4y1S7fU
[Z_1_ _ 30g_sigma _1_2g]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20Z_%7B1%7D%20%3D%2030g%3B%5Csigma%20_%7B1%7D%3D2g
[Z_2_ _ 32g_sigma _2_4g]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20Z_%7B2%7D%20%3D%2032g%3B%5Csigma%20_%7B2%7D%3D4g
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxMjA0NDY0_size_16_color_FFFFFF_t_70]: /images/20220829/5d44a60a5bb443caa2f837479f84413f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxMjA0NDY0_size_16_color_FFFFFF_t_70 1]: /images/20220829/392c6050a9524954a336a44f8d7ff8f8.png
[z _hat]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20z%20%5Chat%7B%7D
[z_hat_ _ Z_1_ _ K_ Z_2_ - Z_1_]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20z%5Chat%7B%7D%20%3D%20Z_%7B_%7B1%7D%7D%20&plus;%20K%28%20Z_%7B_%7B2%7D%7D%20-%20Z_%7B_%7B1%7D%7D%20%29
[sigma _z]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20%5Csigma%20_%7Bz%7D
[sigma _z_2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%5Csigma%20_%7B_%7Bz%7D%7D%5E%7B2%7D
[Var_Z_1_ _ K_ Z_2_ - Z_1_]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20Var%28Z_%7B_%7B1%7D%7D%20&plus;%20K%28%20Z_%7B_%7B2%7D%7D%20-%20Z_%7B_%7B1%7D%7D%20%29%29
[Var_Z_1_ _ KZ_2_ - KZ_1_]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20Var%28Z_%7B_%7B1%7D%7D%20&plus;%20KZ_%7B_%7B2%7D%7D%20-%20KZ_%7B_%7B1%7D%7D%20%29
[Var_color_Red_ _1-K_Z_1_ _ _color_Blue_ KZ_2_]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20Var%28%7B%5Ccolor%7BRed%7D%20%281-K%29Z_%7B_%7B1%7D%7D%7D%20&plus;%20%7B%5Ccolor%7BBlue%7D%20KZ_%7B_%7B2%7D%7D%7D%20%29
[Var_1-K_Z_1_ _Var_KZ_2_]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20Var%28%281-K%29Z_%7B_%7B1%7D%7D%29%20&plus;Var%28KZ_%7B_%7B2%7D%7D%20%29
[_1-K_2_ _Var_Z_1_ _K_2_Var_Z_2_]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20%281-K%29%5E%7B2%7D%20*Var%28Z_%7B_%7B1%7D%7D%29%20&plus;K%5E%7B2%7D*Var%28Z_%7B_%7B2%7D%7D%20%29
[_color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%3D%20%7B%5Ccolor%7BTeal%7D%20%7B%5Ccolor%7BTeal%7D%20%281-K%29%5E%7B2%7D%20*%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;K%5E%7B2%7D*%5Csigma%20_%7B2%7D%5E%7B2%7D%7D%7D
[frac_d_sigma _z_2_dk_ _ 0]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%5Cfrac%7Bd%5Csigma%20_%7Bz%7D%5E%7B2%7D%7D%7Bdk%7D%20%3D%200
[sigma _z_2_ _color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%5Csigma%20_%7Bz%7D%5E%7B2%7D%3D%20%7B%5Ccolor%7BTeal%7D%20%7B%5Ccolor%7BTeal%7D%20%281-K%29%5E%7B2%7D%20*%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;K%5E%7B2%7D*%5Csigma%20_%7B2%7D%5E%7B2%7D%7D%7D
[-2_1-k_sigma _1_2_ _ 2k_sigma _2_2_ _ 0]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20-2%281-k%29%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;%202k%5Csigma%20_%7B2%7D%5E%7B2%7D%20%3D%200
[-_sigma _1_2_ _ k_sigma _1_2_ _ k_sigma _2_2_ _0]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20-%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;%20k%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;%20k%5Csigma%20_%7B2%7D%5E%7B2%7D%20%3D0
[k_frac_sigma _1_2_sigma _1_2_ _ _sigma _2_2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20k%3D%5Cfrac%7B%5Csigma%20_%7B1%7D%5E%7B2%7D%7D%7B%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;%20%5Csigma%20_%7B2%7D%5E%7B2%7D%7D
[k_ _frac_2_2_2_2_ _ 4_2_ _0.2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20k%3D%20%5Cfrac%7B2%5E%7B2%7D%7D%7B2%5E%7B2%7D%20&plus;%204%5E%7B2%7D%7D%20%3D0.2
[z_hat_ _ Z_1_ _ K_ Z_2_ - Z_1_ _ _ 30_0.2_32-30_30.4]: https://latex.codecogs.com/gif.latex?%5Cfn_cm%20z%5Chat%7B%7D%20%3D%20Z_%7B_%7B1%7D%7D%20&plus;%20K%28%20Z_%7B_%7B2%7D%7D%20-%20Z_%7B_%7B1%7D%7D%20%29%20%3D%2030&plus;0.2%2832-30%29%3D30.4
[sigma _z_2_color_Teal_ _color_Teal_ _1-K_2_ _sigma _1_2_ _K_2_sigma _2_2_1-0.2_2_2_2_0.2_2_4_2_3.2]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%5Csigma%20_%7B_%7Bz%7D%7D%5E%7B2%7D%3D%7B%5Ccolor%7BTeal%7D%20%7B%5Ccolor%7BTeal%7D%20%281-K%29%5E%7B2%7D%20*%5Csigma%20_%7B1%7D%5E%7B2%7D%20&plus;K%5E%7B2%7D*%5Csigma%20_%7B2%7D%5E%7B2%7D%7D%7D%3D%281-0.2%29%5E%7B2%7D2%5E%7B2%7D&plus;0.2%5E%7B2%7D4%5E%7B2%7D%3D3.2
[sigma _z 1]: https://latex.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Cfn_cm%20%5Csigma%20_%7B_%7Bz%7D%7D%5E%7B%7D