卡尔曼滤波器知识整理

首先，简要介绍下什么是卡尔曼滤波器：

60年代初，Kalman和Bucy最先提出基于状态空间的递推滤波算法，即卡尔曼滤波算法。卡尔曼滤波器是一种自回归滤波器，建立在信号和噪声的状态空间描述基础上，要求系统状态方程和量测方程都是线性方程，系统噪声和观测噪声均为高斯白噪声且不相关，在统计特性已知的条件下，是对动态系统未知状态进行线性最小方差无偏最优估计的一种递推算法。能够通过一系列不完全和包含噪声的测量值，估计出动态系统的状态。使用状态空间法在时域内设计滤波器，适用于多维随机过程的估计；且具有连续型和离散型两类算法，其中离散型算法很容易实现数字化。算法只需利用当前时刻的测量值和前一时刻的估计值，即可由模型递推出当前时刻状态的估计值。该算法将状态变量引入滤波理论，能够解决时变、多变量、非平稳时间序列的滤波问题；由于这是一种可递推算法，容易在计算机上实现；无需存储历史测量值，计算量和存储量相对较小，便于实时在线处理，这些特点都弥补了wiener滤波的不足，随着计算机技术和计算技术的快速发展，卡尔曼滤波器逐渐得到了广泛的研究和应用。

基于卡尔曼滤波器的方法要求系统模型已知，当模型比较精确的时候，通过比较滤波器的输出与实际输出的残差，实际调整滤波器的参数，通过对系统状态估计误差协方差的极小化，得到递推估计的一组方程，由于它同时得到系统的预报方程，因此在预报领域也得到大量的应用。

下面介绍离散型卡尔曼滤波器的原理。考虑如图所示的线性、离散、定常的系统：

 

系统状态方程为： 

系统的测量方程为：

其中 表示系统状态向量， 是测量输出， 是系统干扰， 是测量噪声，t表示离散时刻下标。 和 是不相关的均值为0的高斯随机向量，协方差分别为 Q≥0，R>0，A为转移矩阵，C为观测矩阵。其中A、B、C、Q、R保持不变。K是卡尔曼增益矩阵。假设下列初始条件成立：

这里E(x)表示对随机变量x求期望， 为x的期望。

卡尔曼滤波用反馈的方法来递推待估计的状态。卡尔曼滤波器先估计某一时刻时过程的状态，然后以含噪声测量变量的方式获得反馈。

滤波器分为两个部分：时间更新方程和测量更新方程，其中时间更新方程部分主要起到预测作用，负责推算当前状态 和误差协方差矩阵 的估计值，为下一个时间状态提供先验估计；测量更新方程部分主要起校正作用，负责反馈，将先验估计值和新的测量值相结合，为下一个时间状态提供修正后的后验估计。

误差协方差矩阵

。

卡尔曼滤波是一种递推算法，启动时必须给出初值 和。当 时，卡尔曼滤波的估计从滤波开始起就是无偏的，且估计的均方误差是最小的。 

工作原理图如下图所示。

第一步：时间更新
 

已知t时刻最优估计值 ，由式（1-1）预测状态先验估计值 ，由式（1-2）预测状态的先验误差协方差矩阵 

 

第二步：测量更新

计算卡尔曼增益 

   

由测量值更新估计

 更新误差协方差

通过观测误差方差最小且状态无偏的原则求出卡尔曼增益并对先验估计值进行修正，由（1-4）可以得到状态变量的最优估计值 ，同时由（1-5）求出最优估计方差矩阵 。 由（1-1）到(1-5)可以看出，卡尔曼滤波是种递推算法。在已知初始条件 和 的条件下，由t+1时刻的量测值 和t+1时刻的输出观测值 之间的偏差和卡尔曼增益，对先验估计值 不停的修正，从而得到最优估计值 。

由上面的公式可以看出，实际上决定了对量测值和上一步估计值利用的比例程度，若增大，则的利用权重增大，而的利用权重相对降低。由量测噪声方差R和上一步估计的均方误差决定。假设Q一定，若量测精度很差，即R很大，则很小，结果是对的利用权重减小，而对的利用权重增大。若精度很差，即很大，而量测精度很高，即R很小，则变大，确定时对的利用权重增大，而对的利用权重相对降低。因此卡尔曼滤波器能定量识别各种信息的质量，自动确定对这些信息的利用程度，是一种具有一定只能功能的算法。

卡尔曼滤波采用这种预测和修正相结合的递归推算方式，在每一时刻，只需根据最新得到的观测数据便可得到当前的状态最优估计值，无需存储大量的历史观测数据，具有良好的实时性，易于计算机实现。 注：若卡尔曼滤波器是一致渐近稳定的，且系统的系数矩阵为时不变矩阵，则随着滤波次数的增加，最优估计值最终将不依赖于系统的初始值，实现无偏估计。卡尔曼滤波器的收敛性会受到多种因素的影响，比如：计算过程中的舍入误差、模型误差以及被研究系统的可观测性等。

---

非定常系统如下：

系统状态方程和测量方程：

各个量代表的含义：

Kalman滤波的递推算法：

 其中第（5）步经常使用上面那个方程求解 ，因为虽然下面的方程形式简单，计算量较小，但是计算中的积累误差容易使失去非负定性甚至对称性。

增益计算回路是独立回路，滤波计算回路依赖增益计算回路。

含有控制量的系统描述，考虑X(k)=AX(k-1)+BU(k-1)+TW(k-1)，Y(k)=HX(k)+V(k)

其中U(k)为控制量，这种情况与以上描述的处理方法一样，只需要将控制量BU(k)加入到预测式中，增益阵和误差阵的递推式完全一样。

 

至于卡尔曼滤波器公式的原理推导，可见https://blog.csdn.net/heyijia0327/article/details/17487467

代码实现（MATLAB）：

卡尔曼滤波用于GPS导航代码实现

 

EKF扩展卡尔曼滤波器介绍：

扩展卡尔曼滤波器（EKF）

UKF无迹卡尔曼滤波器介绍：

无迹卡尔曼滤波器（UKF）