无人驾驶学习（四）：贝叶斯滤波

注：学习笔记，参考他人较多，逐步积累

一、占据栅格地图--二值贝叶斯滤波

1.1 二值贝叶斯滤波背景

        每一个小栅格某一时刻有三种可能的状态--占据occupied、空闲free，其中被激光雷达点扫到的小栅格为占据栅格、未被扫到的为空闲栅格。假设单次激光雷达扫描时，扫到某一小格后，该小格的被占据概率为0.9，空闲的概率为0.1；没有扫到某一小格时，占据的概率为0.2，空闲的概率为0.8。（这几个数字为假设值）

        下面解决一个小格被连续几次扫描到/没有扫描到的问题，即几次扫描中有某些次扫描到了该小格，这几次之后，该小格被占据的概率为多少的问题。下面将格子的占据概率换一种表达方式，即用“置信度”表示。

        p(x)/p(-x)=p(x)/(1-p(x)) 其中x表示该小格被占据，p(x)表示被占据的概率

两边同时取自然对数：

        l(x)=ln(p(x)/(1-p(x)))  其中l(x)表示该小格被占据的置信度

反过来，该小格被占据的概率可以用置信度表示为：

        p(x)=1-1/(1+e^(l(x)))

1.2 贝叶斯滤波

        贝叶斯滤波计算套路：

        l(t)=l(t-1)+ln(p(x|zt)/(1-p(x|zt)))-ln(p(x)/(1-p(x)))

 

 

其中：

        l(t)为t时刻被占据的置信度，

        l(t-1)为t-1时刻被占据的置信度，

        zt为t时刻的观测值，

        p(x|zt)为在t时刻，在观测值（占据或空闲）的情况下占据的概率

        p(x)为不考虑实际情况，任一小格被占据的概率，称为先验概率，此种情况下为0.5

  

        假设1.1中某一小格被占据先连续被扫描到三次，接着没有被扫描到，置信度计算如下：

        初值：L0=ln(0.5/0.5)=0

        第一次被扫描到：L1=L0+ln(0.9/0.1)-L0=ln9

        第二次被扫描到：L2=L1+ln(0.9/0.1)-L0=ln9+ln9

        第三次被扫描到：L3=L2+ln(0.9/0.1)-L0=ln9+ln9+ln9

        第四次被扫描到：L4=L3+ln(0.2/0.8)-L0=ln9+ln9+ln9-log4≈5.2

        则四次扫描后被占据的概率p(4)=1-1/(1+e^5.2)≈0.9945

1.3 参考链接：

占据栅格地图（occupancy grid maps） -- 二值贝叶斯滤波应用https://blog.csdn.net/haishaoli/article/details/79647594

@meng