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

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）

首先，简要介绍下什么是卡尔曼滤波器：60年代初，Kalman和Bucy最先提出基于状态空间的递推滤波算法，即卡尔曼滤波算法。卡尔曼滤波器是一种自回归滤波器，建立在信号和噪声的状态空间描述基础上，要求系统状态方程和量测方程都是线性方程，系统噪声和观测噪声均为高斯白噪声且不相关，在统计特性已知的条件下，是对动态系统未知状态进行线性最小方差无偏最优估计的一种递推算法。能够通过一系列不完全和包含噪声的... 当只知道原系统的输入矩阵U | k和输出矩阵Y | k，但是无法得知某些状态X | k时，可以采用状态观测器的方法。 状态观测器即是将原系统模拟成一个观测器系统，观测器的输入矩阵为原系统的 U | k。若观测器完全复现了原系统，即观测器输出输出矩阵Ŷ | k将与原系统相等，这时即可认为原系统待观测状态矩阵X | k与观测器状态矩阵X^ | k相等；然而事实上观测器总是不能完全拟合原系统，因此引入了负反馈调节。 数学分析X|k = A * X|k-1 + B * U|k + W|k     根据贝叶斯估计的原理，卡尔曼滤波是利用已知系统模型的确定性特性和统计特性等先验 知识 与观测量获得最有估计，在有初始值的情况下，从先验值和最新观测数据中得到的新值的加权平均来更新状态估计。 1、卡尔曼滤波中的关键要素 状态向量及其协方差、系统模型、观测向量及其协方差、观测模型和滤波算法。 2、卡尔曼滤波中的误差与噪声 功率谱密度：单位带宽内的功率，也是单位带宽内的方差。 滤波过程中误差主要有3种：系统噪声，高斯-马尔可夫过程，白噪声。 白噪声序列(white noise sequence) 是零均 首先，对于一个我们关心的物理量，我们假设它符合下面的规律 其中，为该物理量本周期的实际值，为该物理量上一个周期的实际值，当然这个物理量可能不符合这个规律，我们只是做了一个假设。不同的物理量符合的规律不同，是我们的经验，我们根据这个规律可以预测我们关心的物理量。比如，我们关心的物理量是车速，如果车辆接近匀速运动时，则的取值为1，也就是这个周期与上个周期的速度相同。 下面我们再来看 原文链接：　http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/　　　　　　https://blog.csdn.net/u010720661/article/details/63253509 什么是卡尔曼滤波？ 　　你可以在任何含有不确定信息的...