一谈到贝叶斯滤波，就开始联系到各种随机过程、概率密度函数等等，那些曾经上课都听不进去的东西，这里能讲清楚吗？我自己也会有这个疑惑，不过要看懂贝叶斯滤波原理还是需要一定基础的。这篇博客，我会结合自己的理解尽量讲得通俗、方便理解一点。
 
 这篇博客大部分参考了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)∗P(A)=P(A/B)∗P(B), 变形为： P ( B / A ) = ( P ( A / B ) ∗ P ( B ) P ( A ) 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 ) = ( P ( A / B j ) ∗ P ( B j ) ∑ i = 1 i = n P ( A / B i ) ∗ P ( B i ) 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=n​P(A/Bi​)∗P(Bi​)(P(A/Bj​)∗P(Bj​)​

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

二、贝叶斯滤波

1. 问题建模

（1） 符号定义：

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

（2） 问题描述

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

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

{ X k = f ( X k − 1 ) + Q k Y k = h ( X k ) + R k \begin{cases} X_k = f(X_{k-1})+Q_k \\ Y_k = h(X_k)+R_k \end{cases} {Xk​=f(Xk−1​)+Qk​Yk​=h(Xk​)+Rk​​
 假设 X 0 、 Q 1 、 Q 2 . . . Q k 、 R 1 、 R 2 . . . R k 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 Q_k、R_k Qk​、Rk​都是矩阵。另外，如果有多个传感器，那么每个时刻都会有多个观测，这里只写了一个传感器时候的例子。
 现在有一组观测值， y 1 、 y 2 . . . y k y_1、y_2...y_k y1​、y2​...yk​分别表示时刻 x 1 、 x 2 . . . x k x_1 、x_2...x_k x1​、x2​...xk​对应的观测（或者叫传感器测量数据，R其实是表示的是传感器的精度）。

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

 我们首先要根据上一时刻k-1的状态来预测当前状态，得到 f k − ( x ) f^-_k(x) fk−​(x),其实就是求先验
f k ( x ) = d P ( X k < x ) d x f_k{(x)} = \frac{dP(X_k<x)}{dx} fk​(x)=dxdP(Xk​<x)​
 要求解 f k ( x ) f_k{(x)} fk​(x)，可以先求解 P ( X k < x ) P(X_k<x) P(Xk​<x)，求解方法类似与泰勒一阶展开，展开到上一时刻状态量。
P ( X k < x ) = ∑ u = − ∞ x P ( X k = u ) , u 连 续 取 值 P ( X k = u ) = ∑ v = ∞ + ∞ P ( X k = u ∣ X k − 1 = v ) P ( X k − 1 = v ) = ∑ v = ∞ + ∞ P [ X k − f ( X k − 1 ) = u − f ( v ) ∣ X k − 1 = v ] ∗ P ( X k − 1 = v ) = ∑ v = ∞ + ∞ P [ Q K = u − f ( v ) ∣ X k − 1 = v ] ∗ P ( X k − 1 = v ) = ∑ v = ∞ + ∞ P [ Q K = u − f ( v ) ] ∗ P ( X k − 1 = v ) ， 因 为 X k − 1 和 Q k 独 立 = lim ⁡ ε − > 0 ∑ v = ∞ + ∞ f Q k [ u − f ( v ) ] ∗ ε ∗ f k − 1 ( v ) ∗ ε , 见 下 面 注 解 1 = lim ⁡ ε − > 0 ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] ∗ f k − 1 ( v ) d v ∗ ε ， 见 下 面 注 解 2 所 以 , P ( X k < x ) = ∑ u = − ∞ x lim ⁡ ε − > 0 ∫ − ∞ + ∞ f Q k − 1 [ u − f ( v ) ] f k − 1 ( v ) d v ∗ ε = ∫ − ∞ x ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] ∗ f k − 1 ( v ) d v d u 因 此 ： f k − ( x ) = d P ( X k < x ) d x = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] ∗ f k − 1 ( v ) d v \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=−∞∑x​P(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​独立=ε−>0lim​v=∞∑+∞​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,这里顺便把连续贝叶斯公式推导一下,看完推导也就明白上面公式了：
  P ( X < x ∣ Y = y ) = ∑ u = − ∞ x ( P ( X = u ∣ Y = y ) = ∑ u = − ∞ x P ( Y = y ∣ X = u ) P ( x = u ) p ( Y = y ) = lim ⁡ ε − > 0 ∑ u = − ∞ x P ( y < Y < y + ε ∣ X = u ) P ( u < X < u + ε ) P ( y < Y < y + ε ) = lim ⁡ ε − > 0 ∑ u = − ∞ x f Y ∣ X ( η 1 ∣ u ) ∗ ε ∗ f X ( η 2 ) ∗ ε f Y ( η 3 ) ∗ ε ， 其 中 η 1 ∈ ( y , y + ε ) , η 2 ∈ ( u , u + ε ) , η 3 ∈ ( y , y + ε ) = lim ⁡ ε − > 0 ∑ u = − ∞ x f Y ∣ X ( y ∣ u ) ∗ f X ( u ) ∗ ε f Y ( y ) , 因 为 η 1 、 η 3 ∈ ( y , y + ε ) , 而 ε → 0 , 所 以 取 η 1 、 η 3 = y , η 2 取 u = ∫ − ∞ x f Y ∣ X ( y ∣ u ) ∗ f X ( u ) f Y ( y ) d u ， 这 里 利 用 了 定 积 分 定 义 ， 参 考 注 解 2 的 知 乎 链 接 = ∫ − ∞ x f Y ∣ X ( y ∣ x ) ∗ f X ( x ) f Y ( y ) d x 又 因 为 : P ( X < x ∣ Y = y ) = ∫ − ∞ x f X ∣ Y ( x ∣ y ) d x 因 此 : ∫ − ∞ x f X ∣ Y ( x ∣ y ) d x = ∫ − ∞ x f Y ∣ X ( y ∣ x ) ∗ f X ( x ) f Y ( y ) d x 所 以 ， 对 连 续 随 机 变 量 ， 也 有 ： f X ∣ Y ( x ∣ y ) = f Y ∣ X ( y ∣ x ) ∗ f X ( x ) f Y ( y ) ( 贝 叶 斯 公 式 ) \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)因此:∫−∞x​fX∣Y​(x∣y)dx所以，fX∣Y​(x∣y)​=u=−∞∑x​(P(X=u∣Y=y)=u=−∞∑x​p(Y=y)P(Y=y∣X=u)P(x=u)​=ε−>0lim​u=−∞∑x​P(y<Y<y+ε)P(y<Y<y+ε∣X=u)P(u<X<u+ε)​=ε−>0lim​u=−∞∑x​fY​(η3​)∗εfY∣X​(η1​∣u)∗ε∗fX​(η2​)∗ε​，其中η1​∈(y,y+ε),η2​∈(u,u+ε),η3​∈(y,y+ε)=ε−>0lim​u=−∞∑x​fY​(y)fY∣X​(y∣u)∗fX​(u)∗ε​,因为η1​、η3​∈(y,y+ε),而ε→0,所以取η1​、η3​=y,η2​取u=∫−∞x​fY​(y)fY∣X​(y∣u)∗fX​(u)​du，这里利用了定积分定义，参考注解2的知乎链接=∫−∞x​fY​(y)fY∣X​(y∣x)∗fX​(x)​dx=∫−∞x​fX∣Y​(x∣y)dx=∫−∞x​fY​(y)fY∣X​(y∣x)∗fX​(x)​dx对连续随机变量，也有：=fY​(y)fY∣X​(y∣x)∗fX​(x)​(贝叶斯公式)​

• 注解2：参考链接：利用定积分定义求极限的原理与套路，你会了吗？

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

 假设有一个传感器，在k时刻有观测 Y k = y k Y_k=y_k Yk​=yk​,我们需要利用观测去更新 f k − ( x ) f_k^-(x) fk−​(x)成 f k + ( x ) f_k^+(x) fk+​(x),其实就是后验概率 f k ( x ∣ y k ) f_k(x|y_k) fk​(x∣yk​)
f Y k ∣ X k ( y k ∣ x ) = lim ⁡ ε → 0 P ( y k < Y k < y k + ε ∣ X k = x ) ε = lim ⁡ ε → 0 P ( y k − h ( x ) < Y k − h ( X k ) < y k − h ( x ) + ε ∣ X k = x ) ε = lim ⁡ ε → 0 P ( y k − h ( x ) < R k < y k − h ( x ) + ε ∣ X k = x ) ε = lim ⁡ ε → 0 P ( y k − h ( x ) < R k < y k − h ( x ) + ε ) ε , 因 为 R k 和 X k 独 立 = f R k [ y k − h ( x ) ] 所 以 ， 有 ： f k + ( x ) = f k ( x ∣ y k ) = f Y k ∣ X k ( y k ∣ x ) ∗ f k − ( x ) f Y k ( y k ) = f R k [ y k − h ( x ) ] ∗ f k − ( x ) f Y k ( y k ) = η ∗ f R k [ y k − h ( x ) ] ∗ f k − ( x ) 其 中 ， η 为 ： η = { ∫ − ∞ + ∞ f R k ( [ y k − h ( x ) ] ∗ f k − ( x ) d x } − 1 \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​
 经过更新步之后，状态量的方差大大降低，也就是经过贝叶斯滤波后减少了噪声，得到更优更可靠的状态估计值.

三、贝叶斯滤波递推过程

f 0 ( x ) ⇒ 预 测 f 1 − ( x ) = ∫ − ∞ + ∞ f Q 1 [ x − f ( v ) ] f 0 ( v ) d v ⇒ 观 测 更 新 f 1 + ( x ) = η 1 ∗ f R 1 [ y 1 − h ( x ) ] ∗ f 1 − ( x ) ⇒ 预 测 f 2 − ( x ) = ∫ − ∞ + ∞ f Q 2 [ x − f ( v ) ] f 1 + ( x ) d v ⇒ 观 测 更 新 f 2 + ( x ) = η 2 ∗ f R 2 [ y 2 − h ( x ) ] ∗ f 2 − ( x ) … … ⇒ 预 测 f k − ( x ) = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f k − 1 + ( x ) d v ⇒ 观 测 更 新 f k + ( x ) = η k ∗ f R k [ y k − h ( x ) ] ∗ f k − ( x ) 其 中 ， η k = { ∫ − ∞ + ∞ f R k [ y k − h ( x ) ] ∗ f k − ( x ) d x } − 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,其实就是后面那一坨的积分的倒数。​
 公式中，除了初值即可认为是先验，又可以认为是后验，其他都需要经过预测和更新两个步骤，实验证明，初值即使很差，也不会影响后面时刻状态量的估计值。
 这里递推的都是概率密度函数，要求各时刻的状态估计，只需要求均值即可。
x ^ k = ∫ − ∞ + ∞ x ∗ f k + ( x ) d x \hat{x}_k=\int_{-\infty}^{+\infty}x*f_k^+(x)dx x^k​=∫−∞+∞​x∗fk+​(x)dx

四、贝叶斯滤波算法流程

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

(2) 预测步： f k − ( x ) = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] ∗ f k − 1 + ( v ) d v 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 ) = η k ∗ f R k [ y k − h ( x ) ] ∗ f k − ( x ) , 其 中 ， η k = { ∫ − ∞ + ∞ f R k [ y k − h ( x ) ] ∗ f k − ( x ) d x } − 1 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 ) = η ∗ P ( y k ∣ x k ) ∗ ∫ − ∞ + ∞ P ( x k ∣ x k − 1 ) ∗ P ( x k − 1 ) d x k − 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​