Apollo学习笔记（28）贝叶斯滤波

本来是准备计划整粒子滤波的，但是粒子滤波的前提是贝叶斯滤波，所以就找了几篇大佬的贝叶斯拜读了一下，现在做个整理。首先，奉上大佬链接，下面是自己整理的。

1.二维随机变量的条件分布

假设有$(X,Y)$是二维离散型随机变量，其联合概率密度函数为
$$p_{ij}=P(X=x_i,Y=y_j),i,j=1,2,3,\dots \text{\hspace{1em}(1)}$$
在$Y=y$条件下的随机变量$X$的条件概率函数为，
$$p_{X|Y}(x_i|y)=P(X=x_i|Y=y)=\frac{P(Y=y,X=x_i)}{P(Y=y)}=\frac{P(Y=y|X=x_i)P(X=x_i)}{P(Y=y)}\text{\hspace{1em}(2)}$$

通过全概率公式将式(2)的分母展开，即可得到离散型随机变量的贝叶斯公式，
$$p_{X|Y}(x_i|y)=\frac{P(Y=y|X=x_i)P(X=x_i)}{{\sum }_{j=1}^{n}P(Y=y|X=x_j)P(X=x_j)},j=1,2,3,\dots \text{\hspace{1em}(3)}$$

2.二维连续型随机变量的条件分布

连续型随机变量的概率在$Y=y$时，$P(Y=y)$的概率为0，所以通过条件概率的定义无法进行连续型随机变量的条件分布求解。

只能用极限的思路做一点变形进行求解，假设$y<Y<y+\Delta y$，将连续型随机变量的条件分布从一点转化为在一个区间的概率分布，当然有$\Delta y\to 0$，则对于连续型随机变量的概率分布函数为
$$\begin{array}{rl}{F}_{X|Y}(x|y)& =\underset{\Delta y\to 0}{\lim }P(X\le x|y<Y<y+\Delta y)\\ & =\underset{\Delta y\to 0}{\lim }\frac{P(X\le x,y<Y<y+\Delta y)}{P(y<Y<y+\Delta y)}\\ & =\underset{\Delta y\to 0}{\lim }\frac{{\int }_{-\infty }^x{\int }_y^{y+\Delta y}p(x,y)dydx}{{\int }_y^{y+\Delta y}{p}_Y(y)dy}\\ & =\underset{\Delta y\to 0}{\lim }\frac{{\int }_{-\infty }^{x}p(x,y+{\epsilon }_0\Delta y)\Delta ydx}{p_Y(y+{\epsilon }_1\Delta y)\Delta y}(0<{\epsilon }_0<1,0<{\epsilon }_1<1)\\ & =\underset{\Delta y\to 0}{\lim }\frac{{\int }_{-\infty }^{x}p(x,y+{\epsilon }_0\Delta y)dx}{p_Y(y+{\epsilon }_1\Delta y)}\end{array}\text{\hspace{1em}(4)}$$

由于有$\underset{\Delta y\to 0}{\lim }$，且$Y$是连续随机变量，所以把${\epsilon }_0\Delta y=0,{\epsilon }_1\Delta y=0$代入式(4)有
$$F_{X|Y}(x|y)=\frac{{\int }_{-\infty }^{x}p(x,y)dx}{p_Y(y)}\text{\hspace{1em}(5)}$$

对式(5)右侧进行求导，可以得到$Y=y$条件下$X$的条件概率密度函数
$$p_{X|Y}(x|y)=\frac{p(x,y)}{p_Y(y)}=\frac{p(y|x)p_X(x)}{p_Y(y)}=\frac{p(y|x)p_X(x)}{{\int }_{-\infty }^{+\infty }p(y|x)p(x)dx}\text{\hspace{1em}(6)}$$
可以看出式(3)和式(6)很相似，其实仔细琢磨下会发现思路都是一样的。

