1. 說明
本文是來自忠厚老實的老王在B站講的卡爾曼濾波，經過自己理解寫的總結筆記，課講的非常好，一定要去聽
2. 貝葉斯公式和應用
對於事件A和B，設其同時發生的概率為\(P(A =a \bigcap B =b)\)， 則存在：
\[P(A =a \bigcap B = b)=P(A=a|B=b) * P(B=b) = P(B=b|A=a) * P(A=a) \]
這是數學本質，A和B同時發生的概率為發生B的概率和在發生B時發生A的概率的乘積，很好理解。
\[P(A=a|B=b) = \frac{P(B=b|A=a) * P(A=a)}{P(B=b)} \]
經過變形可以得到，這就貝葉斯公式，
貝葉斯公式本質只是條件概率，如何基於這個公式對其進行應用呢？
比如說測量現實溫度，我們預設一個概率空間A對現實進行預測，我們當然可以通過加權積分的方式得到一個最終的預測值，但這個預設的概率空間的准確性很一般，這個時候，找到了一個它的影子A'，他們之間相關，而且狀態空間A‘在現實中能夠直接觀測到一個結果a'，也就說明在這個現實下A’是朝着a'進行塌縮的，那么相對應的A會朝着相似的方向塌縮成a，所以只需要知道A， A‘， a'，還有A與A’之間的聯系，就能夠確定a了
有兩個溫度計A和B，要測量今天的溫度，測得如下結果:
\(P(A=10) = 0.8, P(A=11) = 0.2\)
\(P(B=10) = 0.7, P(B=11) = 0.2\)
而且溫度計B在出廠的時候測的和A的相關關系為:
\(P(B=11|A=10) = 0.3\)
\(P(B=10|A=10) = 0.6\)
現在，我們想要結合兩個測量結果讓結果更准確，讓B去修正A，根據貝葉斯公式:
\(P(A = 10|B = 10) = \frac{P(B = 10|A = 10) * P(A = 10)}{P(B = 10)} = \frac{0.6 * 0.8}{0.7} = 0.685\)
當B=10的時候也就是說在一次觀測中B這個系統朝着B=10這個方向塌縮了, 此時A也跟着塌縮P(A=10 | B= 10) =0.685, 和最初的\(P(A=10)=0.8\)相比就進一步收斂了，當然這里還遺留了一個問題：B=11的時候呢？這就屬於另一個問題，多次觀測的塌縮融合問題，后文另提
-
如何理解似然
\(P_{A|B}(a|b) = \frac{P_{B|A}(b|a) * P_{A}(a)}{P_{B}(b)}\) 通常為 \(后驗估計 = \frac{似然 * 先驗估計}{概率}\) ，
其中\(P_{B|A}(b| a)\)叫做似然概率，表示事件a事件b發生的概率，這個值是一個固定值，只是表示的是A和影子狀態B之前的相關性僅此而已，和\(P_{A}(a)\)大小無關，不會隨着猜測\(P_{A}(a)\)的改變而改變，是一個預設值，表示兩個系統的相關性。
3. 連續貝葉斯公式
在連續的概率分布中，只有單點概率密度而沒有單點的概率，單點概率為0，但還是要計算概率為此要使用微分的思想化積為加，對於一個連續的貝葉斯概率\(P(X<x | Y =y)\)推導推導過程如下:
\[\begin{align*} P(X<x | Y =y) &= \sum_{u=-\infty}^{x}P(X=u | Y=y) \hspace{1.2cm}化連續為累加\\ &= \sum_{u=-\infty}^{x}\frac{P(Y=y |X=u)P(X=u)}{P(Y=y)} \hspace{1.2cm}使用離散貝葉斯公式\\ &= \lim_{\varepsilon \to \infty} \sum_{u=-\infty}^{x}\frac{P(y<Y<y+\varepsilon |X=u)P(u<X<u + \varepsilon)}{P(y<Y<y+\varepsilon)} \hspace{1.2cm}單點概率為0，用無窮小量表征連續性 \\ &=\lim_{\varepsilon \to \infty} \sum_{u=-\infty}^{x}\frac{(f_{Y|X}(\xi_1|u)\times\varepsilon)(f_X(\xi_2)\times \varepsilon)}{f_Y(\xi_3)\times \varepsilon} \hspace{1.2cm} 其中 \xi_1 \in(y, y+\varepsilon), \xi_2 \in(u, u+\varepsilon), \xi_3 \in(y, y+\varepsilon) \\ &=\lim_{\varepsilon \to \infty} \sum_{u=-\infty}^{x}\frac{(f_{Y|X}(y|u))(f_X(u))}{f_Y(y)}\times \varepsilon \hspace{1.2cm} 使用積分中值定理\\ &=\int_{-\infty}^{x} \frac{(f_{Y|X}(y|u))(f_X(u))}{f_Y(y)}d(u) =>\int_{-\infty}^{x} \frac{(f_{Y|X}(y|x))(f_X(x))}{f_Y(y)}d(x) \hspace{1.2cm}(連續貝葉斯) \end{align*} \]
最后的形式和離散貝葉斯很像，但是卻變成了概率密度的相乘，多了個積分符號的dx, 其實很好理解，從推導的過程中可以知道，使用【概率密度*dx】代替【概率】后，分子分母約掉了一個dx，同時把整個x域的概率加起來
因為\(f_{Y}(y)\)可以通過全概率密度的方式計算出來，
\[f_{Y}(y) = \int_{x= -\infty}^{x=+\infty} f_{Y|X}(y|x)f_X(x)dx \]
所以連續貝葉斯公式還有另一個形式
\[P(X<x | Y =y) = \int_{-\infty}^{x} \eta(f_{Y|X}(y|x))(f_X(x))d(x) \\ 其中\eta = \frac{1}{f_{Y}(y)} = \frac{1}{\int_{-\infty}^{+\infty} f_{Y|X}(y|x)f_X(x)dx} \]
4. 貝葉斯濾波
在推導了連續的貝葉斯公式后，接下來就能夠推導貝葉斯濾波算法，貝葉斯濾波基於貝葉斯概率的思想，首先對觀測的建模得到預測方程，就能夠基於前一個狀態對下一個狀態進行預測，同時對下一個狀態進行觀測得到預測方程，最終將兩者融合后就能夠得到一個比較准確的后驗，整個過程如下所示:
對於貝葉斯濾波算法的開始需要預測方程和觀測方程
**預測方程：$$X_{k} = f(X_{k-1}) + Q_{K} $$, **
**觀測方程： \(Y_k = h(X_{K}) + R_{k}\) **
\(X_{k-1}:\) K-1時刻狀態X的實際值
\(X_{k}:\) K時刻狀態X的預測值
\(f:\) 前一狀態和當前時刻的關系
\(h:\) 實際值和觀測值的關系
\(Q_{K}:\) k時刻預測隨機噪聲
\(R_k:\) k時刻的觀測噪聲
\(Y_k:\) k時刻的觀測值
其中： \(X_0, Q_1,Q_2...Q_K, R_1, R_2....R_k\) 相互獨立
並且有觀測值： \(y_1, y_2, ... , y_n\)
設\(X_0\)以及X的概率密度函數\(f_{x0}(x)\), \(Q_K\)的概率密度函數\(f_{Q_k}(x)\)， \(R_K\)的概率密度函數\(f_{R_k}(x)\)
注意，此處的預測方程和觀測方程中變量都是\(X_k, Y_k\)而不是\(x_k, y_k\), 表示的還是一個范圍下的隨機變量；
預測方程使用\(f\)做$ X_{k}\(的隨機過程對系統進行模擬建模，同時用噪聲\)Q_{K}$去1.彌補建模的不准確。 2.模擬實際中存在的噪聲
觀測方程是在預測方程的基礎上，使用\(h\)對預測出來的系統狀態\(X_{k}\)做從系統狀態到狀態的轉換，用\(R_{k}\)去模擬觀測噪聲
這個時候再回到貝葉斯思想的本身去看這兩個方程，為了得到系統狀態，利用隨機過程對系統進行建模得到預測方程A，由於這個模型是不准的，為此找了與A有聯系的影子A’，A‘能在現實中坍縮成a’，所以A也會朝着這個方向塌縮成a；\(X_{k}\)就是A， 而\(Y_{k}\)就是A’，要謹記它們不是一個具體的值，而是概率空間下的隨機變量，\(R_{K}\)和\(Q_{K}\)是讓兩個系統成為概率空間的原因；此外將\(X_{K}代入Y_{k}可以得到Y_{k} = h(f(x_{k-1} + R_{k}) + Q_{K})\) , 提醒我們\(Y_{k}與X_{K}的關聯性經過了兩個隨機噪音R_k和Q_K,所以成了概率關系\)
-
推導過程
\[目標式: f_{X}^{+} (x) = \eta(f_{Y|X}(y|x))f_{X}^{-}(x) \\ \eta = \frac{1}{f_{Y}(y)} = \frac{1}{\int_{-\infty}^{+\infty} f_{Y|X}(y|x)f_X(x)dx} \]
\(f_{X}^{+}(x)\): x的后驗概率密度也就是\(f_{X|Y}(x|y)\)的簡略寫法
\(f_{X}^{-}(x)\): x的先驗概率密度，從觀測方程得出
y: 觀測值y
此處的目標是只需要求出\(f_{X}^{+}(x)\)即可，有了概率密度函數后，后驗x值使用積分\(x_{}^{+} = \int_{-\infty}^{+\infty}xf_{X}^{+}(x)dx\)計算可得
先驗\(f_{X}^{-}(x)\)的推導，概率是概率密度的積分，要求概率密度，對概率求導即可
\[P(X_{1}<x)=\sum_{u = -\infty}^{x}P(X_{1} = u) \\ \begin{align*} P(X_{1}=u)&=\sum_{v=-\infty}^{+\infty}P(X_{1}=u| X_{0}=v)P(X_0=v)(全概率)\\ &=\sum_{v=-\infty}^{+\infty}P(X_1 - f(X_0) = u -f(v)|X_0=v)P(X_0=v)\\ &=\sum_{v=-\infty}^{+\infty}P(Q_1 = u -f(v)|X_0=v)P(X_0=v)\\ &=\sum_{v=-\infty}^{+\infty}P(Q_1 = u -f(v))P(X_0=v) \hspace{1cm} Q_1與上一輪的X值X_0獨立\\ &=\lim_{\varepsilon \to 0} \sum_{v=-\infty}^{+\infty}f_{Q_1}(u -f(v))\cdot \varepsilon \cdot f_{X_0}(v) \cdot \varepsilon \hspace{1cm} 化概率為概率密度\\ &=\lim_{\varepsilon \to 0} \int_{v=-\infty}^{+\infty}f_{Q_1}(u -f(v))f_{X_0}(v)d(v) \cdot \varepsilon \hspace{1cm} 將\varepsilon等效為微分d(v)\\ \end{align*} \]
\[\begin{align*} \therefore P(X_{1}<x)&=\sum_{u = -\infty}^{x}P(X_{1} = u) \hspace{11cm}\\ &= \sum_{u=-\infty}^{+\infty}{}\lim_{\varepsilon \to 0} \int_{v=-\infty}^{+\infty}f_{Q_1}(u -f(v))f_{X_0}(v)d(v) \cdot \varepsilon\\ &= \int_{-\infty}^{x} \int_{v=-\infty}^{+\infty}f_{Q_1}(u -f(v))f_{X_0}(v)d(v)d(u)\\ \end{align*} \]
\[\therefore f_{X_1}^{-}(x) = \frac{d(P(X_1 < x))}{dx} = \int_{v=-\infty}^{+\infty}f_{Q_1}(u -f(v))f_{X_0}(v)d(v) \hspace{2cm}(變限積分求導) \]
似然\(f_{Y|X}(y|x)\)的推導, 思路也是一樣，對概率取一個微積分空間然后求導
\[\begin{align*} f_{Y_1|X_1}(y_1|x_1) &= \lim_{\varepsilon \to 0} \frac{P(y_1 < Y_1 <y_1 + \varepsilon | X_1 = x)}{\varepsilon}\\ &=\lim_{\varepsilon \to 0} \frac{P(y_1 -h(x)< Y_1 -h(X1) <y_1-h(x) + \varepsilon | X_1 = x)}{\varepsilon}\\ &=\lim_{\varepsilon \to 0} \frac{P(y_1 -h(x)< R_1 <y_1-h(x) + \varepsilon | X_1 = x)}{\varepsilon}\\ &=\lim_{\varepsilon \to 0} \frac{P(y_1 -h(x)< R_1 <y_1-h(x) + \varepsilon)}{\varepsilon} \hspace{2cm} 觀測噪聲R_1與X_1獨立\\ &= f_{R_1}(y_1 - h(x)) \end{align*} \]
最后后驗概率的值為:
\[f_{1}^{+}(x) =\eta_{1} \cdot f_{R_1}[y_1 - h(x)] \cdot f_{X_1}^{-}(x) \\ \eta_{1} = \frac{1}{f_{Y_1}(y)} = \frac{1}{\int_{-\infty}^{+\infty} f_{R_1}[y_1 - h(x)]f_{X_1}^{-}(x)dx} \\ \]
5. 卡爾曼濾波
**預測方程：$$X_{k} = F(X_{k-1}) + Q_{K} $$, **
**觀測方程： \(Y_k = H(X_{K}) + R_{k}\) **
由於貝葉斯濾波的每一步推導都有無窮積分，而無窮積分解析解一般不存在導致貝葉斯濾波難以落地，為此做了兩個：
- f和h都假設為線性關系
- \(Q_k,R_k\)假設為正態噪聲服從$Q_k服從N(0,Q)， R_K服從N(0,R) $ ，這就是卡爾曼濾波
假設\(X_{K-1} 服從 N(u_{k-1}^{+}, \sigma_{k-1}^{+})\)，先驗\(f_{K}^{-}(x)\)的計算如下
\[\begin{align*} f_{X_k}^{-}(x) &= \int_{-\infty}^{+\infty}f_{Q}[x-f(v)]f_{X_{k-1}}^{+}(v)dv \hspace{3cm} (1)\\ &= \int_{-\infty}^{+\infty}(2\pi Q)^{-\frac{1}{2}} \cdot e^{-\frac{(x-Fv)^2}{2Q}} \cdot (2\pi \sigma_{k_{j-1}}^{+})^{-\frac{1}{2}} \cdot e^{\frac{(v - u_{k-1}^{+})^2}{2\sigma_{k-1}^{+}}} \cdot dv \end{align*} \]
這個積分想要進一步化簡並不容易，但仔細觀察(1)參考此文可以發現這實質是\(f_{Q}和f_{X_{k-1}}^{-}\)卷積的過程，時域的卷積就等於頻域的乘積，可以通過傅里葉變化計算完后逆變換回來，最后計算得
\[f_{X_k}^{-}(x) \sim N(u_{k}^{-}, \sigma_{k}^{-}) \\ u_{k}^{-} = F \cdot u_{k-1}^{+},\hspace{1cm} \sigma_{k}^{-}=F^2 \sigma_{k-1}{+}Q \]
對於后驗\(f_{X_k}^{+}\)
\[\begin{align*} f_{X_k}^{+} &= \eta f_{R}(y_{k} - h \cdot x) \cdot f_{x_k}^{-}(x)\\ &= \eta (2 \pi R)^{-\frac{1}{2}} \cdot e^{\frac{(y_k-hx)^2}{2R}} \cdot (2\pi \sigma_{k}^{-})^{-\frac{1}{2}} \cdot e^{\frac{(x-u_{k}^{-})^2}{2\sigma_{k}^{-}}} \\ \end{align*} \\ \eta=\frac{1}{\int_{-\infty}^{+\infty}(2 \pi R)^{-\frac{1}{2}} \cdot e^{\frac{(y_k-hx)^2}{2R}} \cdot (2\pi \sigma_{k}^{-})^{-\frac{1}{2}} \cdot e^{\frac{(x-u_{k}^{-})^2}{2\sigma_{k}^{-}}}} \\ \]
最后計算得到
\[最后計算得到 X_{k}^{+} \sim N(u_{k}^{+}, \sigma_{k}^{+}) \\ 其中 u_{k}^{+} = \frac{h\sigma_{k}^{-}y_{k} + Ru_{k}^{-}}{h^2\sigma_{k}^{-} + R}, \sigma_{k}^{+} = \frac{R\sigma_{k}^{-}}{h^2\sigma_{k}^{-}+R}\\ => X_{k}^{+} \sim N(\frac{h\sigma_{k}^{-}y_{k} + Ru_{k}^{-}}{h^2\sigma_{k}^{-} + R}, \frac{R\sigma_{k}^{-}}{h^2\sigma_{k}^{-}+R}) \]
令\(k=\frac{h\sigma_{k}^{-}}{h^2\sigma_{k}^{-} + R}\), 則
\[u_{k}^{+} = u_{k}^{-} +k*(y_{k} - hu_{k}^{-})\\ \sigma_{k}^{+}=(1-kh)\sigma_{k}^{-} \]
最終所有公式為
\[先驗期望:u_{k}^{-} = F \cdot u_{k-1}^{+} \\ 先驗方差:\sigma_{k}^{-}=F^2 \sigma_{k-1}{+}Q\\ 后驗期望:u_{k}^{+} = u_{k}^{-} +k*(y_{k} - hu_{k}^{-})\\ 后驗方差:\sigma_{k}^{+}=(1-kh)\sigma_{k}^{-}\\ 其中：k=\frac{h\sigma_{k}^{-}}{h^2\sigma_{k}^{-} + R} \]
會發現結果已經沒有別的概率，只是期望和方差的加加減減，這是因為高斯函數的運算具有封閉性。
6. 矩陣形式的卡爾曼濾波
期望\(u_{k}^{-}\)變成了向量$\vec{u_{k}^{-}} \(, 方差\)\sigma_{k}\(變成了協方差矩陣\)\Sigma_{k}$，關於正太分布中為什么方差變成了協方差矩陣參考多維高斯分布：
其中要特別注意，構建協方差矩陣時，不是用\(X^2\Sigma\),而是用\(X^T\Sigma X\)這樣的形式，因為這種形式算下來最后是一個1x1的值；
矩陣形式的卡爾曼濾波如下：小寫的\(\sigma，h\)變成了矩陣形式大寫的\(\Sigma，H\), 1變成了單位矩陣\(I\)
\[\vec{u_{k}^{-}} = F \cdot \vec{u_{k-1}^{+}}\\ {\Sigma_{k}^{-}}=F {\Sigma_{k-1}^{+}}F^T+Q\\ \vec{u_{k}^{+}} = \vec{u_{k}^{-}} + k*(\vec{y_{k}} - H\vec{u_{k}^{-}})\\ {\Sigma_{k}^{+}}= {(I-kH)\Sigma_{k}^{-}}\\ 其中: k=\frac{H{\Sigma_{k}^{-}}}{H\Sigma_{k}^{-}H^T + R} \]
7. 應用
用來做系統預測的時候，一定會首先建模
**預測方程：$$X_{k} = F(X_{k-1}) + Q_{K} $$, **
**觀測方程： \(Y_k = H(X_{K}) + R_{k}\) **
其中有一些要點，
1是F會導致預測模型是否能夠擬合實際, 建模可以傻瓜式建模\(X_{k} = X_{k-1} + Q_{K}\), 使用\(Q_K\)去做修正，但會不准確
2.是Q和R，觀測值yk的出現后會R的大小會決定\(Y_k\)的塌縮程度，如果\(R_k\)小說明\(Y_k\)這個系統值很集中塌縮值要求很精確，所以要求輸入得\(X_k\)也要塌縮的精確，這時候候\(x_{k}^{+}\)就更靠近 \(y_k\)，所以說我們更相信觀測值；相反\(R_K\)大說明\(Y_k\)系統值塌縮得不准確，那么\(X_k\)塌縮得范圍也大一些就會靠近自己得先驗均值，這個時候就說$x_{k}^{+} $更靠近預測值
3.是預測值\(X_0\)的初值隨便設置沒關系的原因，X0設置的粗糙只是說系統一開始粗糙，但觀測方程的塌縮，會讓結果塌縮到正確的點，初值影響並不太大。
