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

 一谈到贝叶斯滤波，就开始联系到各种随机过程、概率密度函数等等，那些曾经上课都听不进去的东西，这里能讲清楚吗？我自己也会有这个疑惑，不过要看懂贝叶斯滤波原理还是需要一定基础的。这篇博客，我会结合自己的理解尽量讲得通俗、方便理解一点。
 
 这篇博客大部分参考了b站视频：忠厚老实的王大头
 感谢up主做的视频

一、先验知识

1. 随机过程与概率论

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

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

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

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

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

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

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

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

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

二、贝叶斯滤波

1. 问题建模

（1） 符号定义：

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

（2） 问题描述

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

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

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

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

 我们首先要根据上一时刻k-1的状态来预测当前状态，得到 f k − ( x ) f^-_k(x) f