你真的搞懂贝叶斯滤波了吗？

2023-11-04 08:30:40

一谈到贝叶斯滤波，就开始联系到各种随机过程、概率密度函数等等，那些曾经上课都听不进去的东西，这里能讲清楚吗？我自己也会有这个疑惑，不过要看懂贝叶斯滤波原理还是需要一定基础的。这篇博客，我会结合自己的理解尽量讲得通俗、方便理解一点。

这篇博客大部分参考了b站视频：忠厚老实的王大头
感谢up主做的视频

一、先验知识

1. 随机过程与概率论

两门都是大学学过的课程，那它们之间到底有什么关系呢？呃…其实大学期间自己也没理解，笑cry。

随机过程：研究的随机变量之间不独立，由于不独立，无法做随机实验，难度较高。
概率论：研究的随机变量之间需要独立，比如大数定律等，都以这个为前提。
什么叫随机实验？随机实验需要满足3个条件：①相同条件下，可重复进行； ②一次试验，结果不确定，但所有结果已知（比如抛硬币，要么正要么反，所有结果已知）； ③试验之前，结果未知。

2. 先验、似然、后验概率

大家可能都清楚，就是先验是因，后验是果，似然就是在因的条件下，发生果的概率。那么到底什么是因，什么是果呢？

有博主写过了，写的挺好的，这里就不赘述了：一个例子搞清楚（先验分布/后验分布/似然估计）

还是举个例子讲一下先验、似然、观测、后验。

比如在相机和IMU融合SLAM中（由于博主是做这个的，就以这个为例子了，可能对不是做这个的不友好，所以博主上面给了参考博客链接），机器人的状态（包括位姿、速度、偏置等）的概率分布是先验，相机拍摄的图像信息（特征点、特征匹配等）和IMU提供的运动信息（加速度、角速度积分成速度、位移、旋转），都是观测，你得先有机器人的状态，才会有这些观测。