另外可以看出，$p_Y(y)$与$x$的取值无关，因此经常会出现使用$\eta$来表示$p_Y(y{)}^{-1}$的情况，如下
$$p_{X|Y}(x|y)=\frac{p(y|x)p_X(x)}{p_Y(y)}=\eta p(y|x)p_X(x)\text{\hspace{1em}(7)}$$

3.状态估计

状态估计就是根据获取到的测量数据以及系统之前的状态方程来估算当前系统状态的方法。

传感器测量的数据一般都是离散的，即$t=0,1,2,3,\mathrm{4...}$，相应的，设定对应的观测值为$y_1,y_2,y_3,y_4...$，控制输入为$u_1,u_2,u_3,y_4...$，状态为$x_0,x_1,x_2,x_3,x_4...$（x_0为初始状态）。

4.马尔可夫假设

隐马尔可夫模型(Hidden Markov Model)有两个基本假设：

1. 齐次马尔可夫假设
2. 观测独立假设

齐次马尔可夫假设

马尔可夫假设，简单来说，就是根据上一时刻的状态和控制输入可以预测当前的系统状态。（这里是不是感觉和MPC的思想很相似）

取三个时刻：上一时刻(t-1)，当前时刻(t)，下一时刻(t+1)。

根据马尔可夫假设，现在的状态$x_t$是由上一时刻的状态$x_{t-1}$和当前时刻的控制输入$u_t$决定，下一时刻的状态$x_{t+1}$也类似。

观测独立假设

具体是指，任一时刻的观测只依赖于该时刻的马尔可夫链的状态，与其他观测无关。

这两个假设非常重要，根据马尔可夫假设，才能推导出递归贝叶斯的更新公式。

5.贝叶斯滤波的推导

贝叶斯公式

单条件贝叶斯公式
$$p(x|y)=\frac{p(y|x)p(x)}{{\int }_{-\infty }^{+\infty }p(y|x)p(x)dx}=\eta p(y|x)p_X(x)\text{\hspace{1em}(8)}$$

多条件贝叶斯公式

$$\begin{array}{rl}p(x|y,z)& =\frac{p(y,z|x)p(x)}{p(y,z)}\\ & =\frac{\frac{p(x,y,z)}{p(x)}p(x)}{p(y|z)p(z)}\\ & =\frac{\frac{p(x,y,z)}{p(x,z)}\frac{p(x,z)}{p(x)}p(x)}{p(y|z)p(z)}\\ & =\frac{p(y|x,z)p(z|x)p(x)}{p(y|z)p(z)}\\ & =\frac{p(y|x,z)p(x,z)}{p(y|z)p(z)}\\ & =\frac{p(y|x,z)p(x|z)p(z)}{p(y|z)p(z)}\\ & =\frac{p(y|x,z)p(x|z)}{p(y|z)}\\ & =\eta p(y|x,z)p(x|z)\end{array}\text{\hspace{1em}(9)}$$

由贝叶斯公式推导贝叶斯滤波

在推导公式之前，先说下先验概率、后验概率、似然概率还有边缘概率。

• 先验概率：在结果发生之前，根据历史规律确定原因的概率分布；
• 似然概率：根据原因来估计结果的概率分布，就是似然估计；
• 后验概率：根据结果来估计原因的概率分布，就是后验概率；
• 边缘概率：结果发生的概率，叫做边缘概率。

光这么说，是不是有点晕，可以看看大佬的博客，会理解的更详细点。

对于做定位的来说，

• $p(位姿)$，就是先验概率，根据经验推测产生位姿的概率，记做$P(A)$；
• $p(位姿|传感器和运动信息)$，后验概率，在知道传感器和运动信息的条件下，产生某个位姿的概率，记为$P(A|B)$；
• $p(传感器和运动信息|位姿)$，似然概率，在知道确定的位姿的条件下，曾经发生某些传感器信息或者运动的概率，记为$P(B|A)$
• $p(传感器和运动信息)$，边缘密度，传感器发出某些信息或者发生某些运动的概率，记为$P(B)$

