卡爾曼濾波原理詳解
1. 什麼是卡爾曼濾波
你可以在任何含有不確定資訊的動態系統中使用卡爾曼濾波，對系統下一步的走向做出有根據的預測，即使伴隨著各種干擾，卡爾曼濾波總是能指出真實發生的情況。
在連續變化的系統中使用卡爾曼濾波是非常理想的，它具有佔用記憶體小的優點（除了前一個狀態量外，不需要保留其它歷史資料），並且速度很快，很適合應用於實時問題和嵌入式系統。
2. 卡爾曼濾波可以做什麼
用玩具舉例：你開發了一個可以在樹林裡到處跑的小機器人，這個機器人需要知道它所在的確切位置才能導航。
我們可以說機器人有一個狀態\(\vec{x_k}\)，表示位置和速度：
\[\vec{x_k}=(\vec{p},\ \vec{v}) \]注意這個狀態只是關於這個系統基本屬性的一堆數字，它可以是任何其它的東西。在這個例子中是位置和速度，它也可以是一個容器中液體的總量，汽車發動機的溫度，使用者手指在觸控板上的位置座標，或者任何你需要跟蹤的訊號。
這個機器人帶有GPS，精度大約為10米，還算不錯，但是，它需要將自己的位置精確到10米以內。樹林裡有很多溝壑和懸崖，如果機器人走錯了一步，就有可能掉下懸崖，所以只有GPS是不夠的。
或許我們知道一些機器人如何運動的資訊：例如，機器人知道傳送給電機的指令，知道自己是否在朝一個方向移動並且沒有人干預，在下一個狀態，機器人很可能朝著相同的方向移動。當然，機器人對自己的運動是一無所知的：它可能受到風吹的影響，輪子方向偏了一點，或者遇到不平的地面而翻倒。所以，輪子轉過的長度並不能精確表示機器人實際行走的距離，預測也不是很完美。
GPS 感測器告訴了我們一些狀態資訊，我們的預測告訴了我們機器人會怎樣運動，但都只是間接的，並且伴隨著一些不確定和不準確性。但是，如果使用所有對我們可用的資訊，我們能得到一個比任何依據自身估計更好的結果嗎？回答當然是YES，這就是卡爾曼濾波的用處。
3. 卡爾曼濾波是如何看到你的問題的
下面我們繼續以只有位置和速度這兩個狀態的簡單例子做解釋。
\[\vec{x}= \begin{bmatrix} p\\ v \end{bmatrix} \]卡爾曼濾波假設兩個變數（位置和速度，在這個例子中）都是隨機的，並且服從高斯分佈。每個變數都有一個均值\(\mu\)，表示隨機分佈的中心（最可能的狀態），以及方差\(\sigma^2\)，表示不確定性。
在上圖中，位置和速度是不相關的，這意味著由其中一個變數的狀態無法推測出另一個變數可能的值。
下面的例子更有趣：位置和速度是相關的，觀測特定位置的可能性取決於當前的速度：
這種情況是有可能發生的，例如，我們基於舊的位置來估計新位置。如果速度過高，我們可能已經移動很遠了。如果緩慢移動，則距離不會很遠。跟蹤這種關係是非常重要的，因為它帶給我們更多的資訊
這種相關性用協方差矩陣來表示，簡而言之，矩陣中的每個元素 \(\Sigma_{ij}\) 表示第 \(i\) 個和第 \(j\) 個狀態變數之間的相關度。（你可能已經猜到協方差矩陣是一個對稱矩陣，這意味著可以任意交換 \(i\) 和 \(j\)）。協方差矩陣通常用 \(\Sigma\) 來表示，其中的元素則表示為 \(\Sigma_{ij}\)。
4. 使用矩陣來描述問題
我們基於高斯分佈來建立狀態變數，所以在時刻 \(k\) 需要兩個資訊：最佳估計 \(\hat{x_k}\)（即均值，其它地方常用 \(\mu\) 表示），以及協方差矩陣 \(P_k\) 。
\[\begin{equation} \hat{x_k}= \begin{bmatrix} position \\ velocity \end{bmatrix} , \ \ P_k= \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv}\\ \Sigma_{vp} & \Sigma_{vv} \end{bmatrix} \tag{1} \end{equation} \]（當然，在這裡我們只用到了位置和速度，實際上這個狀態可以包含多個變數，代表任何你想表示的資訊）。
接下來，我們需要根據當前狀態（k-1時刻）來預測下一狀態（k 時刻）。記住，我們並不知道對下一狀態的所有預測中哪個是“真實”的，但我們的預測函式並不在乎。它對所有的可能性進行預測，並給出新的高斯分佈。
我們可以用矩陣 \(F_k\) 來表示這個預測過程：
它將我們原始估計中的每個點都移動到了一個新的預測位置，如果原始估計是正確的話，這個新的預測位置就是系統下一步會移動到的位置。那我們又如何用矩陣來預測下一個時刻的位置和速度呢？下面用一個基本的運動學公式來表示：
\[\begin{align} \textcolor{magenta}{p_k}=\textcolor{blue}{p_{k-1}}+\Delta t \textcolor{blue}{v_{k-1}} \\ \textcolor{magenta}{v_k}=\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \textcolor{blue}{v_{k-1}} \end{align} \]用矩陣表示為：
\[\begin{align} \textcolor{magenta}{\hat{x_k}}= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \textcolor{blue}{\hat{x}_{k-1}} \tag{2}\\ =F_x\textcolor{blue}{\hat{x}_{k-1}}\ \ \ \ \ \ \ \ \ \ \ \ \tag{3} \end{align} \]現在，我們有了一個預測矩陣來表示下一時刻的狀態，但是，我們仍然不知道怎麼更新協方差矩陣。此時，我們需要引入另一個公式，如果我們將分佈中的每個點都乘以矩陣 \(\textcolor{#b22222}{A}\)，那麼它的協方差矩陣 \(\Sigma\) 會怎樣變化呢？很簡單，下面給出公式：
\[\begin{align} \begin{matrix} Cov(x)=\Sigma \\ Cov(\textcolor{#b22222}{A}x)=\textcolor{#b22222}{A}\Sigma\textcolor{#b22222}{A}^T \end{matrix} \tag{4} \end{align} \]結合方程 \((3)\) 和 \((4)\) 得到：
\[\begin{matrix} \textcolor{magenta}{\hat{x}_k}=F_k\textcolor{blue}{\hat{x}_{k-1}} \\ \textcolor{magenta}{P_k}= Cov(F_k\textcolor{blue}{\hat{x}_{k-1}}) =F_k\textcolor{blue}{P_{k-1}}F_k^T \end{matrix} \tag{5} \]5. 外部控制量
我們並沒有捕捉到一切資訊，可能存在外部因素會對系統進行控制，帶來一些與系統自身狀態沒有相關性的改變。
以火車的運動狀態模型為例，火車司機可能會操縱油門，讓火車加速。相同地，在我們機器人這個例子中，導航軟體可能會發出一個指令讓輪子轉向或者停止。如果知道這些額外的資訊，我們可以用一個向量 \(\textcolor{orange}{\vec{u_k}}\) 來表示，將它加到我們的預測方程中做修正。
假設由於油門的設定或控制命令，我們知道了期望的加速度 \(\textcolor{orange}{a}\) ，根據基本的運動學方程可以得到：
\[\begin{align} \textcolor{magenta}{p_k}=\textcolor{blue}{p_{k-1}}+\Delta t \textcolor{blue}{v_{k-1}}+ \frac{1}{2}\textcolor{orange}{a}\Delta t^2\\ \textcolor{magenta}{v_k}=\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \textcolor{blue}{v_{k-1}}+ \textcolor{orange}{a}\Delta t \ \ \ \ \ \end{align} \]以矩陣的形式表示就是：
\[\begin{align} \textcolor{magenta}{\hat{x_k}}=F_x\textcolor{blue}{\hat{x}_{k-1}}+ \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix}\textcolor{orange}{a} \\ =F_x\textcolor{blue}{\hat{x}_{k-1}}+B_k\textcolor{orange}{\vec{u_k}} \ \ \ \ \ \ \end{align} \]\(B_k\) 稱為控制矩陣，\(\textcolor{orange}{\vec{u_k}}\) 稱為控制向量（對於沒有外部控制的簡單系統來說，這部分可以忽略）。讓我們再思考一下，如果我們的預測並不是100%準確的，該怎麼辦呢？
6. 外部干擾
如果這些狀態量是基於系統自身的屬性或者已知的外部控制作用來變化的，則不會出現什麼問題。
但是，如果存在未知的干擾呢？例如，假設我們跟蹤一個四旋翼飛行器，它可能會受到風的干擾，如果我們跟蹤一個輪式機器人，輪子可能會打滑，或者路面上的小坡會讓它減速。這樣的話我們就不能繼續對這些狀態進行跟蹤，如果沒有把這些外部干擾考慮在內，我們的預測就會出現偏差。
在每次預測之後，我們可以新增一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型：
原始估計中的每個狀態變數更新到新的狀態後，仍然服從高斯分佈。我們可以說 \(\textcolor{blue}{\hat{x}_{k-1}}\) 的每個狀態變數移動到了一個新的服從高斯分佈的區域，協方差為 \(\textcolor{green}{Q_k}\) 。換句話說就是，我們將這些沒有被跟蹤的干擾當作協方差為 \(\textcolor{green}{Q_k}\) 的噪聲來處理。
這產生了具有不同協方差（但是具有相同的均值）的新的高斯分佈。
我們通過簡單地新增 \(\textcolor{green}{Q_k}\) 得到擴充套件的協方差，下面給出預測步驟的完整表示式：
\[\begin{align} \begin{matrix} \textcolor{magenta}{\hat{x_k}}=F_x\textcolor{blue}{\hat{x}_{k-1}}+B_k\textcolor{orange}{\vec{u_k}}\ \\ \textcolor{magenta}{P_k}=F_k\textcolor{blue}{P_{k-1}}F_k^T+\textcolor{green}{Q_k} \end{matrix} \tag{7} \end{align} \]由上式可知，新的最優估計是根據上一最優估計預測得到的，並加上外部控制量的修正。
而新的不確定性由上一不確定性預測得到，並加上外部環境的干擾。
好了，我們對系統可能的動向有了一個模糊的估計，用 \(\textcolor{magenta}{\hat{x_k}}\) 和來 \(\textcolor{magenta}{P_k}\) 表示。如果再結合感測器的資料會怎樣呢？
7. 用測量值來修正估計值
我們可能會有多個感測器來測量系統當前的狀態，哪個感測器具體測量的是哪個狀態變數並不重要，也許一個是測量位置，一個是測量速度，每個感測器間接地告訴了我們一些狀態資訊。
注意，感測器讀取的資料的單位和尺度有可能與我們要跟蹤的狀態的單位和尺度不一樣，我們用矩陣 \(H_k\) 來表示感測器的資料。
我們可以計算出感測器讀數的分佈，用之前的表示方法如下式所示：
\[\begin{matrix} \vec{\mu}_{expected}=H_k\textcolor{magenta}{\hat{x}_k} \\ \Sigma_{expected}=H_k\textcolor{magenta}{P_k}H_k^T \end{matrix} \tag{8} \]我們就完成了對觀測值的預測：
\[(\textcolor{red}{\vec{\mu}_0},\textcolor{magenta}{\Sigma_0})= (\textcolor{red}{H_k\hat{x}_k},\textcolor{magenta}{H_kP_kH_k^T}) \tag{9} \]卡爾曼濾波的一大優點就是能處理感測器噪聲，換句話說，我們的感測器或多或少都有點不可靠，並且原始估計中的每個狀態可以和一定範圍內的感測器讀數對應起來。
從測量到的感測器資料中，我們大致能猜到系統當前處於什麼狀態。但是由於存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。
我們將這種不確定性（例如：感測器噪聲）用協方差 \(\textcolor{#00cec9}{R_k}\) 來表示，該分佈的均值就是我們讀取到的感測器資料，稱之為 \(\textcolor{#4cd137}{\vec{z_k}}\) ，感測器讀數（觀測值）服從高斯分佈：
\[(\textcolor{#4cd137}{\vec{\mu_1}},\textcolor{#00cec9}{\Sigma_1})=(\textcolor{#4cd137}{\vec{z_k}},\textcolor{#00cec9}{R_k})\tag{10} \]現在我們有了兩個高斯分佈，一個是在預測值附近（式 \((9)\) ），一個是在感測器讀數附近（式 \((10)\) ）。
我們必須在預測值和感測器測量值之間找到最優解。
那麼，我們最有可能的狀態是什麼呢？對於任何可能的讀數 \((z_1,z_2)\) ，有兩種情況：
- 感測器的測量值；
- 由前一狀態得到的預測值。
如果我們想知道這兩種情況都可能發生的概率，將這兩個高斯分佈相乘就可以了。
卡爾曼濾波需要做的最重要的最核心的事就是融合預測和觀測的結果，充分利用兩者的不確定性來得到更加準確的估計。通俗來說就是怎麼從上面的兩個橢圓中來得到中間部分的高斯分佈，看起來這是預測和觀測高斯分佈的重合部分，也就是概率比較高的部分。
這個重疊部分的均值就是兩個估計最可能的值，也就是給定的所有資訊中的最優估計。 對於任意兩個高斯分佈，將二者相乘之後還是高斯分佈，我們利用高斯分佈的兩個特性進行求解，其一是均值處分佈函式取極大值，其二是均值處分佈曲線的曲率為其二階導數。
8. 融合高斯分佈
先以一維高斯分佈來分析比較簡單點，具有方差 \(\sigma^2\) 和 \(\mu\) 的高斯曲線可以用下式表示：
\[N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}} \tag{11} \]如果把兩個服從高斯分佈的函式相乘會得到什麼呢？
\[N(x,\textcolor{magenta}{\mu_0},\textcolor{magenta}{\sigma_0})\cdot N(x,\textcolor{#4cd137}{\mu_1},\textcolor{#4cd137}{\sigma_1})\frac{?}{} N(x,\textcolor{blue}{\mu'},\textcolor{blue}{\sigma'}) \tag{12} \]將式 \((11)\) 代入到式 \((12)\) 中（注意重新歸一化，使總概率為1）可以得到：
\[\begin{matrix} \textcolor{blue}{\mu'}=\mu_0+\frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2} \\ \textcolor{blue}{\sigma'}^2=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2} \end{matrix}\tag{13} \]將式 \((13)\) 中的兩個式子相同的部分用 \(\textcolor{#8b008b}{k}\) 表示：
\[\textcolor{#8b008b}{k}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2} \tag{14} \]\[\begin{matrix} \textcolor{blue}{\mu'}=\mu_0+\textcolor{#8b008b}{k}(\mu_1-\mu_0) \\ \textcolor{blue}{\sigma'}^2=\sigma_0^2-\textcolor{#8b008b}{k}\sigma_0^2 \end{matrix}\tag{15} \]下面進一步將式 \((14)\) 和 \((15)\) 寫成矩陣的形式，如果 \(\Sigma\) 表示高斯分佈的協方差，\(\vec{\mu}\) 表示每個維度的均值，則：
\[\textcolor{#8b008b}{K}=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}\tag{16} \]\[\begin{matrix} \textcolor{blue}{\mu'}=\mu_0+\textcolor{#8b008b}{K}(\mu_1-\mu_0) \\ \textcolor{blue}{\Sigma'}=\Sigma_0-\textcolor{#8b008b}{K}\Sigma_0 \end{matrix}\tag{17} \]矩陣 \(\textcolor{#8b008b}{K}\) 稱為卡爾曼增益，下面將會用到。
9. 整合公式
將預測部分的高斯分佈（式 \((9)\) ）\((\textcolor{red}{\vec{\mu}_0},\textcolor{magenta}{\Sigma_0})=(\textcolor{red}{H_k\hat{x}_k},\textcolor{magenta}{H_kP_kH_k^T})\) 和測量部分的高斯分佈（式 \((10)\) ）\((\textcolor{#4cd137}{\vec{\mu_1}},\textcolor{#00cec9}{\Sigma_1})=(\textcolor{#4cd137}{\vec{z_k}},\textcolor{#00cec9}{R_k})\) 與式 \((17)\) 聯立得出：
\[\begin{align} \begin{matrix} \ \ \ H_k\textcolor{blue}{\hat{x}_k'}=\textcolor{red}{H_k\hat{x}_k}\ \ \ \ \ \ \ \ \ +\textcolor{#8b008b}{K}(\textcolor{#4cd137}{\vec{z_k}}-\textcolor{red}{H_k\hat{x}_k}) \\ H_k\textcolor{blue}{P_k'}H_k^T=\textcolor{magenta}{H_kP_kH_k^T}\ \ \ -\textcolor{#8b008b}{K}\textcolor{magenta}{H_kP_kH_k^T}\ \ \ \ \ \ \ \ \ \end{matrix}\tag{18} \end{align} \]由式 \((16)\) 可得卡爾曼增益為：
\[\textcolor{#8b008b}{K}=\textcolor{magenta}{H_kP_kH_k^T}(\textcolor{magenta}{H_kP_kH_k^T}+\textcolor{#00cec9}{R_k})^{-1}\tag{19} \]將式 \((18)\) 和式 \((19)\) 的兩邊同時左乘矩陣的逆 \(H_k^{-1}\)（注意 \(\textcolor{#8b008b}{K}\) 裡面包含了 \(H_k\) ）將其約掉：
\[\textcolor{#8b008b}{K'}=\textcolor{magenta}{P_kH_k^T}(\textcolor{magenta}{H_kP_kH_k^T}+\textcolor{#00cec9}{R_k})^{-1}\tag{20} \]\[\begin{align} \begin{matrix} \textcolor{blue}{\hat{x}_k'}=\textcolor{red}{\hat{x}_k}+\textcolor{#8b008b}{K'}(\textcolor{#4cd137}{\vec{z_k}}-\textcolor{red}{H_k\hat{x}_k}) \\ \textcolor{blue}{P_k'}H_k^T=\textcolor{magenta}{P_kH_k^T}-\textcolor{#8b008b}{K'}\textcolor{magenta}{H_kP_kH_k^T} \end{matrix}\tag{21} \end{align} \]再將式 \((21)\) 的第二個等式兩邊同時右乘矩陣 \(H_k^T\) 的逆得到以下等式：
\[\textcolor{blue}{P_k'}=\textcolor{magenta}{P_k}-\textcolor{#8b008b}{K'}\textcolor{magenta}{H_kP_k} \]整理最終結果為：
\[\begin{align} \begin{matrix} \textcolor{blue}{\hat{x}_k'}=\textcolor{red}{\hat{x}_k}+\textcolor{#8b008b}{K'}(\textcolor{#4cd137}{\vec{z_k}}-\textcolor{red}{H_k\hat{x}_k}) \\ \textcolor{blue}{P_k'}=\textcolor{magenta}{P_k}-\textcolor{#8b008b}{K'}\textcolor{magenta}{H_kP_k} \end{matrix}\tag{22} \end{align} \]\[\textcolor{#8b008b}{K'}=\textcolor{magenta}{P_kH_k^T}(\textcolor{magenta}{H_kP_kH_k^T}+\textcolor{#00cec9}{R_k})^{-1}\tag{23} \]上式就是完整的更新步驟方程。\(\textcolor{blue}{\hat{x}_k'}\) 就是新的最優估計，我們可以將它和 \(\textcolor{blue}{P_k'}\) 放到下一個預測和更新方程中不斷迭代。
10. 總結
以上所有公式中，你只需要用到式 \((7)\) 、 \((22)\) 、 \((23)\)。（如果忘了的話，你可以根據式 \((4)\) 和 \((17)\) 重新推導一下）。
我們可以用這些公式對任何線性系統建立精確的模型，對於非線性系統來說，我們使用擴充套件卡爾曼濾波，區別在於EKF多了一個把預測和測量部分進行線性化的過程。