The Kalman Filter Approach for Robot Localization


一维卡尔曼滤波

如果理解了前面使用直方图进行Localization的原理,也就大概理解了卡尔曼滤波的原理。这一节我们会继续对上一一篇文章中的例子再进行一些完善,将离散概率分布函数变成连续的概率分布,因为在现实世界中,位置往往是时间的函数,而时间是连续的,因此我们需要用一个连续概率函数对上述场景进行模拟。

不论概率分布式是离散还是连续的,其定位的原理是不变的,都是sense和move两个过程的交替更迭。

高斯函数

回顾前面介绍Localization的步骤,第一步是需要有一个先验概率,一个连续型的概率密度函数,这个函数可以使用高斯函数来表示,

对于高斯函数来说,方差越大,概率分布函数越“胖”,样本偏离均值的幅度越大;相反的,方差越小,样本偏离均值的幅度越小,概率分布函数越“瘦”。如果用高斯函数来描述机器人的位置,我们希望方差越小越好,这样均值位置就越接近机器人的真实位置。

更多关于高斯函数的特性请自行查阅文档。

Measurement Update

回到定位问题上,假设我们有一个先验的高斯概率密度函数为$f(x)$,其均值和方差分别为(mu=120, sigma=40),校验因子的概率密度函数为$g(x)$,其均值和方差分别为(mu=200,sigma=30),如下图所示

接下来机器人进行了一个次measure,根据贝叶斯公式,后验概率应该等于$f(x)*g(x)$,得到的结果如下图

从这个结果中,我们能观察出几条重要的结论:

  1. 计算后的高斯分布的均值位于120200之间
  2. 方差变的更小,峰值为三者最高,说明measure获取了新的信息,提高了位置预测准确率

产生这个结果的原因从直观上不太好理解,实际上可以从数学的角度进行证明,当两个高斯函数相乘后,产生的新的均值为

由于$\mu_2 > \mu_1$,很直观的有 $\mu_1<\mu<\mu_2$。相应的,产生的新的方差为

这个看起来没有那么直观,我们不妨代入几个数试试,令$\sigma^2_1 = \sigma^2_2 = 4$,$\sigma=2$的值为2;令$\sigma^2_1 = 8$,$\sigma^2_2 = 2$,$\sigma=1.6$,以此类推。

measure过程的Python代码如下:

def measure(mean1, var1, mean2, var2):
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
    new_var = 1/(1/var1 + 1/var2)
    return (new_mean, new_var)

为了简化计算,上述代码只返回了均值和方差,如果要计算对于高斯函数,将其带入即可

Motion Update

sense完之后就可以开始move了,move是一个损失信息的过程,为了量化损失的信息,我们引入一个损失概率密度函数,该函数也服从高斯分布,我们用(u,r)表示,其中u是均值,r是方差。为了计算损耗后的概率分布,我们只需要让当前的概率密度函数和损失概率密度函数“求和”即可:

因此我们可以写出move的代码:

def predict(mean1, var1, mean2, var2):
    new_mean = mean1 + mean2
    new_var = var1 + var2
    return (new_mean, new_var) 

sense & move

将生面的sense和move结合起来,我们就得到了一维卡尔曼滤波的完整过程:

import math
from Kalman.predict import predict
from Kalman.measure import measure

##先验概率
mu = 0
sig = 1000

##矫正因子
measurements = [5.0,6.0,7.0,9.0,10.0]
measurement_sig = 4.0

##损耗因子
motion = [1.0,1.0,2.0,1.0,1.0]
motion_sig = 2.0

## sense and move
for index in range(0,len(measurements)):
    #sense
    (mu,sig) = measure(mu,sig, measurements[index],measurement_sig)
    #move
    (mu,sig) = predict(mu,sig, motion[index],motion_sig)

上述代码逻辑和前面离散demo基本一致,不同的是概率密度函数变成了连续的高斯函数,这里就不再做过多的分析。下图为移动5次后的概率分布结果

Resources