引言:我在前段时间因为一些有关陀螺仪与云台的控制问题,而接触了卡尔曼滤波。一开始对卡尔曼滤波的感觉近乎“玄学”,在实际应用了一段时间后,重新学习,有了一些较新的理解,故本文在此进行一些简单的探讨以加深对卡尔曼滤波的理解。
卡尔曼滤波(Kalman filter)是由鲁道夫·E·卡尔曼发明的一种滤波算法,是一种高效率的递归滤波器,主要适用于满足线性高斯分布的系统。在技术领域有着许多的应用,其首次的应用是在NASA的阿波罗计划中由斯坦利·施密特实现的,用于对火箭的轨道预测。并且在当今的控制理论、计算机视觉等领域仍然发挥着作用。
卡尔曼滤波发展至今已经演化出诸多不同的形式,如拓展卡尔曼滤波(EKF)、卡尔曼-布西滤波等。由于本人自身水平有限,在此只对“原始”的卡尔曼滤波做一些简单的理解和探讨。
卡尔曼滤波主要由下列五个公式组成。通常,式①②被称为预测公式,式③④⑤则被称为更新公式,下面我们从一个简单模型来探讨这些变量的含义。(下面所讨论的变量中带上标“-”均表示先验估计)
①
②
③
④
⑤
假如我们需要测定一辆路上行驶车辆的位置,我们可以通过传感器(卫星)测定其位置p,同时我们将车本身所自有的属性称为状态值
,而有传感器观测得到的属性
称为观测值
。但是传感器总是存在缺陷而导致我们所测定的状态并不准确,即传感器存在测量噪声,定义测量噪声的协方差为R。同时在一段较小的时间内我们可以将车视为匀速运动,所以我们不仅可以从卫星实时测量得到位置,也可以通过上一时刻的状态
来得知此刻的状态
(先验估计),即
则被称为状态转移矩阵F。可实际上,车辆并不可能总是保持匀速直线运动的,所以我们所建立的模型也会有误差,即存在过程噪声,定义过程噪声的协方差为Q(在卡尔曼滤波里,Q和R都是服从高斯分布的噪声的协方差,现实中大部分情况也确实如此)。不仅如此,我们可能会通过踩油门或者刹车改变此时车辆的运动状态,这种改变被称为控制量
,由于这种情况的加入比较复杂,在此我们不讨论B与
。
此时我们就得到了先验估计状态值与观测值
,它们都可以表示车辆此时可能所在的位置p。我们该如何从中进行选取,即是卡尔曼滤波器所需要做的事了。
由式④可看出卡尔曼滤波通过卡尔曼增益来完成对于
与
一个权重取值,实际上卡尔曼滤波的主要作用就是进行这样的权重取值,决定我们的后验估计状态值
是应该更倾向于
还是
。观测矩阵H是对
进行维度变换以与
运算:
在讨论前,我们先介绍状态估计协方差
,顾名思义
是一个协方差矩阵,我们由矩阵协方差的公式:
可以推导出式②。同时在这个示例中Pt取值如下:
一般来说观测矩阵H是恒定的,那么假设我们对系统的更新的时间间隔也是恒定的,即状态转移矩阵F不变,此时
与
只受Q和R的影响。回到上面我们所谈到的权重问题,现在我们可看出,我们
对于
与
权重取值是取决于Q和R的。
在上面我们已经介绍过了Q和R分别是根据模型所存在的系统误差和传感器所存在的测量误差得到的。在我们朴实的观点中,如果Q较大,即系统误差较大,那么对于由我们所定义的匀速直线运动模型得出的的信任度应当较低,所以最后
的取值应当更倾向于
;反之,如果R较大,最后
的取值应当更倾向于
。在卡尔曼滤波中也确实是这样的。
为了方便理解,现在我们假设我们的状态值xt和观测值zt
都是一维的,即:
那么会有F=H=1,式③则可化简为:
可看出如果Q增加R减小,则增加,由式④知此时
的取值更倾向于
;反之,
的取值更倾向于
。符合上述我们的观点。
观察式②③⑤,可以发现值的更新几乎是独立于系统运行状态外的,它不受观测值的影响,如果状态转移矩阵F不变,它其实就是一直处于一种“自更新”的状态。如下图:
在我的一些测试当中发现,当它更新到一定次数时,会收敛到一定的值。下为其中一组测试数据(Kalman Gain取矩阵中各个值的总和):
综上所述,卡尔曼滤波的作用其实可以简单地看作为:根据系统噪声和测量噪声的协方差设定权重来纠正状态值,且使状态值更倾向于协方差较小的一方。