P(状态) - 先验 , 记为 P(A)
P(状态/图像和运动信息) - 后验，在知道观测的条件下，状态的概率（知果求因） , 记为P(A/B)
P(图像和运动信息/状态) - 似然概率，在知道状态的条件下，发生这些观测的概率（知因求果），记为P(B/A)
贝叶斯公式： $P(B/A)*P(A) = P(A/B)*P(B)$ P(B/A)∗P(A)=P(A/B)∗P(B), 变形为： $P(B/A)=\frac{(P(A/B)*P(B)}{P(A)}$ P(B/A)=P(A)(P(A/B)∗P(B)
若有多个观测，那么 $P(B_j/A)=\frac{(P(A/B_j)*P(B_j)}{\sum_{i=1}^{i=n}P(A/B_i)*P(B_i)}$ P(Bj/A)=∑i=1i=nP(A/Bi)∗P(Bi)(P(A/Bj)∗P(Bj)

有一个问题，不知道大家是否想过，为什么状态会是一个概率，难道它是随机事件吗？
个人认为，是因为我们通常会认为我们估计出来的状态，由于传感器观测会包含随机噪声，所以状态也可认为服从一个高斯分布，它的均值就是最优的状态结果。

二、贝叶斯滤波

1. 问题建模

（1）符号定义：

X 表示先验，也就是状态变量，x 表示状态变量的一个取值。Y 表示观测，y 表示观测的一个取值。
Q 表示建模误差，R表示传感器误差，都认为服从高斯分布。
$f_k{(x)}$ fk(x)表示第k时刻的状态概率密度函数（pdf），它是 $P(X_k<x)$ P(Xk<x)对x的导数。
$f_k{(x)} = \frac{dP(X_k<x)}{dx}$ fk(x)=dxdP(Xk<x)
$f_{Q_k}(x)$ fQk(x)表示预测噪声 $Q_k$ Qk的概率密度函数， $f_{R_k}(x)$ fRk(x)表示观测噪声（传感器精度） $R_k$ Rk的概率密度函数
下面推导中，注意 $f_k{(x)}$ fk(x)、 $f{(x)}$ f(x)和 $f_{Y_k|X_k}(y_k|x_k)$ fYk∣Xk(yk∣xk)的区别， $f{(x)}$ f(x)里面的 $f$ f是状态转移函数，就是下面状态方程里面那个 $f$ f ; 而 $f_k{(x)}$ fk(x)表示k时刻状态量 $X_k$ Xk概率密度函数,同理， $f_{k-1}{(x)}$ fk−1(x)就表示 $X_{k-1}$ Xk−1概率密度函数 ; 如果是 $f_{Y_k|X_k}(y_k|x_k)$ fYk∣Xk(yk∣xk),就表示 $P(Y_k|X_k)$ P(Yk∣Xk)的概率密度函数。它们没有关系。

（2）问题描述

问题一：我们求的是状态，为什么扯这么多概率？
因为状态服从一个高斯分布，没有贝叶斯滤波前，随机性很高噪声很大，通过这些预测和观测我们把噪声很大的状态的先验概率分布，转化为经过预测和观测纠正的后验概率，降低它的随机性和噪声，噪声的降低体现在概率分布中的方差减小了（后验概率方差<先验概率方差）。后验概率分布的均值就是我们要求的最优状态估计结果。
$X_k$ Xk表示状态 $x_k$ xk的概率分布， $f_k{(x)}$ fk(x)表示状态 $x_k$ xk的概率密度函数，我们要求k时刻状态 $x_k$ xk的最优估计，那就是求 $X_k$ Xk的均值即可。那
$\mu_k = \int^{+\infty}_{-\infty}x*f_k{(x)}dx$ μk=∫−∞+∞x∗fk(x)dx
如果 $f_k{(x)}$ fk(x)是高斯分布，那可以从 $f_k{(x)}$ fk(x)的公式中直接获取。
所以我们的求解步骤: 求解 $f_k^+{(x)}$ fk+(x) -> 计算均值得到最优 $x_k$ xk
问题二：怎么求解 $f_k^+{(x)}$ fk+(x)呢？
现在我们有的数据是这样的：k-1时刻的状态 $x_{k-1}$ xk−1已知 ; 对 $x_{k}$ xk有传感器的观测数据
两部分信息我们都不想浪费掉，因此我们分两步计算 $f_k^+{(x)}$ fk+(x)

1] 根据上一时刻状态预测，称为预测步，对应着状态方程，预测得到 $f_k^-{(x)}$ fk−(x)。
2] 根据当前时刻的观测来估计，称为更新步，对应着观测方程,更新得到 $f_k^+{(x)}$ fk+(x)。

（3）状态方程和观测方程

$\begin{cases} X_k = f(X_{k-1})+Q_k \\ Y_k = h(X_k)+R_k \end{cases}$ {Xk=f(Xk−1)+QkYk=h(Xk)+Rk
假设 $X_0、Q_1、Q_2...Q_k、R_1、R_2...R_k$ X0、Q1、Q2...Qk、R1、R2...Rk相互独立。
状态量可能是一个向量，因为状态量通常不只有一个，在机器人里面，就包含位姿、速度、偏置等，因此大多数情况下，状态量是一个向量，那么 $Q_k、R_k$ Qk、Rk都是矩阵。另外，如果有多个传感器，那么每个时刻都会有多个观测，这里只写了一个传感器时候的例子。
现在有一组观测值， $y_1、y_2...y_k$ y1、y2...yk分别表示时刻 $x_1 、x_2...x_k$ x1、x2...xk对应的观测（或者叫传感器测量数据，R其实是表示的是传感器的精度）。

2、预测步推导（求先验）

我们首先要根据上一时刻k-1的状态来预测当前状态，得到 $f^-_k(x)$ fk−(x),其实就是求先验
$f_k{(x)} = \frac{dP(X_k<x)}{dx}$ fk(x)=dxdP(Xk<x)
要求解 $f_k{(x)}$ fk(x)，可以先求解 $P(X_k<x)$ P(Xk<x)，求解方法类似与泰勒一阶展开，展开到上一时刻状态量。
$\begin{aligned} P(X_k < x) &= \sum_{u = -\infty}^x{P(X_k = u)} , u连续取值 \\ 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) ，因为X_{k-1}和Q_k独立 \\ &=\lim_{\varepsilon->0}\sum_{v=\infty}^{+\infty}f_{Q_k}[u-f(v)]*\varepsilon*f_{k-1}(v)*\varepsilon,见下面注解1 \\ &=\lim_{\varepsilon->0}\int_{-\infty}^{+\infty}f_{Q_k}[u-f(v)]*f_{k-1}(v)dv*\varepsilon ，见下面注解2\\ 所以,P(X_k<x)&=\sum_{u=-\infty}^x\lim_{\varepsilon ->0}\int_{-\infty}^{+\infty}f_{Q_{k-1}}[u-f(v)]f_{k-1}(v)dv * \varepsilon \\ &=\int_{-\infty}^x\int_{-\infty}^{+\infty}f_{Q_k}[u-f(v)]*f_{k-1}(v)dvdu \\ 因此：f_k^-(x) &= \frac{dP(X_k<x)}{dx}=\int_{-\infty}^{+\infty}f_{Q_k}[x-f(v)]*f_{k-1}(v)dv \\ \end{aligned}$ P(Xk<x)P(Xk=u)所以,P(Xk<x)因此：fk−(x)=u=−∞∑xP(Xk=u),u连续取值=v=∞∑+∞P(Xk=u∣Xk−1=v)P(Xk−1=v)=v=∞∑+∞P[Xk−f(Xk−1)=u−f(v)∣Xk−1=v]∗P(Xk−1=v)=v=∞∑+∞P[QK=u−f(v)∣Xk−1=v]∗P(Xk−1=v)=v=∞∑+∞P[QK=u−f(v)]∗P(Xk−1=v)，因为Xk−1和Qk独立=ε−>0limv=∞∑+∞fQk[u−f(v)]∗ε∗fk−1(v)∗ε,见下面注解1=ε−>0lim∫−∞+∞fQk[u−f(v)]∗fk−1(v)dv∗ε，见下面注解2=u=−∞∑xε−>0lim∫−∞+∞fQk−1[u−f(v)]fk−1(v)dv∗ε=∫−∞x∫−∞+∞fQk[u−f(v)]∗fk−1(v)dvdu=dxdP(Xk<x)=∫−∞+∞fQk[x−f(v)]∗fk−1(v)dv

注解1,这里顺便把连续贝叶斯公式推导一下,看完推导也就明白上面公式了：
$\begin{aligned} 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)} \\ &= \lim_{\varepsilon ->0}\sum_{u=-\infty}^x \frac{P(y<Y<y+\varepsilon |X=u)P(u<X<u+\varepsilon)}{P(y<Y<y+\varepsilon)} \\ &=\lim_{\varepsilon ->0}\sum_{u=-\infty}^x \frac{f_{Y|X}(\eta_1 |u)*\varepsilon*f_X(\eta_2)*\varepsilon}{f_Y(\eta_3)*\varepsilon} ，其中\eta_1\in(y,y+\varepsilon),\eta_2\in(u,u+\varepsilon),\eta_3\in(y,y+\varepsilon) \\ &=\lim_{\varepsilon ->0}\sum_{u=-\infty}^x\frac{f_{Y|X}(y|u)*f_X(u)*\varepsilon}{f_Y(y)},因为\eta_1、\eta_3\in(y,y+\varepsilon),而\varepsilon\to0,所以取\eta_1、\eta_3=y,\eta_2取u \\ &=\int_{-\infty}^x\frac{f_{Y|X}(y|u)*f_X(u)}{f_Y(y)}du ，这里利用了定积分定义，参考注解2的知乎链接 \\ &=\int_{-\infty}^x\frac{f_{Y|X}(y|x)*f_X(x)}{f_Y(y)}dx \\ 又因为:\\ P(X<x|Y=y) &=\int_{-\infty}^xf_{X|Y}(x|y)dx \\ 因此:\\ \int_{-\infty}^xf_{X|Y}(x|y)dx&=\int_{-\infty}^x\frac{f_{Y|X}(y|x)*f_X(x)}{f_Y(y)}dx \\ 所以，&对连续随机变量，也有：\\ f_{X|Y}(x|y)&=\frac{f_{Y|X}(y|x)*f_X(x)}{f_Y(y)}(贝叶斯公式) \end{aligned}$ P(X<x∣Y=y)又因为:P(X<x∣Y=y)因此:∫−∞xfX∣Y(x∣y)dx所以，fX∣Y(x∣y)=u=−∞∑x(P(X=u∣Y=y)=u=−∞∑xp(Y=y)P(Y=y∣X=u)P(x=u)=ε−>0limu=−∞∑xP(y<Y<y+ε)P(y<Y<y+ε∣X=u)P(u<X<u+ε)=ε−>0limu=−∞∑xfY(η3)∗εfY∣X(η1∣u)∗ε∗fX(η2)∗ε，其中η1∈(y,y+ε),η2∈(u,u+ε),η3∈(y,y+ε)=ε−>0limu=−∞∑xfY(y)fY∣X(y∣u)∗fX(u)∗ε,因为η1、η3∈(y,y+ε),而ε→0,所以取η1、η3=y,η2取u=∫−∞xfY(y)fY∣X(y∣u)∗fX(u)du，这里利用了定积分定义，参考注解2的知乎链接=∫−∞xfY(y)fY∣X(y∣x)∗fX(x)dx=∫−∞xfX∣Y(x∣y)dx=∫−∞xfY(y)fY∣X(y∣x)∗fX(x)dx对连续随机变量，也有：=fY(y)fY∣X(y∣x)∗fX(x)(贝叶斯公式)
注解2：参考链接：利用定积分定义求极限的原理与套路，你会了吗？

3、更新步推导（求后验）

假设有一个传感器，在k时刻有观测 $Y_k=y_k$ Yk=yk,我们需要利用观测去更新 $f_k^-(x)$ fk−(x)成 $f_k^+(x)$ fk+(x),其实就是后验概率 $f_k(x|y_k)$ fk(x∣yk)
$\begin{aligned} f_{Y_k|X_k}(y_k|x) &=\lim_{\varepsilon \to 0}\frac{P(y_k<Y_k<y_k+\varepsilon|X_k=x)}{\varepsilon} \\ &=\lim_{\varepsilon\to 0}\frac{P(y_k-h(x)<Y_k-h(X_k)<y_k-h(x)+\varepsilon|X_k=x)}{\varepsilon} \\ &=\lim_{\varepsilon\to0}\frac{P(y_k-h(x)<R_k<y_k-h(x)+\varepsilon|X_k=x)}{\varepsilon} \\ &=\lim_{\varepsilon\to0}\frac{P(y_k-h(x)<R_k<y_k-h(x)+\varepsilon)}{\varepsilon},因为R_k和X_k独立\\ &=f_{R_k}[y_k-h(x)] \\ 所以，有： \\ f_k^+(x)&=f_k(x|y_k)=\frac{f_{Y_k|X_k}(y_k|x)*f_k^-(x)}{f_{Y_k}(y_k)} \\ &=\frac{f_{R_k}[y_k-h(x)]*f_k^-(x)}{f_{Y_k}(y_k)} \\ &=\eta*f_{R_k}[y_k-h(x)]*f_k^-(x) \\ 其中，\eta为：\\ \eta&=\{\int_{-\infty}^{+\infty}f_{R_k}([y_k-h(x)]*f_k^-(x)dx\}^{-1} \end{aligned}$ fYk∣Xk(yk∣x)所以，有：fk+(x)其中，η为：η=ε→0limεP(yk<Yk<yk+ε∣Xk=x)=ε→0limεP(yk−h(x)<Yk−h(Xk)<yk−h(x)+ε∣Xk=x)=ε→0limεP(yk−h(x)<Rk<yk−h(x)+ε∣Xk=x)=ε→0limεP(yk−h(x)<Rk<yk−h(x)+ε),因为Rk和Xk独立=fRk[yk−h(x)]=fk(x∣yk)=fYk(yk)fYk∣Xk(yk∣x)∗fk−(x)=fYk(yk)fRk[yk−h(x)]∗fk−(x)=η∗fRk[yk−h(x)]∗fk−(x)={∫−∞+∞fRk([yk−h(x)]∗fk−(x)dx}−1
经过更新步之后，状态量的方差大大降低，也就是经过贝叶斯滤波后减少了噪声，得到更优更可靠的状态估计值.

三、贝叶斯滤波递推过程

$\begin{aligned} f_0(x)&\xRightarrow{预测}f_1^-(x)=\int_{-\infty}^{+\infty}f_{Q_1}[x-f(v)]f_0(v)dv\xRightarrow{观测更新}f_1^+(x)=\eta_1*f_{R_1}[y_1-h(x)]*f_1^-(x) \\ &\xRightarrow{预测}f_2^-(x)=\int_{-\infty}^{+\infty}f_{Q_2}[x-f(v)]f_1^+(x)dv\xRightarrow{观测更新}f_2^+(x)=\eta_2*f_{R_2}[y_2-h(x)]*f_2^-(x) \\ &…… \\ &\xRightarrow{预测}f_k^-(x)=\int_{-\infty}^{+\infty}f_{Q_k}[x-f(v)]f_{k-1}^+(x)dv\xRightarrow{观测更新}f_k^+(x)=\eta_k*f_{R_k}[y_k-h(x)]*f_k^-(x) \\ 其中，\eta_k&=\{\int_{-\infty}^{+\infty}f_{R_k}[y_k-h(x)]*f_k^-(x)dx\}^{-1},其实就是后面那一坨的积分的倒数。\end{aligned}$ f0(x)其中，ηk预测f1−(x)=∫−∞+∞fQ1[x−f(v)]f0(v)dv观测更新f1+(x)=η1∗fR1[y1−h(x)]∗f1−(x)预测f2−(x)=∫−∞+∞fQ2[x−f(v)]f1+(x)dv观测更新f2+(x)=η2∗fR2[y2−h(x)]∗f2−(x)……预测fk−(x)=∫−∞+∞fQk[x−f(v)]fk−1+(x)dv观测更新fk+(x)=ηk∗fRk[yk−h(x)]∗fk−(x)={∫−∞+∞fRk[yk−h(x)]∗fk−(x)dx}−1,其实就是后面那一坨的积分的倒数。
公式中，除了初值即可认为是先验，又可以认为是后验，其他都需要经过预测和更新两个步骤，实验证明，初值即使很差，也不会影响后面时刻状态量的估计值。
这里递推的都是概率密度函数，要求各时刻的状态估计，只需要求均值即可。
$\hat{x}_k=\int_{-\infty}^{+\infty}x*f_k^+(x)dx$ x^k=∫−∞+∞x∗fk+(x)dx

四、贝叶斯滤波算法流程

(1) 设初值：初始状态 $x_0$ x0和它的概率密度函数 $f_0(x)$ f0(x)

(2) 预测步： $f_k^-(x)=\int_{-\infty}^{+\infty}f_{Q_k}[x-f(v)]*f_{k-1}^+(v)dv$ fk−(x)=∫−∞+∞fQk[x−f(v)]∗fk−1+(v)dv

(3) 更新步： $f_k^+(x)=\eta_k * f_{R_k}[y_k-h(x)]*f_k^-(x),其中，\eta_k=\{\int_{-\infty}^{+\infty}f_{R_k}[y_k-h(x)]*f_k^-(x)dx\}^{-1}$ fk+(x)=ηk∗fRk[yk−h(x)]∗fk−(x),其中，ηk={∫−∞+∞fRk[yk−h(x)]∗fk−(x)dx}−1
这里要提一下有些博客不准确的说法： $P(x_k|y_k)=\eta*P(y_k|x_k)*\int_{-\infty}^{+\infty}P(x_k|x_{k-1})*P(x_{k-1})dx_{k-1}$ P(xk∣yk)=η∗P(yk∣xk)∗∫−∞+∞P(xk∣xk−1)∗P(xk−1)dxk−1

(4) 求状态量： $\hat{x}_k=\int_{-\infty}^{+\infty}x*f_k^+(x)dx$ x^k=∫−∞+∞x∗fk+(x)dx

五、贝叶斯滤波算法优缺点

可以有效滤除噪声，得到比较精准的状态估计;
缺点也很明显，从 $f_{k-1}^+(x)\to f_k^-(x)$ fk−1+(x)→fk−(x),计算 $\eta$ η，计算期望 $\hat{x}_k$ x^k，都需要做无穷积分，大多数情况下没有解析解。

码农公寓