這篇主要介紹卡爾曼濾波公式詳細推導，使用示例參考卡爾曼濾波示例 。
主要講解基本的卡爾曼濾波算法，有時候也說是離散或者線性卡爾曼濾波。
首先來看一個數學公式，這部分僅僅是給定一個思路，和最後實際算法無關。目前考慮到要估計當前系統的狀態，而且有兩個已知量，一個上一個狀態的估計值以及當前狀態的測量值，這兩個都有一定的噪聲，需要做的就是把這兩個結合起來，很簡單的思路就是按照比例相加得到當前狀態的估計值：X ^ k = K k ⋅ Z k + ( 1 − K k ) ⋅ X ^ k − 1
\hat{X}_k = K_k \cdot Z_k + (1 - K_k) \cdot \hat{X}_{k-1}
X ^ k = K k ⋅ Z k + ( 1 − K k ) ⋅ X ^ k − 1
k k k
X ^ k \hat{X}_k X ^ k
Z k Z_k Z k
X ^ k − 1 \hat{X}_{k-1} X ^ k − 1
K k K_k K k
後面卡爾曼濾波算法就是按照上面思路利用上一狀態以及測量值去估計當前狀態，只不過模型要更加複雜。
卡爾曼濾波的狀態方程，利用線性隨機差分方程（Linear Stochastic Difference equation）利用上一個系統狀態估計當前系統狀態（這裏假設上一狀態與下一一狀態有某種線性關係，比如恆溫環境的溫度，勻速運動的速度等，但是因爲現實環境的複雜，這種線性關係不是完全平滑的，也就是會有一些擾動）：x k = A x k − 1 + B u k − 1 + w
x_k = Ax_{k-1} + Bu_{k-1}+w
x k = A x k − 1 + B u k − 1 + w u u u x k = A x k − 1 + w ( 1.1 )
x_k = Ax_{k-1} + w \qquad {(1.1)}
x k = A x k − 1 + w ( 1 . 1 ) z k = H x k + v ( 1.2 )
z_k = Hx_k + v \qquad {(1.2)}
z k = H x k + v ( 1 . 2 )
k − 1 k-1 k − 1 k k k
x ∈ R n x \in R^n x ∈ R n
A ∈ R n × n A \in R^{n \times n} A ∈ R n × n
u ∈ R l u \in R^l u ∈ R l
B ∈ R n × l B \in R^{n \times l} B ∈ R n × l
z ∈ R m z \in R^m z ∈ R m
H ∈ R m × n H \in R^{m \times n} H ∈ R m × n
w ∈ R n w \in R^n w ∈ R n
v ∈ R m v \in R^m v ∈ R m
在上面轉換矩陣 A A A B B B H H H w w w v v v
上面提到的過程噪聲 w w w v v v p ( w ) ∼ N ( 0 , Q ) ( 1.3 ) p ( v ) ∼ N ( 0 , R ) ( 1.4 ) Q = w w T R = v v T E ( w ) = 0 E ( v ) = 0
\begin{aligned}
p(w) & \sim N(0, Q) \qquad {(1.3)} \\
p(v) & \sim N(0, R) \qquad {(1.4)} \\
Q & = ww^T \\
R & = vv^T \\
E(w) & = 0 \\
E(v) & = 0
\end{aligned}
p ( w ) p ( v ) Q R E ( w ) E ( v ) ∼ N ( 0 , Q ) ( 1 . 3 ) ∼ N ( 0 , R ) ( 1 . 4 ) = w w T = v v T = 0 = 0
Q ∈ R n × n Q \in R^{n \times n} Q ∈ R n × n w w w w w w
R ∈ R m × m R \in R^{m \times m} R ∈ R m × m v v v v v v
在上面協方差矩陣 Q Q Q R R R
上面是給出了卡爾曼濾波的整體思路和基本模型，在這裏一步步分解來看卡爾曼濾波的推導過程。首先確定了卡爾曼濾波的基本模型，那麼下一步是怎樣根據這個模型來不斷求取後面的狀態。
首先定義幾個變量：
x ^ k ˉ \bar{\hat{x}_k} x ^ k ˉ
x ^ k \hat{x}_k x ^ k z k z_k z k
需要注意的是，卡爾曼濾波的最終目標是求 x ^ k \hat{x}_k x ^ k
根據上面的定義忽略過程噪聲可以得到：x ^ k ˉ = A x ^ k − 1 + B u k − 1
\bar{\hat{x}_k} = A\hat{x}_{k-1} + Bu_{k-1}
x ^ k ˉ = A x ^ k − 1 + B u k − 1 u u u x ^ k ˉ = A x ^ k − 1 ( 2.1 )
\bar{\hat{x}_k} = A\hat{x}_{k-1} \qquad {(2.1)}
x ^ k ˉ = A x ^ k − 1 ( 2 . 1 ) z k ˉ = H x ^ k ˉ
\begin{aligned}
\bar{z_k} & = H\bar{\hat{x}_k}
\end{aligned}
z k ˉ = H x ^ k ˉ
z k ˉ \bar{z_k} z k ˉ
那麼很簡單的可以用實際得到的測量值減去無噪聲由上一狀態預測的測量值：z k ˙ = z k − z k ˉ = z k − H x ^ k ˉ
\begin{aligned}
\dot{z_k} & = z_k - \bar{z_k} \\
& = z_k - H\bar{\hat{x}_k}
\end{aligned}
z k ˙ = z k − z k ˉ = z k − H x ^ k ˉ
z k ˙ \dot{z_k} z k ˙ w w w v v v
現在得到了一個無噪聲的狀態的估計 x ^ k ˉ \bar{\hat{x}_k} x ^ k ˉ z k ˙ \dot{z_k} z k ˙ 其實對於下面的公式在數學上有嚴格的證明過程，但是僅僅是使用卡爾曼濾波就不去深究了 ）：x ^ k = x ^ k ˉ + K k z k ˙ = x ^ k ˉ + K k ( z k − H x ^ k ˉ ) ( 2.2 )
\begin{aligned}
\hat{x}_k & = \bar{\hat{x}_k} + K_k\dot{z_k} \\
& = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k}) \qquad {(2.2)}
\end{aligned}
x ^ k = x ^ k ˉ + K k z k ˙ = x ^ k ˉ + K k ( z k − H x ^ k ˉ ) ( 2 . 2 )
K k ∈ R n × m K_k \in R^{n \times m} K k ∈ R n × m
到這裏，有了上面的公式(2.2)，爲了得到 x ^ k \hat{x}_k x ^ k K k K_k K k e k ˉ ≡ x k − x ^ k ˉ e k ≡ x k − x ^ k
\bar{e_k} \equiv x_k - \bar{\hat{x}_k} \\
e_k \equiv x_k - \hat{x}_k
e k ˉ ≡ x k − x ^ k ˉ e k ≡ x k − x ^ k P k ˉ = E [ e k ˉ e ˉ k T ] P k = E [ e k e k T ]
\bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] \\
P_k = E[e_ke_{k}^T]
P k ˉ = E [ e k ˉ e ˉ k T ] P k = E [ e k e k T ] x ^ k \hat{x}_k x ^ k P k P_k P k
根據公式(1.1)，(2.2)，(1.2)，(2.1)可以得到：e k = x k − x ^ k = x k − ( x ^ k ˉ + K k ( z k − H x ^ k ˉ ) ) = x k − ( x ^ k ˉ + K k ( ( H x k + v ) − H x ^ k ˉ ) ) = x k − x ^ k ˉ − K k H x k − K k v + K k H x ^ k ˉ = ( I − K k H ) ( x k − x ^ k ˉ ) − K k v = ( I − K k H ) e k ˉ − K k v
\begin{aligned}
e_k & = x_k - \hat{x}_k \\
& = x_k - (\bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k})) \\
& = x_k - (\bar{\hat{x}_k} + K_k((Hx_k + v) - H\bar{\hat{x}_k})) \\
& = x_k - \bar{\hat{x}_k} - K_kHx_k - K_kv + K_kH\bar{\hat{x}_k} \\
& = (I - K_kH)(x_k-\bar{\hat{x}_k}) - K_kv \\
& = (I - K_kH)\bar{e_k} - K_kv
\end{aligned}
e k = x k − x ^ k = x k − ( x ^ k ˉ + K k ( z k − H x ^ k ˉ ) ) = x k − ( x ^ k ˉ + K k ( ( H x k + v ) − H x ^ k ˉ ) ) = x k − x ^ k ˉ − K k H x k − K k v + K k H x ^ k ˉ = ( I − K k H ) ( x k − x ^ k ˉ ) − K k v = ( I − K k H ) e k ˉ − K k v e k e k T = [ ( I − K k H ) e k ˉ − K k v ] [ ( I − K k H ) e k ˉ − K k v ] T = ( I − K k H ) e k ˉ e k ˉ T ( I − K k H ) T − K k v e k ˉ T ( I − K k H ) T − ( I − K k H ) e k ˉ v T K k T + K k v v T K k T
\begin{aligned}
e_ke_{k}^T & = \left[ (I - K_kH)\bar{e_k} - K_kv \right]\left[ (I - K_kH)\bar{e_k} - K_kv \right]^T \\
& = (I - K_kH)\bar{e_k}\bar{e_k}^T(I - K_kH)^T - K_kv\bar{e_k}^T(I - K_kH)^T - (I - K_kH)\bar{e_k}v^TK_k^T + K_kvv^TK_k^T
\end{aligned}
e k e k T = [ ( I − K k H ) e k ˉ − K k v ] [ ( I − K k H ) e k ˉ − K k v ] T = ( I − K k H ) e k ˉ e k ˉ T ( I − K k H ) T − K k v e k ˉ T ( I − K k H ) T − ( I − K k H ) e k ˉ v T K k T + K k v v T K k T e k ˉ \bar{e_k} e k ˉ z 1 , ⋯  , z k − 1 z_{1} , \cdots , z_{k - 1} z 1 , ⋯ , z k − 1 z k z_k z k
E ( e k ˉ v T ) = 0 E ( v e k ˉ T ) = 0
E(\bar{e_k}v^T) = 0 \\
E(v\bar{e_k}^T) = 0
E ( e k ˉ v T ) = 0 E ( v e k ˉ T ) = 0
帶入整理得到：P k = E [ e k e k T ] = ( I − K k H ) P k ˉ ( I − K k H ) T + K k R K k T = P k ˉ − K k H P k ˉ − P k ˉ H T K k T + K k H P k ˉ H T K k T + K k R K k T
\begin{aligned}
P_k = E[e_ke_{k}^T] & = (I-K_kH)\bar{P_k}(I - K_kH)^T + K_kRK_k^T \\
& = \bar{P_k} - K_kH\bar{P_k} - \bar{P_k}H^TK_k^T + K_kH\bar{P_k}H^TK_k^T + K_kRK_k^T
\end{aligned}
P k = E [ e k e k T ] = ( I − K k H ) P k ˉ ( I − K k H ) T + K k R K k T = P k ˉ − K k H P k ˉ − P k ˉ H T K k T + K k H P k ˉ H T K k T + K k R K k T K k K_k K k P k = P k ˉ − P k ˉ H T ( H P k ˉ H T + R ) − 1 H P k ˉ + [ K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 ] ( H P k ˉ H T + R ) [ K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 ] T
\begin{aligned}
P_k & = \bar{P_k} - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}H\bar{P_k} \\
& + \left[ K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1}\right ](H\bar{P_k}H^T + R)\left[K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \right ]^T
\end{aligned}
P k = P k ˉ − P k ˉ H T ( H P k ˉ H T + R ) − 1 H P k ˉ + [ K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 ] ( H P k ˉ H T + R ) [ K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 ] T K k K_k K k P k P_k P k K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 = 0
K_k - \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} = 0
K k − P k ˉ H T ( H P k ˉ H T + R ) − 1 = 0 K k = P k ˉ H T ( H P k ˉ H T + R ) − 1 ( 2.5 )
K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} \qquad {(2.5)}
K k = P k ˉ H T ( H P k ˉ H T + R ) − 1 ( 2 . 5 ) P k = ( I − K k H ) P k ˉ ( 2.4 )
P_k = (I-K_kH)\bar{P_k} \qquad {(2.4)}
P k = ( I − K k H ) P k ˉ ( 2 . 4 )
帶入上面的公式(1.1)，(2.1)可以得到：e k ˉ = x k − x ^ k ˉ = ( A x k − 1 + w ) − ( A x ^ k − 1 ) = A ( x k − 1 − x k − 1 ^ ) + w = A e k − 1 + w
\begin{aligned}
\bar{e_k} & = x_k - \bar{\hat{x}_k} \\
& = (Ax_{k-1} + w) - (A\hat{x}_{k-1}) \\
& = A(x_{k-1} - \hat{x_{k-1}}) + w \\
& = Ae_{k-1} + w
\end{aligned}
e k ˉ = x k − x ^ k ˉ = ( A x k − 1 + w ) − ( A x ^ k − 1 ) = A ( x k − 1 − x k − 1 ^ ) + w = A e k − 1 + w e k ˉ e ˉ k T = ( A e k − 1 + w ) ( A e k − 1 + w ) T = A e k − 1 e k − 1 T A T + w w T + w e k − 1 T A T + A e k − 1 w T
\begin{aligned}
\bar{e_k}\bar{e}_{k}^T & = (Ae_{k-1} + w)(Ae_{k-1} + w)^T \\
& = Ae_{k-1}e_{k-1}^TA^T + ww^T + we_{k-1}^TA^T + Ae_{k-1}w^T
\end{aligned}
e k ˉ e ˉ k T = ( A e k − 1 + w ) ( A e k − 1 + w ) T = A e k − 1 e k − 1 T A T + w w T + w e k − 1 T A T + A e k − 1 w T
又因爲：E ( e k − 1 w T ) = 0 E ( w e k − 1 T ) = 0
E(e_{k-1}w^T) = 0 \\
E(we_{k-1}^T) = 0
E ( e k − 1 w T ) = 0 E ( w e k − 1 T ) = 0
然後可以得到：P k ˉ = E [ e k ˉ e ˉ k T ] = A P k − 1 A T + Q ( 2.3 )
\bar{P_k} = E[\bar{e_k}\bar{e}_{k}^T] = AP_{k-1}A^T + Q \qquad {(2.3)}
P k ˉ = E [ e k ˉ e ˉ k T ] = A P k − 1 A T + Q ( 2 . 3 )
從上面的公式可以看出來，如果過程誤差 P k ˉ = 0 \bar{P_k} = 0 P k ˉ = 0 K k = 0 K_k = 0 K k = 0 x ^ k = x ^ k ˉ = A x ^ k − 1 \hat{x}_k = \bar{\hat{x}_k} = A\hat{x}_{k-1} x ^ k = x ^ k ˉ = A x ^ k − 1 P 0 = 0 P_0 = 0 P 0 = 0 Q Q Q R R R R R R
最後得到了上面的上面(2.1)-(2.5)五個方程式，是經典的卡爾曼濾波的五個基本公式，利用這五個公式就可以完成卡爾曼濾波的求解過程。根據迭代過程分成兩個方程集合，一個是time update方程，一個是measurement update方程，如下：
Time Update Measurement Update
x ^ k ˉ = A x ^ k − 1 \bar{\hat{x}_k} = A\hat{x}_{k-1} x ^ k ˉ = A x ^ k − 1 K k = P k ˉ H T ( H P k ˉ H T + R ) − 1 K_k = \bar{P_k}H^T(H\bar{P_k}H^T + R)^{-1} K k = P k ˉ H T ( H P k ˉ H T + R ) − 1
P k ˉ = A P k − 1 A T + Q \bar{P_k} = AP_{k-1}A^T + Q P k ˉ = A P k − 1 A T + Q x ^ k = x ^ k ˉ + K k ( z k − H x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - H\bar{\hat{x}_k}) x ^ k = x ^ k ˉ + K k ( z k − H x ^ k ˉ )
P k = ( I − K k H ) P k ˉ P_k = (I - K_kH)\bar{P_k} P k = ( I − K k H ) P k ˉ
得到上面的基本公式以後，就可以計算計算後面的狀態，但是公式中有一些參數是未知的，需要定義，如下：
u u u
A A A B B B H H H k k k
R R R
Q Q Q
z z z
對於R R R Q Q Q
The better you estimate the noise parameters, the better estimates you get.
那麼這樣只需要傳入基本的 x ^ 0 \hat{x}_0 x ^ 0 P 0 P_0 P 0 x ^ 0 \hat{x}_0 x ^ 0 x x x P 0 P_0 P 0 x ^ 0 \hat{x}_0 x ^ 0
算法迭代過程如下圖（其中 u u u
Kalman Filter For Dummies Greg Welch, Gary Bishop, “An Introduction to the Kalman Filter”, University of North Carolina at Chapel Hill Department of Computer Science, 2001
一個應用實例詳解卡爾曼濾波及其算法實現 卡爾曼濾波算法–核心公式推導導論
