詳解Kalman Filter

中心思想

現有：

1. 已知上一刻狀態，預測下一刻狀態的方法，能得到一個“預測值”。（當然這個估計值是有誤差的）
2. 某種測量方法，可以測量出系統狀態的“測量值”。（當然這個測量值也是有誤差的）

我們如何去估計出系統此時真實的狀態呢？
答案是需要結合“預測值”和“測量值”。例如我們可以加權求和，但是這個權重要怎麼定義，才能準確估計出真實狀態呢？這個權重就是Kalman Filter解決的事情。

系統建模

預測方法

\[x_k=F_kx_{k-1}+B_ku_k+w_k \]

我們假設這個預測方法是線性變換，\(B_ku_k\)是除了狀態轉移之外的控制量。\(w_k\)是預測誤差，假設為高斯分佈（即均值為0的多元正態分佈），其協方差矩陣為\(Q_k\)。
也就是說，下一刻我們的預測值，有一部分與上一刻狀態有關，一部分與外力控制有關（外力控制與上一刻狀態無關），還有一部分被噪聲所影響。
舉個例子：
假設一小車在x軸上向前以速度\(v\)勻速運動，\(x_k\)表示的是k時刻小車在x軸上的座標。顯然我們有$$x_k=x_{k-1}+vt$$，這裡速度v和時間t都和上一個狀態無關，屬於讓小車位置變化的“外力”。在這個例子裡\(F_kx_{k-1}\)部分就是\(x_{k-1}\)，而\(B_ku_k\)部分就是\(vt\)。

測量方法

\[z_k=H_kx_k+v_k \]

因為我們不一定有測量儀器能直接測量出系統狀態，因此我們假設測量方法也是線性變換。
也就是說，當我們的測量儀器用於測量系統狀態\(x_k\)時，它的讀數是系統狀態加上一定的線性變換\(H_k\)，以及測量噪聲\(v_k\)，假設為高斯分佈（即均值為0的多元正態分佈），其協方差矩陣為\(R_k\)。
舉個例子：
我們稱體重需要得到以“斤”為單位的資料，這是系統狀態，但是我們的稱只能讀出單位為“kg”的資料（這就是\(z_k\)），那我們就需要做一個單位轉換（對應\(H_k\)），此外，由於稱不一定準，所以最後稱的讀數還得加上一點噪聲。

所有引數中：\(F_k\)、\(B_k\)、\(u_k\)、\(Q_k\)、\(H_k\)、\(R_k\)都需要已知，要麼自己根據公式和經驗定義，要麼從樣本資料裡估計一個值。

演算法

Kalman Filter將一直維護對系統狀態\(x_k\)的最優估計值，以及這個估計值的偏差：

• \(\hat{x}_k\)，系統狀態，可以是多維的。
• \(P_k\)，\(\hat{x}_k\)的誤差。當\(\hat{x_k}\)是一維時，\(P_k\)是方差；當\(\hat{x_k}\)是多維時，\(P_k\)是協方差\(cov(\hat{x_k})\)，就是\(\hat{x_k}\)裡各維兩兩協方差。

預測階段

首先，通過系統的預測方法，我們可以得到“預測值”：

\[\bar{x_k}=F_k\hat{x}_{k-1}+B_ku_k \]

由於誤差不知道，且假設其均值為0，所以這裡不算誤差
那麼協方差也可以從上一個狀態轉移：

\[\bar{P}_k=F_kP_{k-1}F_k^T+Q_k \]

更新階段

這個階段需要結合“預測值”和“測量值”。先把具體的方程擺上來：

• 計算測量值殘差（innovation/measurement pre-fit residual）：\(\tilde{y}_k=z_k-H_k\bar{x_k}\)（我的理解是\(H_k\)就是為了把測量值和預測值轉換到同一個空間）
• 計算測量值殘差的協方差（Innovation (or pre-fit residual) covariance）：\(S_k=H_k\bar{P}_kH_k^T+R_k\)
• 計算卡爾曼增益（Kalman Gain）：\(K_k=\bar{P}_kH_k^TS_k^{-1}\)
• “更新” 最終得到的估計值：\(\hat{x}_k=\hat{x}_{k-1}+K_k\tilde{y}_k\)
• “更新” 最終得到的估計值的協方差：\(P_k=(I-K_kH_k)P_{k-1}\)

