一、概述

kalman滤波是一个重要的融合手段,虽然最近几年,随着基于优化的方法不断推广,基于滤波的方法逐渐式微,但是,由于二者各有优缺点,滤波方法现在及将来都仍然是重要的融合手段之一。

在多传感器融合定位系统中,kalman解决的其实是这样一个任务

它的本质是,结合预测与观测,得到最“精确”的后验值。实际中,预测与观测均从传感器而来(预测往往从IMU、编码器等传感器递推而来,观测往往从GPS、雷达、相机等传感器而来),因此滤波器的作用便是结合各传感器得到一个最好的融合结果。

kalman滤波的推导会显得很复杂,而且有多种推导方式,我们理解其中一种就可以,大可不必全都掌握。就像孔乙己会沾沾自喜地告诉别人,茴香的茴有四种写法,没多大用处(当然,这里指的是工作人员,如果是研究滤波理论的,那深入到多深都不为过,就像如果你是专门研究汉字的,茴香的茴了解这四种写法可能都不够,还要研究它的演化史,甚至不同地域的不同发音)。

关于工程人员的理论深度,我们可以在本篇文章末尾再更详细地讨论一次。

二、概率基础知识

kalman 滤波的推导会用到大量的概率基础知识,为了后面的推导过程更流畅,我们就先把基础知识解决了。

1.概率、概率密度

上图中,

 [公式] 为x在区间[a,b]上的概率密度,它表示的是随机变量在区间的分布情况。

 [公式] 代表的是x在区间[c,d]上的概率,它是概率密度的积分

[公式]

我们平时所说“高斯分布”、“非高斯分布”均是指它的概率密度。

2.联合概率密度

x∈[a,b]y∈[r,s]的联合概率密度函数可以表示为 p(x,y) ,其积分表示x和y同时处在某个区间的概率,满足下式

[公式]

特别地,当x和y统计独立的时候,有

[公式]

3.条件概率密度

x关于y的条件概率密度函数可以表示为 p(x|y) ,其含义是,在 y∈[r,s]的前提下, x∈[a,b]的概率分布,并且满足下式

[公式]

特别地,当x和y统计独立的时候,有

[公式]

4.贝叶斯公式

联合概率密度分解成条件概率密度和边缘概率密度的乘积,即

[公式]

重新整理,即可得贝叶斯公式

[公式]

5.贝叶斯推断

贝叶斯推断可以理解为贝叶斯公式的运用,它是指,如果已知先验概率密度函数 p(x) ,以及传感器模型p(y|x),那么就可以根据贝叶斯公式推断出后验概率密度。

[公式]

实际中,贝叶斯推断有时也称为贝叶斯估计。

6. 高斯概率密度函数

一维情况下,高斯概率密度函数表示为

[公式]

其中 μ 为均值,

[公式] 为方差。

多维情况下,高斯概率密度函数表示为

[公式]

其中均值为 μ ,方差为 Σ 。一般把高斯分布写成

 [公式] 

7. 联合高斯概率密度函数

若有高斯分布

[公式]

则它们的联合概率密度函数可以表示为

[公式]

由于联合概率密度满足下式

[公式]

该式在高斯分布的前提下可以重新分解。

由于高斯分布中指数项包含方差的求逆,而此处联合概率的方差是一个高维矩阵,对它求逆的简洁办法是运用舒尔补。舒尔补的主要目的是把矩阵分解成上三角矩阵、对角阵、下三角矩阵乘积的形式,方便运算,即

[公式]

其中

 [公式] 称为矩阵D关于原矩阵的舒尔补。此时有

[公式]

利用舒尔补,联合分布的方差矩阵可以写为

[公式]

它的逆矩阵为

[公式]

联合分布 p(x,y) 仍为高斯分布

[公式]

它的指数部分的二次项包含如下内容

[公式]

最后得到两个二次项的和,由于同底数幂相乘后,底数不变,指数相加,且

[公式]

因此有

[公式]

8. 高斯随机变量的线性分布

在上面的例子中,若已知xy之间有如下关系

[公式]

其中G是一个常量矩阵, 

[公式] 为零均值白噪声,在实际中指的是观测噪声。则xy的均值和方差之间必然存在联系,其联系可通过以下推导获得。

均值:

[公式]

方差:

[公式]

方差的交叉项:

[公式]

同理可得:

[公式]

三、滤波器推导

由于kalman滤波是贝叶斯滤波的一种(如下图),而贝叶斯滤波又是递推式状态估计的一种,我们得从大到小把这些东西讲一讲,逐渐缩小到kalman滤波这个地方,这样容易把问题讲清楚。