7.随机过程的贝叶斯滤波

在机器人定位或者无人驾驶的定位中，对车辆或者机器人的运动估计就是一个随机过程。轨迹过程彼此之间并不是独立的，其符合马尔科夫链，如下所示：

其中$X0$为初始值，一般由经验确定（先验）。那么如何从初始状态$X0$，加上观测信息，从而估计到$k$时刻的状态$Xk$，同时又保证估计值的精度呢？

方法1：所有的$X0$ ~ $Xk$的先验概率都靠观测值，完全忽略了根据系统模型的预测值。这样做的缺点也很明显，完全依赖观测值，放弃使用预测信息，使得估计值的误差就是传感器的误差。

方法2：其中$X0$是靠先验猜测，其余的$X\mathrm{１}$ ~ $Xk$均靠系统模型的递推，此时，随着递推的次数增加，估计值的误差会一直增加，假设有系统状态方程$X_k=2X_{k-1}$，则有
$$\begin{array}{rl}{X}_1& =2X_0\backsim N(0,2^2)\\ X_2& =2X_1\backsim N(0,2^4)\\ \cdots & \\ X_k& =2X_{k-1}\backsim N(0,2^k)\end{array}$$

可以发现，如果只通过系统状态方程进行预测，误差会在不断的迭代过程中，导致方差会越来越大，这样明显是不想见到的结果。

这两种方式都各有各的问题，下面来看一下一种把系统状态方程和观测方程的提高准确度的方法。

预测：上一时刻的后验概率，结合系统状态方程，计算出这一时刻的先验概率
更新：这一时刻的先验概率，结合观测方程，计算出这一时刻的后验概率（下一时刻的先验概率）

这种思路和卡尔曼滤波是一样的，下面进行详细的推导：

