原來各種濾波都是貝葉斯濾波算法的實現哦~

作為解決畢業論文的主要算法，將貝葉斯濾波算法的所有實現算法，都仿真調試一下，並對比結果。

貝葉斯濾波三大概率

• 先驗概率
• 似然概率
• 后驗概率

---

離散情況下的貝葉斯濾波

全概率公式：\(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）