2.3 多传感器数据融合中的卡尔曼滤波理论

2.3.1 卡尔曼滤波简介

针对传感器信息的跟踪滤波算法，大多数工程技术人员会选用卡尔曼滤波算法。卡尔曼滤波算法是R.E.Kalman在1960年发表的一篇著名论文中所阐述的一种递归解算法。该算法在解决离散数据的线性滤波问题方面有着广泛的应用，特别是随着计算机技术的发展，给卡尔曼滤波提供了广泛的研究空间。卡尔曼滤波器是由一组数学方程所构成，它以最小化均方根的方式，来获得系统的状态估计值。滤波器可以依据过去状态变量的数值，对当前的状态值进行滤波估计，对未来值进行预测估计。

一个离散的线性状态方程和观测方程如下式所示：

其中，X（k）为状态向量，Y（k）为观测向量；W（k）为状态噪声，或称为系统噪声；V（k）为观测噪声。假定W（k）和V（k）为互不相关的白噪声序列，分别符合N（0，Q）和N（0，R）的正态分布。

系统噪声的协方差矩阵为

观测噪声的协方差矩阵为

卡尔曼滤波器就是在已知观测序列{Y（0），Y（1），…，Y（k）}的前提条件下，要求解X（k）的估计值，使得后验误差估计的协方差矩阵P（k/k）最小。其中

在式（2.5）中，e（k/k）为后验误差估计，它可以由下式求得：

定义先验误差估计如下式所示：

可以得到先验误差估计的协方差矩阵为

假定卡尔曼滤波的后验估计如下式所示：

将式（2.9）代入到式（2.6）中，得到

将式（2.10）代入到式（2.5），可得

假设：随机信号W（k）与V（k）与已知的观测序列{Y（0），Y（1），…，Y（k）}是正交的，则有E［W（k-1）Y（k-1）］=0，E［V（k-1）Y（k-1）］=0。

式（2.11）可以化简为

对式（2.12）求导，并令其为零，可得到

同理，可得到

因此，可以得到状态估计如下式所示：

状态预测估计为

进一步计算得出误差的协方差矩阵如下式所示：

由此可以获得卡尔曼滤波的递推公式如图2.7所示。