原来各种滤波都是贝叶斯滤波算法的实现哦~

作为解决毕业论文的主要算法，将贝叶斯滤波算法的所有实现算法，都仿真调试一下，并对比结果。

贝叶斯滤波三大概率

• 先验概率
• 似然概率
• 后验概率

---

离散情况下的贝叶斯滤波

全概率公式：\(P(T_m=10.3)=P(T_m=10.3|T=10)P(T=10)+P(T_m=10.3|T=11)P(T=11)\)

其中\(P(T_m=10.3|T=10)\)是似然概率（代表传感器精度），\(P(T=10)\)是先验概率（已经开始假设了），所以\(P(T_m=10.3)\)为常数。

\(P(T_m=10.3)\)与T的取值无关，仅与T的分布律有关。T = 10，T = 11代表随机试验的一个结果，结果不会影响到分布律

所以就可以改写贝叶斯公式：

\[P(状态(因)|观测(果))=\eta P(观测|状态)P(状态) \]

\[P(T=10|T_m=10.3)=\frac {P(T_m=10.3|T=10)P(T=10)}{P(T_m=10.3)}=\eta P(T_m=10.3|T=10)P(T=10) \]

\[P(T=11|T_m=10.3)=\frac {P(T_m=10.3|T=11)P(T=11)}{P(T_m=10.3)}=\eta P(T_m=10.3|T=11)P(T=11) \]

\[………… \]

即：

\[后验 = \eta * 似然 * 先验 \]

求\(\eta\)（归一化方法）：

\[\sum后=\eta \sum似 * 先，并且\sum后=1，所以\eta =\frac{1}{\sum似 *先} \]

连续情况下的贝叶斯滤波

\[离散：P(X=x|Y=y)=\frac{P(Y=y|X=x)P(X=x)}{P(Y=y)} \]

\[连续：P(X<x|Y=y)=\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)}=\eta f_{Y|X}(y|x)f_X(x) \]

求\(\eta\)（归一化方法）：

\[\eta = \frac {1}{\int_{-\infty}^x {f_{Y|X}(y|x)f_X(x)}dx} \]

似然概率与狄拉克函数

X：状态 Y：观测

重要定理：

若\(f_X(x)\rightarrow N(μ_1,\sigma^2)\)，\(f_{Y|X}(y|x)\rightarrow N(μ_2,\sigma_2^2)\)，则：

\[f_{X|y}(x|y)\rightarrow N(\frac {\sigma_1^2}{\sigma_1^2+\sigma_2^2}μ_2+\frac {\sigma_2^2}{\sigma_1^2+\sigma_2^2}μ_1,\frac {\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2}) \]

可继续推出：

\[若\sigma_1^2\gt\gt\sigma_2^2，后验\rightarrow N(μ_2,\sigma_2^2)，倾向于“观测值（似然）” \]

\[若\sigma_1^2\lt\lt\sigma_2^2，后验\rightarrow N(μ_1,\sigma_1^2)，倾向于“预测值（先验）” \]

随机过程的贝叶斯滤波

1~5讲，X先验，Y观测，仅有一个X，一个观测Y

随机过程\(X_0\rightarrow X_1 \rightarrow……\rightarrow X_k\)有一个初值\(X_0\)，有k个观测值\(y_1,y_2,……,y_k\)，怎么办？

1. 所有的\(X_0, ……, X_k\)的先验概率都靠猜

   • 缺点：过于依赖观测（似然），放弃了预测（先验）信息

     例如：\(X_k=2X_{k-1}+Q_k\) \(X_k=X_{k-1}^2+Q_k\)

2. 只有\(X_0\)的概率是猜的，\(X_1,……,X_k\)得先验概率是递推的

怎么做？

1. 马尔科夫假设，观测独立假设
2. 状态方程，观测方程（建模）

分两步：

1. 预测步：上一时刻的后验\(\rightarrow\)这一时刻的先验（通过状态方程得到）
2. 更新步：这一时刻的先验\(\rightarrow\)这一时刻的后验/下一时刻的先验

即：\(X_0\rightarrow X_1^-\rightarrow X_1^+\rightarrow X_2^- \rightarrow X_2^+……\)

贝叶斯滤波很大的缺点：从\(f_{k-1}^-\)到\(f_k^-\)，算\(\eta\)、期望都要进行无穷积分，大多数情况下无法得到解析解。