總之最後的演算法就是，每得到一個“測量值”，就重複一遍預測和更新階段。

\(\mathbf{K}\)就是Kalman Gain，它衡量了“測量值”和“預測值”之間的權重比例，\(\mathbf{K}\)越大，“測量值”所佔權重越大。從公式可以看出:

• \(Q_k\)越大，\(\bar{P}_k\)越大，\(\mathbf{K_k}\)越大。這從物理意義上也是說得通的，當預測值的誤差更大的時候，當然應該多信任測量值。
• \(R_k\)越大，\(S_k\)越大，\(\mathbf{K_k}\)越小。也就是說，當“測量值”的誤差越大時，該公式將更信任“預測值”。

所有引數中，\(F_k\)、\(B_k\)、\(u_k\)、\(H_k\)基本都需要根據你的問題的建模來決定，而除此之外的\(Q_k\)和\(R_k\)就是兩個用於控制預測靈活性的引數，有點類似於指數滑動平均演算法的那個\(\alpha\)。

更新階段原理（試圖）詳解

結合“預測值”和“測量值”的思想

參考：如何通俗並儘可能詳細地解釋卡爾曼濾波？ - 肖暢的回答 - 知乎

（結合高斯分佈解釋）具體公式怎麼推導

參考：圖說卡爾曼濾波，一份通俗易懂的教程

讓我們從一維看起，設方差為\(\sigma^2\)，均值為\(\mu\)，一個標準一維高斯鐘形曲線方程如下所示：

\[\mathcal{N}(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}} \]

那麼兩條高斯曲線相乘呢？

\[\mathcal{N}(x,\mu_0,\sigma_0)\cdot\mathcal{N}(x,\mu_1,\sigma_1)=\mathcal{N}(x,\mu',\sigma') \]

把這個式子按照一維方程進行擴充套件，可得：

\[\mu'=\mu_0+\frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2} \]

\[\sigma'^2=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2} \]

如果有些太複雜，我們用k簡化一下：

\[\mathbf{k}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2} \]

\[\mu'=\mu_0+\mathbf{k}(\mu_1-\mu_0) \]

\[\sigma'^2=\sigma_0^2-\mathbf{k}\sigma_0^2 \]

以上是一維的內容，如果是多維空間，把這個式子轉成矩陣格式：

\[\mathbf{K}=\Sigma_0(\Sigma_0+\Sigma_1)^{-1} \]

\[\mu'=\mu_0+\mathbf{K}(\mu_1-\mu_0) \]

\[\Sigma'=\Sigma_0-\mathbf{K}\Sigma_0 \]

其中，\(\Sigma\)表示協方差。
代入到Kalman Filter裡，我們把“預測分佈”\((\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)\)，和“測量分佈”\((\mu_1, \Sigma_1)=(z_k,R_k)\)代入到上面的等式裡，那麼新分佈\((\mu',\Sigma')=(H_k\hat{x}_k, H_kP_kH_k^T)\)為：

這裡\((\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)\)乘以了係數\(H_k\)是為了把\(x_k\)轉換到和\(z_k\)一個座標系。

\[K=H_k\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1} \]

\[H_k\hat{x}_k=H_k\bar{x}_k+K(z_k-H_k\bar{x}_k) \]

\[H_kP_kH_k^T=H_k\bar{P}_kH_k^T-KH_k\bar{P}_kH_k^T \]

等式兩邊消掉\(H_k\)並化簡後：

\[K_k=\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1} \]

\[\hat{x}_k=\bar{x}_k+K_k(z_k-H_k\bar{x}_k) \]

\[P_k=(I-K_kH_k)\bar{P}_k \]