1.状态估计模型

实际状态估计任务中,待估计的后验概率密度可以表示为

[公式]

其中

 [公式] 表示的是状态初始值,

 [公式] 表示从1到k时刻的输入,

 [公式] 表示从0到k时刻的观测。因此,滤波问题可以直观表示为,根据所有历史数据(输入、观测、初始状态)得出最终的融合结果。历史数据之间的关系,可以用下面的图模型表示

图模型中体现了马尔可夫性,即当前状态只跟前一时刻状态相关,和其他历史时刻状态无关。

该性质的数学表达为:

运动方程:

 [公式]

观测方程:

 [公式]

2. 贝叶斯滤波

根据贝叶斯公式,k时刻后验概率密度可以表示为

[公式]

根据观测方程,yk 只和 xk 相关,因此上式可以简写为

[公式]

利用条件分布的性质,可得

[公式]

再利用马尔可夫性,可得

[公式]

经过以上化简,最终后验概率可以写为

这种公式表达形式,其实就可以称为是贝叶斯滤波了。根据以上结果,可以画出贝叶斯滤波的信息流图如下

3.kalman滤波推导

终于进入到了这个环节,不过要说明的是,我们这里kalman滤波的推导并不是按照上面贝叶斯滤波的表达形式去推的,上面贝叶斯滤波的讲述只是为了从更大的角度说明kalman是贝叶斯滤波的一种,这样更容易理解它的本质。此处推导kalman的方式是广义高斯滤波。

在线性高斯假设下,模型的数学表达可以重新写为下面的形式

运动方程:

 [公式]

观测方程:

 [公式]

把上一时刻的后验状态写为

[公式]

则当前时刻的预测值为

[公式]

根据高斯分布的线性变化,它的方差为(可仿照第二节8中的推导过程自行推导)

[公式]

其中

 [公式] 为当前输入噪声的方差。

若把k时刻状态和观测的联合高斯分布写为

[公式]

根据第二节7中的推导结果,k时刻的后验概率可以写为

[公式]

其中

[公式]

[公式]分别为后验均值和方差。若定义

[公式]

则有

[公式]

把第二节8中的推导得出的线性变换后的均值、方差及交叉项带入上面的式子,可以得到

[公式]

上面三个方程与之前所述预测方程(如下),就构成了卡尔曼经典五个方程

[公式]

需要说明的是,若不把第二节8中的结果带入,而保留原始形式,则对应的五个方程被称为广义高斯滤波。

4.扩展kalman滤波(EKF)推导

当运动方程或观测方程为非线性的时候,无法再利用之前所述的线性变化关系进行推导,常用的解决方法是进行线性化,把非线性方程一阶泰勒展开成线性。即

运动方程:

 [公式]

观测方程:

 [公式]

其中

[公式]

[公式]

[公式]

[公式]

[公式]

[公式]

根据该线性化展开结果,可以得到预测状态的统计学特征为

[公式]

[公式]

同理,可得到观测的统计学特征为

[公式]

[公式]

[公式]

把均值和方差的具体形式,带入广义高斯滤波的公式,最终得到EKF下得经典五个公式

[公式]

5. 迭代扩展卡尔曼滤波(IEKF)推导

由于非线性模型中做了线性化近似,当非线性程度越强时,误差就会较大。但是,由于线性化的工作点离真值越近,线性化的误差就越小,因此解决该问题的一个方法是,通过迭代逐渐找到准确的线性化点,从而提高精度。

在EKF的推导中,其他保持不变,仅改变观测的线性化工作点,则有

[公式]

其中

[公式]

按照与之前同样的方式进行推导,可得到滤波的校正过程为

[公式]

可见唯一的区别是后验均值

 [公式] 更新的公式与之前有所不同。滤波过程中,反复执行这2个公式,以上次的后验均值作为本次的线性化工作点,即可达到减小非线性误差的目的。需要注意的是,在这种滤波模式下, 后验方差应放在最后一步进行。

[公式]

四、kalman滤波的物理理解

我个人有个习惯,就是喜欢从物理意义上去理解公式,而且我也坚持认为,只有从物理意义上理解了,才算是真正的理解了(虽然我还没有完全做到这一点,很多公式我还没有在物理上给出让自己信服的解释)。

公式是用来实现算法的,而不是理解算法的,这二者之间有非常大的区别。物理史上,很多好的理论创建的时候,也都是从一个简单的物理假设出发,再去用公式去证明它的正确性和梳理它的实现过程。

我们先再次列出kalman的这五个公式

[公式]

