如何理解卡尔曼滤波(Kalman Filter)实现数据融合

本篇博客主要是记录自己 KF 的学习过程，其中遇到的疑惑，和自己的理解。如有错误欢迎指正交流～

参考相关资料

• 基础理论介绍：
  扩展卡尔曼滤波EKF用于多源传感器融合
  详解卡尔曼滤波原理
  我所理解的卡尔曼滤波
• 相关 robot pose ekf 代码解析：
  robot_pose_ekf源码解读 - zhxue_11
  Navigation源码阅读之robot_pose_ekf - 若愚和小巧
  robot_pose_ekf源码解读 - 首飞爱玩机器人

---

卡尔曼滤波在任何含有不确定信息的动态系统中，能对系统下一步的走向做出有根据的预测。
在卡尔曼滤波中，存在两个基础假设：（1）线性系统 （2）符合高斯分布

无偏估计 是用样本统计量来估计总体参数时的一种无偏推断。估计量的数学期望等于被估计参数的真实值，则称此估计量为被估计参数的无偏估计，即具有无偏性，是一种用于评价估计量优良性的准则。无偏估计的意义是：在多次重复下，它们的平均数接近所估计的参数真值。（百度百科）

协方差 在概率论和统计学中用于衡量两个变量的总体误差。而方差是协方差的一种特殊情况，即当两个变量是相同的情况。

首先卡尔曼滤波中，因为已经假设其符合高斯分布，所以在高斯分布中存在均值和协方差，均值即数学期望，由于卡尔曼滤波是一个无偏估计，所以被认为就是真实值。所以卡尔曼滤波的最终目的就是要求解每一时刻的均值，从而得到该时刻的真实值。

而协方差代表了变量之间的关系，在百度百科上有下面这样一段介绍：
协方差表示的是两个变量总体误差的期望。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值时另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值；如果两个变量的变化趋势相反，即其中一个变量大于自身的期望值时另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。如果两个变量统计独立的，那么二者之间的协方差就是零。

关于方差和协方差的计算公式如下：
$${\sigma }_x^2=\frac{1}{n-1}\sum _{i=1}^n(x_i-\bar{x}{)}^2$$$$\sigma (x,y)=\frac{1}{n-1}\sum _{i=1}^n(x_i-\bar{x})(y_i-\bar{y})$$

方差度量单个随机变量的离散程度，协方差度量两个随机变量（变化趋势）的相似程度。但是协方差也代表了多个变量与均值（真值）之间的误差。因此最小化协方差，也就是一个逼近真值的过程。

---

所谓的多传感器融合，并不是传感器之间的融合，而是传感器对系统状态的异步更新。

在对传感器数据进行融合的过程中，每个传感器都会存在误差，且服从高斯分布。首先对于每个传感器所测量得到的数据可能不一致，例如GPS得到的经纬度，IMU得到的加速度等等，因此需要现将其数据格式保持一致，例如都表示为当前的车辆位置和速度，也就是预测状态。相当于所有传感器的高斯分布的均值代表相同的含义，为同一种真值。
所以在卡尔曼滤波中定义了该转换关系，即测量函数$H(x)$，表示了观测值和预测状态之间的关系。

在两个基本的假设中，已知系统状态为线性的，那么便可以从$K$时刻推导出$K+1$时刻的状态。对于不同的系统状态量的设定，其线性方程也是不一样的，如果该系统状态定义为当前位置和速度$(p,v)$，那么他的推导关系为：
$$p_k=p_{k-1}+\Delta t{v}_{k-1}$$$$v_k=v_{k-1}$$
这种关系在卡尔曼滤波中，被称为状态转换函数$F(x)$，即从$K$状态计算得到$K+1$的状态。

关于协方差的状态转换都是一样的：$$P^{\prime }=FP{F}^T$$具体可参考博客VIO残差函数的构建以及IMU预积分和协方差传递

那么现在就可以从两种途径获得预测状态，不过无论是从传感器测量值计算预测状态，还是从上一状态通过状态转换函数测量当前预测状态，都存在噪声。

• (1) 运动噪声和过程噪声指的是相同的情况：预测位置时对象位置的不确定性。 该模型假定速度在两个时间间隔之间是恒定的，但实际上我们知道物体的速度会由于加速度而发生变化。 该模型通过过程噪声包括了这种不确定性。两次时间间隔越大不确定性也就越大，随机噪声会随着预测时间之间的间隙增加而增加。

• (2)测量噪声是指传感器测量中的不确定性。

那么在这两种途径里，如何去有效的融合提取呢？在前面提到，协方差也代表了其测量值与均值（真值）之间的关系，那么协方差越小也代表了其可信度的程度就越高。因此需要权衡这两种途径所得到的预测状态，通过协方差计算融合二者的权重，也就是卡尔曼增益Kg。

首先这两种途径计算得到的预测状态，依然是符合高斯分布的，那么融合两个高斯分布，即两个分布相乘。并且两高斯分布的乘积仍然是高斯态分布，且新的高斯分布的均值和方差可通过如下公式计算：
$$kg=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$$$${\mu }_{new}={\mu }_0+k({\mu }_1-{\mu }_0)$$$${\sigma }_{new}=k{\sigma }_1^2=(1-k){\sigma }_0^2$$
以上是单变量概率密度函数的计算结果，如果是多变量的，那么，就变成了协方差矩阵的形式：
$$KG=\frac{{\Sigma }_0}{{\Sigma }_0+{\Sigma }_1}$$$${\overrightarrow{\mu }}_{new}={\overrightarrow{\mu }}_0+K({\overrightarrow{\mu }}_1-{\overrightarrow{\mu }}_0)$$$${\Sigma }_{new}=K{\Sigma }_1=(I-K){\Sigma }_0$$

所以对于卡尔曼滤波系统中所说的预测和更新，预测就是通过状态转移函数计算当前状态值，而更新就是融合了当前的观测状态，相当于去更新预测值。所以更新本质上就是在计算两个高斯分布融合后的均值和协反差矩阵。这样以来，又可以利用更新后的状态继续预测下一时刻的状态，循环下去，且每一次计算仅和上一状态有关。

---

理解完卡尔曼滤波后，扩展卡尔曼滤波只是对非线性系统进行泰勒展开线性化后，利用卡尔曼滤波的思想。

由于线性化方式的不同，也出现了无迹卡尔曼滤波（UKF），主要思想是“近似概率分布要比近似非线性函数更容易”，其并不用泰勒展开求解，而是取附近点近似估计，避免了求解雅克比矩阵，效率更高。

在具体的实现上，根据以上公式，定义如下参数：

• 预测：
  $$x^{\prime }=Fx+\mu$$$$P^{\prime }=FP{F}^T+Q$$状态量：$x$，状态转移函数：$F$，协方差矩阵：$P$，运动噪声：$\mu$，高斯分布不确定性干扰：$Q$

• 更新：
  $$K=\frac{P^{\prime }{H}^T}{HP{H}^T+R}$$$$x=x^{\prime }+K(z-H{x}^{\prime })$$$$P=(1-KH)P^{\prime }$$卡尔曼增益：$K$，测量函数：$H$，协方差矩阵预测值：$P$，测量值协方差：$R$，预测值：$x$，测量值：$z$

对于一个非线性函数，需要计算其雅可比矩阵$F_j$ , $H_j$​将其线性化。

代码待更…