解决：

1. 作假设
   • \(f(X_{k-1})、h(X_k)\)为线性，\(Q_k,R_k\)为正态高斯分布（卡尔曼滤波）
   • \(f(X_{k-1})、h(X_k)\)为非线性，\(Q_k,R_k\)为正态高斯分布（扩展卡尔曼滤波，即EKF、UKF等）
2. 霸王硬上弓，直接对无穷积分作数值积分
   • 高斯积分（不常用）
   • 蒙特卡洛积分（粒子滤波Particle Filter）
   • 直方图滤波

贝叶斯滤波算法

原料：

1. \(X_k=f(X_{k-1})+Q_k\)

2. \(Y_k=h(X_k)+R_k\)

   其中：\(X_k、X_{k-1}、Y_k、Q_k、R_k\)都是随机变量

3. 假设：\(X_0，Q_1,……,Q_k,R_1,……,R_k\)相互独立

4. 有观测值：\(y_1,y_2,……,y_k\)，设初值\(X_0->f_0(k),Q_k->f_{Q_k}(x),R_k->f_{R_k}(x)\)

5. 重要定理：条件概率里的条件可做逻辑推导

计算步骤：

初值： \(X_0\rightarrow f_0^+(x)\)

预测：\(f_k^-(x)= \int_{-\infty} ^{+\infty} {f_Q[x-f(v)]f_{k-1}^+(v)}dv\)

更新：\(f_k^+(x)=\eta f_R[y_k-h(x)]f_k^-(x)\)

其中：\(\eta = (\int_{-\infty}^{+\infty}f_R[y_k-h(x)])^{-1}\)

估计：\(\hat x_k^+= \int_{-\infty}^{+\infty}xf_x^+(x)dx\)

贝叶斯滤波的实现之卡尔曼滤波

1. Filter问题：请用计算机生成一个含正态噪声的信号，并用Kalman Filter滤波
2. Sensor Fusion传感器融合：

使用matlab、C、C++、python

tips：

1. 使用矩阵形式而不是一阶
2. F、H可以不是方阵，阶数也可以不相同
3. 泰勒展开

贝叶斯滤波的实现之粒子滤波

应用广泛，原理最复杂，术语最多

适用环境：静态环境，动态可预测环境 。如：电池电量估算，视频跟踪，封闭环境导航（激光雷达+pf slam）

缺点：无穷积分，一般无解析解

粒子滤波完整算法：

1. 给初值\(X_0\rightarrow N(μ,\sigma^2)\)；
2. 生成\(x_0^{(i)},w_0^{(i)}= \frac1n\)；
3. 预测步，生成\(x_1^{(i)}=f(x_0^{(i)}+v,v\)为\(N(0,Q)\)的随机数，共\(n\)个；
4. 更新步，设观测值为\(y_1\)，生成\(w_1^{(i)}=f_R[y_1-h(x_1^{(i)})]*w_0^{(i)}\)；
5. 将\(w_1^{(i)}\)归一化，\(w_1^{(i)}=\frac {w_1^{(i)}}{\sum w_1^{(i)}}\)；
6. 此时，得到新的粒子\(x_1^{(i)}\)，新的权重\(w_1^{(i)}\)；
7. 再由预测步生成\(x_2^{(i)}=f(x_1^{(i)})+v\)；
8. 再由更新步生成\(w_2^{(i)}=f_R[y_2-h(x_2^{(i)})]w_1^{(i)}\)，归一化；
9. 如此递推……

粒子滤波总结：

1. 由大数定律，暗示了\(pdf\)可由带权重（简单地理解为平均值）的粒子表示；
2. 粒子数量，粒子位置，粒子权重完全决定了\(cdf\)（概率分布函数），同时即决定了\(pdf\)（概率密度函数）；
3. 预测步改变粒子位置；
4. 更新步更新粒子权重。

粒子滤波之重采样

1. 重采样有一定的减弱粒子退化的能力；
2. 重采样必然会导致粒子多样性丧失（概率大的粒子被复制的机会越大，概率小的粒子可能被淘汰）；
3. 重采样必然会减慢particle filter的速度

贝叶斯滤波的实现之扩展卡尔曼滤波（EKF）

贝叶斯滤波的实现之无迹卡尔曼滤波（UKF）