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

前言

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

视频地址：【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题_哔哩哔哩_bilibili

数据融合 Date Fusion

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

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

数学形式记为：

第一个称 $Z_{1} = 30g;\sigma _{1}=2g$

第二个称 $Z_{2} = 32g;\sigma _{2}=4g$

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

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

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

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

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

$z\hat{} = Z_{_{1}} + K( Z_{_{2}} - Z_{_{1}} )$

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

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

设：估计值 $z \hat{}$ ，估计值标准差 $\sigma _{z}$ ，估计值的方差Var( $z \hat{}$ )。

估计值的方差 $\sigma _{_{z}}^{2}$ =

$= Var(Z_{_{1}} + K( Z_{_{2}} - Z_{_{1}} ))$

$= Var(Z_{_{1}} + KZ_{_{2}} - KZ_{_{1}} )$

$= Var({\color{Red} (1-K)Z_{_{1}}} + {\color{Blue} KZ_{_{2}}} )$

$= Var((1-K)Z_{_{1}}) +Var(KZ_{_{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}}}$

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

如果要求估计值方差的最小值，需要对k求导，然后令它等于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}}}$ ，进行求导得到：

$-2(1-k)\sigma _{1}^{2} + 2k\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，这个k值能使得估计值方差的最小

然后把数据代入，

第一个称 $Z_{1} = 30g;\sigma _{1}=2g$ 解释：称得是30g，标准差是2g。

第二个称 $Z_{2} = 32g;\sigma _{2}=4g$ 解释：称得是32g，标准差是4g。

先求k值卡尔曼增益， $k= \frac{2^{2}}{2^{2} + 4^{2}} =0.2$

然后计算估计值 $z \hat{}$ ， $z\hat{} = Z_{_{1}} + K( Z_{_{2}} - Z_{_{1}} ) = 30+0.2(32-30)=30.4$

最后更新一下估计值的方差 $\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}}^{}$ 等于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)

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

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

卡尔曼滤波器1——递归算法（笔记篇 + 代码实现）
前言本文是观看DR_CAN老师的视频后,简单总结了一下的笔记,并根据思路写了示例代码:这里主要讲卡尔曼滤波器与递归算法. 视频地址:https://www.bilibili.com/video/BV ...
六.卡尔曼滤波器开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
卡尔曼滤波器 bilibili视频版
卡尔曼滤波器又叫最佳线性滤波器,好处有实现简单,纯时域(无须频域变换)所以在工程上又很广泛的应用. 因为它只能描述状态与状态之间的线性关系. 既然是线性关系,所以就可以用矩阵表示出来(如下图) . 图 ...
七.卡尔曼滤波器开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
优达学城《无人驾驶入门》学习笔记——卡尔曼滤波器实现详解
优达学城<无人驾驶入门>的第二个项目是实现矩阵类,要求通过python编写一个用来计算矩阵的类Matrix.编写这个类并不难,涉及到的线性代数方面的知识也不多,比如矩阵的加法.减法.乘法, ...
滤波融合（二）基于C++完成一个简单的扩展卡尔曼滤波器的非线性系统模型
之前已经简单的实现过线性卡尔曼滤波:滤波融合(一)基于C++完成一个简单的卡尔曼滤波器线性的系统和测量模型那么对于非线性的系统,区别就是多了线性化的过程,因为高斯映射到非线性函数,其结果不再是一 ...
信号去噪，基于Sage-Husa自适应卡尔曼滤波器实现海浪磁场噪声抑制及海浪磁场噪声的产生附Matlab代码
信号去噪,基于Sage-Husa自适应卡尔曼滤波器实现海浪磁场噪声抑制及海浪磁场噪声的产生附Matlab代码信号处理中的一个关键问题就是信号去噪.在实际应用中,很多信号可能会受到环境噪声的干扰,这些 ...
四.卡尔曼滤波器(EKF)开发实践之四: ROS系统位姿估计包robot_pose_ekf详解
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
一. 卡尔曼滤波器开发实践之一: 五大公式详解
既然标题名称是开发实践,本系列文章将主要介绍如何在工程实践中使用卡尔曼滤波器,至于卡尔曼滤波器的五大公式如何推导而来,网上有很多大拿们写的都很精彩,这里不再叙述.可以参考了下面两篇博文: 1. 卡尔曼 ...

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

前言

数据融合 Date Fusion

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

最新文章

热门文章