已知，系统的状态方程和观测方程为
$$\{\begin{array}{l}{X}_k=f(X_{k-1})+Q_k\\ Y_k=h(X_k)+R_k\end{array}\text{\hspace{1em}(10)}$$

上式中，$X_0,Q_1\dots Q_k,R_1\dots R_k$相互独立，观测值$y_1,y_2\dots y_k$已知，初始状态量$X_0$，预测方差$Q_k$，和观测方差$R_k$都是已知。

下面给出一个重要定理：条件概率的条件可以做逻辑推导，比如：
$$P(X=1|Y=2,Z=3)=P(X+Y=3|Y=2,Z=3)=P(X+Y=3|Y=2,Z-Y=1)$$

预测步推导

首先，要通过前一时刻$k-1$的系统状态来预测当前$k$时刻的系统状态，得到$f_k^{-}(x)$，也就是求先验值
$$f_k^{-}(x)=\frac{dP(X_k<x)}{dx}\text{\hspace{1em}(11)}$$

要求解$f_k(x)$，必须要先求解出$P(X_k<x)$，求解方式与第一部分讲的随机变量的条件分布类似，如下
$$P(X_k<x)=\sum _{u=-\infty }^{x}P(X_k=u)\text{\hspace{1em}(12)}$$
式中，$u$为连续取值。
$$\begin{array}{rl}P(X_k=u)& =\sum _{v=-\infty }^{+\infty }P(X_k=u|X_{k-1}=v)P(X_{k-1}=v)\\ & =\sum _{v=-\infty }^{+\infty }P[X_k-f(X_{k-1})=u-f(v)|X_{k-1}=v]P(X_{k-1}=v)\\ & =\sum _{v=-\infty }^{+\infty }P[Q_K=u-f(v)|X_{k-1}=v]P(X_{k-1}=v)\\ & =\sum _{v=-\infty }^{+\infty }P[Q_K=u-f(v)]P(X_{k-1}=v)\\ & =\underset{\epsilon \to 0}{\lim }\sum _{v=-\infty }^{+\infty }{f}_{Q_k}[u-f(v)]\epsilon f_{k-1}(v)\epsilon \\ & =\underset{\epsilon \to 0}{\lim }{\int }_{-\infty }^{+\infty }{f}_{Q_k}[u-f(v)]f_{k-1}(v){\epsilon }^2\end{array}\text{\hspace{1em}(13)}$$

到此，对上式进行下解释，不然不太好理解。

第二步，是因为条件概率的逻辑条件可以做逻辑推导，所以可以这么写。

第三步，参考公式(10)。

第四步，因为$Q_K$和$X_{k-1}$相互独立，所以可以将第一项的条件去掉。

第五步，这里把连续型贝叶斯公式推导下，
$$\begin{array}{rl}P(X<x|Y=y)& =\sum _{u=-\infty }^{x}P(X=u|Y=y)=\sum _{u=-\infty }^x\frac{P(Y=y|X=u)P(X=u)}{P(Y=y)}\\ & =\underset{\epsilon \to 0}{\lim }\sum _{u=-\infty }^x\frac{P(y<Y<y+\epsilon |X=u)P(u<X<u+\epsilon )}{P(y<Y<y+\epsilon )}\\ & =\underset{\epsilon \to 0}{\lim }\sum _{u=-\infty }^x\frac{f_{Y|X}({\eta }_1|u)\cdot \epsilon \cdot f_X({\eta }_2)\cdot \epsilon }{f_Y({\eta }_3)\cdot \epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\sum _{u=-\infty }^x\frac{f_{Y|X}(y|u)\cdot f_X(u)\cdot \epsilon }{f_Y(y)}\\ & ={\int }_{-\infty }^x\frac{f_{Y|X}(y|u)\cdot f_X(u)}{f_Y(y)}du\\ & ={\int }_{-\infty }^x\frac{f_{Y|X}(y|x)\cdot f_X(x)}{f_Y(y)}dx\end{array}\text{\hspace{1em}(14)}$$

又因为，
$$P(X<x|Y=y)={\int }_{-\infty }^x{f}_{X|Y}(x|y)dx$$

因此，式（14）可以化为，
$${\int }_{-\infty }^x{f}_{X|Y}(x|y)dx={\int }_{-\infty }^x\frac{f_{Y|X}(y|x)\cdot f_X(x)}{f_Y(y)}dx\text{\hspace{1em}(15)}$$

所以，对于连续型随机变量，贝叶斯公式同样适用，
$$f_{X|Y}(x|y)dx=\frac{f_{Y|X}(y|x)\cdot f_X(x)}{f_Y(y)}dx\text{\hspace{1em}(16)}$$

好，这里继续式（13）后，继续推，

$$\begin{array}{rl}P(X_k<x)& =\sum _{u=-\infty }^{x}P(X_k=u)=\sum _{u=-\infty }^x\underset{\epsilon \to 0}{\lim }{\int }_{-\infty }^{+\infty }{f}_{Q_k}[u-f(v)]f_{k-1}(v)\cdot {\epsilon }^2\\ & ={\int }_{-\infty }^x{\int }_{-\infty }^{+\infty }{f}_{Q_k}[u-f(v)]f_{k-1}(v)dvdu\end{array}\text{\hspace{1em}(17)}$$

所以，有
$$f_k^{-}(x)=\frac{dP(X_k<x)}{dx}={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]\cdot f_{k-1}(v)dv\text{\hspace{1em}(18)}$$

更新步推导

根据传感器在$k$时刻的观测值$Y_k=y_k$，更新先验概率$f_k^{-}(x)$到后验概率$f_k^{+}(x)$，也可以写为$f_k(x|y_k)$，下面进行更新步的推导，

$$\begin{array}{rl}{f}_{Y_k|X_k}(y_k|x)& =\underset{\epsilon \to 0}{\lim }\frac{P(y_k<Y_k<y_k+\epsilon |X_k=x_k)}{\epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\frac{P(y_k-h(x_k)<Y_k-h(x_k)<y_k-h(x_k)+\epsilon |X_k=x_k)}{\epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\frac{P(y_k-h(x_k)<R_k<y_k-h(x_k)+\epsilon |X_k=x_k)}{\epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\frac{P(y_k-h(x_k)<R_k<y_k-h(x_k)+\epsilon )}{\epsilon }\\ & =f_{R_k}[y_k-h(x)]\end{array}\text{\hspace{1em}(19)}$$

上式中，第三步到第四步是因为$R_k$和$X_k$相互独立，因此，有

$$\begin{array}{rl}{f}_k^{+}(x)& =f_k(x|y_k)=\frac{f_{Y_k|X_k}(y_k|x_k)\cdot f_k^{-}(x)}{f_{Y_k}(y_k)}\\ & =\frac{f_{R_k}[y_k-h(x)]\cdot f_k^{-}(x)}{f_{Y_k}(y_k)}\\ & =\eta \cdot f_{R_k}[y_k-h(x)]\cdot f_k^{-}(x)\end{array}\text{\hspace{1em}(20)}$$

式中，
$$\eta ={\left\{\begin{array}{c}{\int }_{-\infty }^{+\infty }{f}_{R_k}[y_k-h(x)]\cdot f_{k-1}^{-}(x)dx\end{array}\right\}}^{-1}$$

经过更新步后，状态量的方差降低了很多，提高了状态估计的准确度。

8.贝叶斯滤波递推过程

$$\begin{array}{rl}{f}_0(x)& \stackrel{预测}{\Rightarrow }{f}_1^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_1}[x-f(v)]\cdot f_0(v)dv\stackrel{观测}{\Rightarrow }{f}_1^{+}(x)={\eta }_1\cdot f_{R_1}[y_1-h(x)]\cdot f_1^{-}(x)\\ & \stackrel{预测}{\Rightarrow }{f}_2^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_2}[x-f(v)]\cdot f_1^{+}(v)dv\stackrel{观测}{\Rightarrow }{f}_2^{+}(x)={\eta }_2\cdot f_{R_2}[y_2-h(x)]\cdot f_2^{-}(x)\\ & \dots \\ & \stackrel{预测}{\Rightarrow }{f}_k^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]\cdot f_{k-1}^{+}(v)dv\stackrel{观测}{\Rightarrow }{f}_k^{+}(x)={\eta }_k\cdot f_{R_k}[y_k-h(x)]\cdot f_k^{-}(x)\end{array}$$

其中，
$${\eta }_k={\left\{\begin{array}{c}{\int }_{-\infty }^{+\infty }{f}_{R_k}[y_k-h(x)]\cdot f_{k-1}^{-}(x)dx\end{array}\right\}}^{-1}$$

公式中，除了初值$f_0(x)$可以认为是先验，也可以认为是后验，其实的每个值$f_k(x)$都需要经过先验和后验两个步骤。

这里递推的都是概率密度函数，想求各个时刻的状态估计，只需要求均值即可。
$${\hat{x}}_k={\int }_{-\infty }^{+\infty }x\cdot f_k^{+}(x)dx\text{\hspace{1em}(21)}$$

这里总结下，贝叶斯滤波算法的流程:

1. 设定初始状态$x_0$和其概率密度$f_0(x)$；
2. 计算下一时刻的先验值：$f_k^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]\cdot f_{k-1}^{+}(v)dv$；
3. 计算后验值：$f_k^{+}(x)={\eta }_k\cdot f_{R_k}[y_k-h(x)]\cdot f_k^{-}(x)$；
4. 求当前的状态量：${\hat{x}}_k={\int }_{-\infty }^{+\infty }x\cdot f_k^{+}(x)dx$。

贝叶斯滤波可以有效的去除噪声，计算出比较准确的状态估计值，但是缺点也很明显，从先验计算后验，以及计算$\eta$时，都需要进行无穷积分计算，必须一点点加，没有一个解析解。

最后，在这里向拜读过的各个大神致敬，链接有点多，就不一一列举了。