卡爾曼濾波

卡爾曼濾波的介紹與使用

一開始理解卡爾曼濾波花了很多時間，一旦瞭解了之後就沒有開始時那種困難的感受了。基本上下面這張圖就歸納了卡爾曼濾波。

可見就是咱們能夠具體知道的信息。其中u是咱們預先知道的東西，Z是經過傳感器等獲得的觀測值。

模型

模型是反映現實狀況的，圖中的箭頭方向是事情的因果順序。（我當時就卡在混淆了模型和計算的區別）

x表示系統的真實狀態，它是不可知的。好比說室內的溫度，其實咱們沒有辦法知道確切的室內溫度，咱們老是經過感受或者傳感器（溫度計）來獲取溫度值。而Z就是那個咱們可以獲得的溫度值。這個模型很好地反映了現實中的狀況。在外部因素u的做用下x發生了改變，而咱們經過方法H去測量x獲得z。然而現實中老是不可避免地發生一些不肯定因素，因此引入偏差w與v。
須要注意的是卡爾曼濾波是一個線性的模型，並非因此場景下都能使用。所引入的偏差w與v服從正態分佈。

計算

接下來咱們要根據模型中的內容，來獲得咱們想要的東西。也就是已知u與z，求x。

1. 預測
   $${\hat {{\textbf {x}}}}_{{k|k-1}}={\textbf {F}}_{{k}}{\hat {{\textbf {x}}}}_{{k-1|k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}(預測狀態)$$

   $${\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}} {\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}（預測估計協方差矩陣）$$

2. 更新
   首先要算出如下三個量：

   $${\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1}} {\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-{\textbf {H}}_{{k}}{\hat {{\textbf {x}}}}_{{k|k-1}}(測量餘量，measurement residual)$$

   $${\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}} {\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}(測量餘量協方差)$$

   $${\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}} {\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}（最優卡爾曼增益）$$
   而後用它們來更新濾波器變量x與P：

   $${\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}} {\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}(更新的狀態估計)$$

   $${\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}} {\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}（更新的協方差估計）$$

   其中\({x}_{k|k}\) 就是咱們但願獲取的系統真是值。\({P}_{k|k}\) 是須要不斷迭代的預測估計。
   須要注意的是\({Q}_{k}\),\({R}_{k}\) 並不能從前面創建的模型中取得。它們是協方差矩陣，表明了模型中的偏差w與v的可行度。也就是說值越大就越不可信。所以在實際使用中須要對\({Q}_{k}\),\({R}_{k}\)進行測量估計，調參來使得提高濾波效果。它們間的取值反映了當我有傳感器數據，有模型預測數據的時候，如何對它們進行權衡。

實例

下面我將經過軟件模擬數據來解釋如何使用卡爾曼濾波。
假設咱們在燒開水，其真實狀況是溫度關於時間的線性函數 \(x=1.3 t\).咱們的溫度計存在均值爲0，標準差爲5

在這個簡單模型下F、H均爲1. R=25,Q=1

1. 預測

   X=X+1.3;
   P=P+Q;

2. 更新

   S=P+R;
   K=P/(S);
   X=X+K(Z-X);
   P=(1-K)P;

進階

若是隻能處理精準模型下的預測，那功能就太侷限了。如今把前面溫度計的例子作一點修改。\(x=K t\) 其中K是一個未知數。接下來不只要對數據進行濾波，還要肯定參數K！
新的模型爲X=X+K
$$F=\left(\begin{array}{cc} 1 & 1 \\ 0 & 1 \\\end{array}\right)$$
$$H=\left(\begin{array}{cc} 1 & 0 \\\end{array}\right)$$

協方差矩陣Q,R：
$$Q=\left(\begin{array}{cc} 0 & 0. \\ 0. & 0.0001 \\\end{array}\right);R=25;$$

代入數據：

fun[{X_, P_, i_}] := Module[{X1 = F .X,
   P1 = F . P.Transpose[F] + Q},
  y = z[i] - H . X1;
  S = H .P1. Transpose[H] + R;
  K = P1.Transpose[H].Inverse[S];
  {X1 + K . y, N[(IdentityMatrix[2] - K.H).P1], i + 1}]

代入不正確的初值(0,0.6)依然獲得比較接近的結果。

若是調小Q爲0 那麼最後會穩定到原函數。

後續

1. 協方差矩陣R,Q對平衡觀測數據與預測結果有相當重要的影響。R,Q的值如何肯定？這個我暫時沒能力解釋。
2. 卡爾曼濾波只能處理線性模型。對於非線性模型可使用擴展卡爾曼濾波。