[公式]

[公式]

[公式]

[公式]

为了更好地理解它的物理意义,我们把(5)式拆开,并把这些公式重新排一下顺序(注意这个顺序并不是实际计算的顺序,只是方便理解),在每个公式前面加上它的物理意义。

1) 状态预测:[公式]

2) 根据预测的状态得到预测的观测: [公式]

3) 得到实际的观测值: [公式]

4) 计算实际的观测值和预测的观测值之间的差: [公式]

5) 把“预测的状态”和“观测反馈的目标状态”做加权: [公式]

这一步需要解释一下了:

a.预测的状态当然没什么好说的

b.“观测反馈的目标状态”指的是,如果预测的状态是准的,那么根据预测状态得到的预测观测

 [公式] 也应该是准的,那么这时候 

[公式] 就是0,反过来讲,当它不为0时,就等于观测告诉我,预测的值不准,我应该按观测指的方向把预测值修一修。

c.但是,这还没完,观测就一定是准的吗,不确定呦,因为如果知道谁一定准,直接用它就行了,何必还融合呢,所以合理的做法是搞一个加权,把“预测告诉我的状态”和“观测告诉我的状态”放在一起按照一定的加权系数计算一个值出来。就是5)中的这个公式了。

d.下面就是计算加权系数的公式

[公式]

到了这一步好像就万事大吉了,但实际上并不是,因为我们看到计算加权的时候用到了方差矩阵

 [公式] ,方差本质上就是权重啦,所以kalman的公式里必须有一个计算方差的步骤。

6) 计算方差: [公式]

这一步是不是结束了,发现仍然没有,我们看到方差的计算中出现了

 [公式] ,它是上一步融合结果的方差,也就是说,如果下一步还想让kalman计算下去,就得把这一步融合结果的方差也算出来。

7) 计算融合结果的方差(后验方差): [公式]

ok,这次终于完事了,抽根烟!

那么啰嗦了这么一大堆,其实我们可以看到,kalman的物理意义其实就是“把预测的值和观测的值做一个加权,而且这个加权权重是用方差计算的”。现在再回过头去看概述中的那张图,应该可以更好地理解了吧。

五、总结

kalman 推导的内容倒没什么可总结的了,此处可以讨论一下开头提到的那个话题,即工程人员到底应该对理论的掌握深入到什么程度。当然,这是一个开放性的话题,没有标准答案,我也只是表达我个人观点而已,抛砖引玉,不一定对。

首先理论是为工程服务的(仅限于工程领域),这个应该没什么疑问,那么理论的掌握就本着“够用”就好,怎样理解“够用”,我认为它包括两点,第一,是要理解它的物理含义,第二,是要能够在它因任务的不同而发生变化时,能相应地做出理论上的修改。对于这里的 kalman 来讲,它的物理含义我们已经介绍过了,完全可以用这五个公式描述,而变化,实际工程中,倒并没有需要kalman做过多的变化(那些停留在论文里,但很少被工程化的那些改进版的kalman滤波理论以外)。这样看,单单对于kalman来讲,我认为,即使一种推导方式都不会,也没啥大不了的,仍然可以理直气壮,不用觉得低人一头。真正应该感到羞愧的,是那些公式推导很熟悉,但是对这些方法在实际系统中表现出的优缺点却知之甚少的人。

必须说明的是,前面仅仅是对kalman这一件事来讲,它的五个公式已经满足“够用”的标准,对于其他事,比如前面讲的误差模型推导,那就真的得掌握了,因为不同传感器、甚至同一传感器不同精度,所需要的误差模型都不一样,这种灵活多变的需求,使它的结论本身不能满足“够用”的标准。

这个观点可能很多人并不同意,应该用的最多的反对理由就是“理论深度不够怎么行,况且理论掌握的深了又没有什么坏处”。对于前半句,其实我们通过对“够用”的分析,算是已经反驳过了,对于后半句,倒也不能说错,毕竟技多不压身嘛,不过我们应该注意到的是,这个“多”是要付出代价的,最宝贵的代价当属时间,在校生要挤更多时间做课题,工作的能抽出时间学习已经不容易,即使有一些时间,还想着去搞搞自己的kpi,总之,应该没有谁的时间是无限的。那么,这时候,刚才的后半句就得变成“在付出代价的情况下,相比于理论掌握的深入,他俩谁的价值更大”,相信大家肯定明白我在说什么。

一刀切(越多越好)谁都会,权衡(资源有限情况下的取舍)才是最难的事情,它需要认清每件事情的价值,这需要不间断的思考,甚至称得上是一门艺术。