对于一个正在运动中的小车来说,如何准确的知道它所处的位置?
理论家说:我可以通过牛顿公式来计算!
实践家说:给它装个GPS不就得了!
好吧,你们说的听上去都很有道理,但我们到底该相信谁?
现实情况是:
理论家没有考虑到现实存在的摩擦力、空气阻力、时间测量误差等因素,算出来的结果存在较大误差;
实践家没有考虑GPS的测量存在较大误差。
这样一说,感觉两位半斤八两,都有误差,感觉谁都不可信。不过,我们还是要解决问题的嘛,能不能让这两位合作一下呢,理论联系实际嘛,马克思他老人家说的准没错。
我们知道,误差其实是一种噪声,那么是不是可以用滤波的方法呢?
感觉可以,我们把他俩的结果加起来,再除二嘛,均值滤波谁不会呢!但是,但是传感器的测量值(不一定是GPS)有时候简直会上天哪,这样简单粗暴肯定不靠谱,那有没有更牛X的方法呢?
-------------------------------------------------------------------------------------------------严肃的分隔线----------------------------------------------------------------------------------------
在刚刚提出的例子里,小车的状态向量可以表示为:
这时候,理论家提出,他可以预测k时刻的小车状态,并很难受的承认,这个预测是有误差的,假设这个误差服从正态分布:
给出误差的协方差矩阵
假设小车是匀速行驶,使用基本的运动学公式来表示,有:
即:
其中:
称为预测矩阵。
现实中,小车肯定不是匀速的,因为各种阻力的存在,需要时不时的给小车一个向前的力,才能使它继续向前行驶:
假设某个时刻给了小车一个向前的力,产生了一个向前的加速度a,那么:
即:
其中:
称为控制矩阵。
对于施加给小车的加速度a,它也称为一个向量,叫做控制向量,常用来表示,即有:
到这里,理论家的事情也差不多了,我们为他补上最后一步。在系统会中,可能会有其他不可预知的外力可能会干扰预测结果,我们需要把这一部分误差也加进来,假设它也是一个服从正态分布的误差,我们已经知道:
则有:
可以看出,是有上一步得来的外部不确定性误差的总和。
这时,实践家也给出了他买的山寨GPS的参数,知道了这个GPS的误差范围,那么有如下关系:
其中, 为观测向量,
为传感器与现实单位换算的转换矩阵(这里因为GPS输出单位与现实一致,就是1:1的关系),
为GPS的正态分布协方差矩阵。
OK,到现在为止,理论和实践的要素都准备完成,是时候把他们整在一起了,我们先来看一幅图片:
图中紫色的小车正在行走,理论家计算出的位置为绿色曲线(正态分布),实践家测量出的位置为蓝色曲线(也是正态分布),那么我们如果把这两个正态分布融合起来,是不是得出一个更“瘦”的曲线呢?
如上图红色所示,我们得出了融合后的曲线,这个曲线的方差要比另外两个要小了,至于为啥高斯分布和高斯分布融合起来还是高斯分布,请参考论文《Products and Convolutions of Gaussian Probability Density Functions》有详细的推导和论证,这里就不多说啦。
现在,我们有了两个不同的数据集,分别是预测的数据和测量的数据,我们用 来归一到相同的单位,则有:
稍微加以变形,不难得出:
设:
那么上面的公式变为:
代入:
为了得到最后的结果,把上面三个式子化简,最终得到:
总结一下,我们把卡尔曼滤波的过程一般分为两大部分,预测更新和测量更新:
到这里,了解了卡尔曼滤波大概的推理和机制,说白了,卡尔曼滤波就是通过不断的迭代,将测量结果不断收敛的一种方法。可能到这里我们还是不太明白卡尔曼滤波有啥牛X的,那我们接下来具体计算一下。
以小车的运动为例,为了简化运算假设它是匀速行驶,我们每隔1秒获取一次GPS的数据并算出相对位移,数据如下:
此时,由上述条件容易得知:
1) 由于是匀速行驶,所以不需要控制量,可以忽略;
2) 1秒一次的更新间隔,预测矩阵F,这里是预测因子为1;
3) GPS输出的测量数据已转换为和现实一致,故H=1;
4) 假设小车初速为0,方差为1;
5) 就上面说的,GPS的误差是R=1米
可以看出,滤波后的结果相比测量的结果,是比较收敛的,接近于小车匀速运动的现实情况。
References:
[1]"A New Approach to Linear Filtering and Prediction Problems" by R.E.Kalman,1960
[2]"Kalman filter" in wikipedia
[3]"How a Kalman filter works,in pictures" by Bzarg's blog
[4]《Probabilistic Robotics》