卡尔曼滤波对gps轨迹数据清洗_快速理解卡尔曼滤波

前序

今天跟大家聊一聊博主理解的卡尔曼滤波器。有兴趣看这篇文章的同学大多应该都是做导航定位、多传感器数据融合或者图像识别研究的，所以不再过多赘述卡尔曼滤波器应用的场景，而是以卡尔曼滤波器能解决的实际问题为切入点，解释卡尔曼滤波器。具体地，解释内容分为以下三个部分：

• 什么是卡尔曼滤波
• 什么问题可以用卡尔曼滤波进行处理
• 卡尔曼滤波与统计学

一、什么是卡尔曼滤波

一句话概括：在含有不确定信息的动态系统中，对系统状态的最优估计。

• 不确定信息指向系统的两种不确定：1.系统的状态变量是随机且服从高斯分配的；2.系统存在不确定的外部干扰。
• 系统状态由可以描述系统某种物理意义（例如温度、重量、速度、位置等）的变量组成，可以是单变量系统状态，也可以是多变量状态。在多变量状态中，变量之间可以是相关的也可以是不相关的。
• 最优估计是指求得的状态与系统状态真实值之间的偏离程度最小。估计的过程可以用一句话进行概括：根据当前时刻的仪器测量值和上一个时刻对当前时刻的预测值，计算得到当前时刻的最优量，并依据该最优量预测下一个时刻的值。

从最优估计的过程描述可以知道，卡尔曼滤波需要的数据仅仅是上一个时刻的最优值和当前时刻的测量值，是一种向前的递推，属于IIR滤波器范畴，且计算量和存储量要求都很少，可以进行实时处理，因此非常易于工程实现。

二、什么问题可以用卡尔曼滤波进行处理

卡尔曼滤波应用场景有很多，不再一一列举，大多数网上可查的举例都与相应的场景知识掺杂在一起，不利于初学者快速理解卡尔曼滤波。在这里，博主通过两个简单的例子让大家能够快速了解卡尔曼滤波到底能解决什么样的问题。

例子1：房间温度的研究

我们要研究一个房间的温度，以一分钟为时间单位，根据经验判断，这个房间的温度是恒定的，但是对这种经验我们不可能是完全相信的，可能存在上下几度的偏差，这种偏差是服从高斯分配的白噪声。另外房间里还放了一个温度计，且该温度计也不准确，测量得到的温度与实际值存在偏差。那怎么才能知道房间的实际温度到底是多少呢。利用卡尔曼滤波解决这个问题，对照卡尔曼滤波中最优估计的描述即是：通过预测的温度和温度计测量的温度去估计房间的实际温度。那到底怎么估计呢？我们先把问题放在着，继续往下，待清楚了卡尔曼滤波与统计学后，我们再回过头来解决这个具体的问题。

例子2：汽车行驶状态的研究

我们要研究一辆汽车的行驶状态，即汽车行驶的位置和行驶的速度，在这个例子中，我们设定一个前提，小车位置和速度的方向是确定的，且位置和速度的大小在其定义域内具有正态的高斯分布规律。用如下矩阵来描述我们关注的汽车行驶状态（位置和速度）：

上式中，

对于不熟悉标准差、方差、协方差、协方差矩阵定义的同学可通过下面链接直接复习。

标准差_百度百科​baike.baidu.com 协方差矩阵_百度百科​baike.baidu.com

根据卡尔曼滤波最优估计的描述，我们首先要利用当前时刻的最优估计

用矩阵A来表示上图中的状态预测步骤，该矩阵可以把当前时刻状态的每种可能状态均一一预测为下一个时刻的可能状态，这是卡尔曼滤波的一个前提，保证了只要当前状态中包含了真实状态，那么所预测的下一个时刻的状态就必定包含下一个时刻的真实状态。

通过汽车行驶系统进行说明如何获取矩阵A。使用基本的运动学方程

上式中，

预计汽车行驶速度不变。将状态变量代入上式可得：

那么所预测的

根据协方差的矩阵的性质，可以得到

至此，卡尔曼滤波通过预测更新系统内部状态已经完成。进一步地，为了让卡尔曼滤波更能解决实际的工程问题，还要考虑外部对系统的影响。

在现实世界中，外部对系统的影响一般可以分为确定的影响和不确定的影响，确定的影响一般由人为的控制量输入产生，譬如汽车驾驶员会踩油门让汽车加减速；不确定的影响一般由外部环境所的干扰引起，这种干扰无法进行准确的跟踪和量化，譬如汽车在行驶过程中会碰到一些坑洼，影响汽车的行驶状态。

• 控制量对系统的影响

以汽车驾驶员踩油门为例说明控制量对系统确定的影响。在驾驶员踩油门后，汽车行驶在期望加速度为

将状态变量代入上式可得：

式中，B为控制矩阵，

• 外部扰动对系统的影响

设外部扰动对系统影响的噪声为

在卡尔曼滤波应用中对外部扰动有一个假设的前提：外部扰动服从如下高斯分布

所以在状态预测方程中，可以看出预测后得到的状态量仍然服从高斯分布，即整个预测的过程是将当前的状态量

至此，我们就得到了系统通过卡尔曼滤波进行处理时的预测方程：状态预测方程（4）和协方差预测方程（5）。进行分析可知， 状态预测方程由当前的最优估计、下一个时刻的确定性的控制输入和环境中的不确定的扰动组成；相对应的协方差矩阵预测方程由当前时刻最优估计的协方差矩阵和不确定扰动的协方差矩阵组成。

虽然可以对汽车的状态进行预测了，但是这种预测往往是粗略和不精准的。在现实世界中，汽车一般还装有各种仪器设备（GPS、电子陀螺和里程仪等）来测量汽车的状态，即测量汽车的位置和速度。那么，接下来继续分析汽车上的测量设备测量所得到的状态是怎么对最终的估计产生影响的。

首先，引入一个矩阵

因此，在观测空间内所预测的状态

然后，假设在观测空间内传感器的输出

式中，

现在在观测空间内，出现了2组均满足高斯分布的系统状态，与例子1所描述的温度问题一样，此时该相信哪种状态，或者说该更相信哪种状态呢？

三、卡尔曼滤波与统计学

可以用融合高斯分布的方法来找到上述问题中的最优估计，对于融合高斯分布可以查看如下资料

legal high：数据融合——高斯分布的特性​zhuanlan.zhihu.com

根据上述资料我们可以得到，两个高斯分布（8）和（9）融合后得到的新的高斯分布为

其中

将式（6）和式（7）带入上述式子中最终得到卡尔曼滤波的一次最优估计，可以用下列5个式子进行描述

回到例1中的问题，我们假设我们在k时刻知道房间的温度是23度，同时该温度的标准差为 5度；温度计显示的温度为25度，温度计测量的标准差为4度。在不考虑外部环境影响的情况下(

所以利用卡尔曼滤波对k+1时刻房间温度的最优估算温度为24.218度，该温度的标准差为3.126度，k+1时刻卡尔曼滤波器的增益为0.609。