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

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

贝叶斯滤波三大概率

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

离散情况下的贝叶斯滤波

全概率公式：(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}^xfrac {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) ightarrow N(μ_1,sigma^2))，(f_{Y|X}(y|x) ightarrow N(μ_2,sigma_2^2))，则：

[f_{X|y}(x|y) ightarrow 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^2sigma_2^2}{sigma_1^2+sigma_2^2}) ]

可继续推出：

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

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

随机过程的贝叶斯滤波

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

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

所有的(X_0, ……, X_k)的先验概率都靠猜
- 缺点：过于依赖观测（似然），放弃了预测（先验）信息
  
  例如：(X_k=2X_{k-1}+Q_k) (X_k=X_{k-1}^2+Q_k)
只有(X_0)的概率是猜的，(X_1,……,X_k)得先验概率是递推的

怎么做？

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

分两步：

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

即：(X_0 ightarrow X_1^- ightarrow X_1^+ ightarrow X_2^- ightarrow X_2^+……)

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

解决：

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

贝叶斯滤波算法

原料：

(X_k=f(X_{k-1})+Q_k)
(Y_k=h(X_k)+R_k)

其中：(X_k、X_{k-1}、Y_k、Q_k、R_k)都是随机变量
假设：(X_0，Q_1,……,Q_k,R_1,……,R_k)相互独立
有观测值：(y_1,y_2,……,y_k)，设初值(X_0->f_0(k),Q_k->f_{Q_k}(x),R_k->f_{R_k}(x))
重要定理：条件概率里的条件可做逻辑推导

计算步骤：

初值： (X_0 ightarrow 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)

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

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

使用matlab、C、C++、python

tips：

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

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

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

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

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

粒子滤波完整算法：

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

粒子滤波总结：

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

粒子滤波之重采样

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