算法笔记-卡尔曼滤波器简单解释
动态时间规整/规划(Kalman Filter, 又称linear quadratic estimation)也是一个比较老的算法,大概在1960年左右被提出来,算法的名字来自算法早期的一个提出者Rudolf E. Kálmán。卡尔曼滤波器至今还是十分常用的一种滤波算法,并且在此基础上发展出了很多种延伸的算法。卡尔曼滤波器算法的核心可由几道简洁的公式表示,但对于刚刚接触的人来说可能比较难以彻底理解这些公式,这篇笔记尝从一个实际的运动场景出发,具体解释卡尔曼滤波器的核心算法。
1.简介
卡尔曼滤波器在监控、导航和控制系统上有大量的运用。在这些系统里面,系统的观测量和这些被观测量的真实值往往是有细微差异的,基于我们对这些系统的认识(例如已有的描述系统的模型)和观测值,我们能够更好地推测系统的真实状态。这也是卡尔曼滤波器的核心思路,下面会先从公式出发,然后到一个简单的例子,进一步介绍这个思想是如何体现的。
2.公式
首先,假设一个系统在t时刻的状态是跟t-1时刻的状态是有关的:
其中各项的意义如下:
xt 是一个代表系统在t时刻的真实状态向量,该向量可以包含着例如位置,速度等信息;
Ft 是t时刻的状态转换矩阵,代表着系统状态在t-1时刻对系统t时刻的影响,例如t-1时没有速度和加速度,那么在t时位置应该不变;
ut 是t时刻的控制输入向量,例如是否有踩油门和转方向盘等;
Bt 是t时刻的输入转换矩阵,即量化了输入对状态的影响;
wt 是t时刻的过程噪声,我们假设该噪声服从均值为0的多变量正态分布,其协方差矩阵我们标记为 Qt 。
然后,我们要留意到,我们观察或测量的值往往不能直接反应我们关心的状态值,而且测量也存在测量误差,我们用下面这一式子反应这些:
其中各项的意义如下:
zt 代表系统在t时刻的观察值向量,该向量包含着系统测量的结果;
Ht 是t时刻的测量转换矩阵,这个矩阵表示状态向量和观察值向量之间的关系,当观察值就是状态时,该矩阵为单位矩阵;
vt 是t时刻的测量误差,类似过程噪声,我们假设该误差服从均值为0的多变量正态分布,其协方差矩阵我们标记为 Rt 。
卡尔曼滤波器要解决的问题,或者说将式子(1)和(2)简单概括一下,就是已知 Ft , ut , Bt , zt , Ht , xt−1 并且 wt 与 vt 服从已知的正态分布1,求解 xt 。
我们首先假设所求状态服从一个多变量的正太分布,即 x∼(μ,Σ) 。然后卡尔曼滤波器是通过一个迭代的过程解决这个问题的,每一次迭代包含两步,分别是预测和观测两步。
第一步预测:
此处出现的几个新的项:
首先是带帽的状态量 x̂ ,主要用于与真实值区分开,这个值是卡尔曼滤波器的运算结果,即我们的对状态量的估计值。从另一个角度讲,这个就是我们估测的 μ 。
而 P 是一个协方差矩阵,也就是我们估计的 Σ 。
其中 t|t−1 指的是 t 时刻的预测步的结果,而
第二步观测: