本文在參考文獻的基礎上添加了自己的理解，若有不當之處，敬請指正 卡爾曼濾波以前接觸過，但是沒有仔細推導，這次參考文獻仔細推導實現，也是第一次完全的通過vscode+markdown來完成寫作。
1960年, R.E. Kalman發表了他使用迭代方法解決離散線性濾波問題的文章（《A New Approach to Linear Filtering and Prediction Problems》），揭開了卡爾曼濾波的歷史篇章，得益於數字計算能力的發展，卡爾曼濾波方法在得到了廣泛應用，尤其是在自主/輔助導航方面。 現在該方法在計算機圖像處理，多數據融合方面也得到廣泛應用。1）第一個問題：什麼是卡爾曼濾波 2）第二個問題:爲什麼用卡爾曼濾波
本方法注重思路，詳細過程請參考文章開始列出的文獻。
前邊提到，卡爾曼是一種線性二次估計，相當於一種優化算法，優化算法的目標就是針對損失函數，並使得損失函數最大或者最小。首先考慮一個最簡單的線性模型：
y k = a k x k + n k y_k = a_kx_k + n_k y k = a k x k + n k x k x_k x k e k = x k − x k ^ e_k = x_k - \hat{x_k} e k = x k − x k ^ －－這個誤差越小，建模狀態量越準確，得到的輸出值也就越準確 e k e_k e k 均方差（mean squared error， MSE） , 可以只向一個方向優化：l o s s f u n c i t o n = E ( e k 2 ) lossfunciton = E(e_k^2) l o s s f u n c i t o n = E ( e k 2 )
原理：從已知結果出發，反推參數值，這個參數值將使得結果出現的概率最大。x k ^ \hat{x_k} x k ^
m a x ( P [ y ∣ x ^ ] ) max(P[y|\hat x]) m a x ( P [ y ∣ x ^ ] )
P ( y k ∣ x ^ k ) = K k e x p − ( ( y k − a k x ^ k ) 2 2 σ k 2 ) P(y_k|\hat x_k) = K_kexp-(\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) P ( y k ∣ x ^ k ) = K k e x p − ( 2 σ k 2 ( y k − a k x ^ k ) 2 )
P ( y k ∣ x ^ k ) = Π k K k e x p − ( ( y k − a k x ^ k ) 2 2 σ k 2 ) P(y_k|\hat x_k) =\mathop{\Pi} \limits_{k} K_kexp-(\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) P ( y k ∣ x ^ k ) = k Π K k e x p − ( 2 σ k 2 ( y k − a k x ^ k ) 2 )
l o g P ( y k ∣ x ^ k ) = − 1 2 ∑ k ( ( y k − a k x ^ k ) 2 2 σ k 2 ) + c o n s t a n t logP(y_k|\hat x_k) = -\frac{1}{2}\sum \limits_{k} (\frac{(y_k -a_k\hat x_k)^2}{2\sigma _k^2}) + constant l o g P ( y k ∣ x ^ k ) = − 2 1 k ∑ ( 2 σ k 2 ( y k − a k x ^ k ) 2 ) + c o n s t a n t
所以目標就是，找到x ^ k \hat x_k x ^ k l o g P ( y k ∣ x ^ k ) logP(y_k|\hat x_k) l o g P ( y k ∣ x ^ k ) x ^ k \hat x_k x ^ k l o g P ( y k ∣ x ^ k ) logP(y_k|\hat x_k) l o g P ( y k ∣ x ^ k ) x ^ k \hat x_k x ^ k
總體思路還是比較簡單：由狀態構建一個誤差表達式，並將該表達式構建爲MSE形式，根據已知條件推導，將MES改寫稱爲一個包含已知量的表達式，求導，令導數等於零求得極小點，將該點回代至表達式，結束！整個推導過程的步驟也比較簡單，只是涉及的量比較多，以及矩陣表達的形式，看起來比較繁雜而已。
已知條件，6個
狀態方程： x k + 1 = Φ x k + w k x_{k+1} = \Phi x_k + w_k x k + 1 = Φ x k + w k
觀測方程： z k = H x k + v k z_k = Hx_k + v_k z k = H x k + v k
w k w_k w k v k v_k v k
Q = E ( w k w k T ) Q = E(w_kw_k^T) Q = E ( w k w k T ) R = E ( v k v k T ) R = E(v_kv_k^T) R = E ( v k v k T ) e k = x k − x ^ k e_k = x_k - \hat x_k e k = x k − x ^ k x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) \hat x_k =\hat x_k^{'} + K_k(z_k - H\hat x_k^{'}) x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) … x ^ k ′ \ldots \hat x_k^{'} … x ^ k ′ 該式將在下一節的極大似然估計中推導 。
求解問題
l o s s f u n c i t o n = E ( e k 2 ) = E ( e k e k T ) = P k lossfunciton = E(e_k^2) = E(e_ke_k^T) = P_k l o s s f u n c i t o n = E ( e k 2 ) = E ( e k e k T ) = P k
推導過程
先處理已知條件：
z k z_k z k x ^ k = x ^ k ′ + K k ( z k − H x ^ k ) = x ^ k ′ + K k ( H x k + v k − H x ^ k ′ ) = K k H x k + ( I − K k H ) x k ′ + K k v k \hat x_k =\hat x_k^{'} + K_k(z_k - H\hat x_k) = \hat x_k^{'} + K_k(Hx_k + v_k - H\hat x_k^{'}) =K_kHx_k + (I - K_kH)x_k^{'} +K_kv_k x ^ k = x ^ k ′ + K k ( z k − H x ^ k ) = x ^ k ′ + K k ( H x k + v k − H x ^ k ′ ) = K k H x k + ( I − K k H ) x k ′ + K k v k x ^ k \hat x_k x ^ k e k = x k − x ^ k = x k − ( K k H x k + ( I − K k H ) x k ′ + K k v k ) = ( I − K k H ) ( x k − x k ′ ) − K k v k e_k = x_k - \hat x_k =x_k -(K_kHx_k + (I - K_kH)x_k^{'} +K_kv_k) = (I - K_kH)(x_k - x_k^{'}) - K_kv_k e k = x k − x ^ k = x k − ( K k H x k + ( I − K k H ) x k ′ + K k v k ) = ( I − K k H ) ( x k − x k ′ ) − K k v k 將e k e_k e k P k = E ( e k e k T ) = E [ [ ( I − K k H ) ( x k − x k ′ ) − K k v k ] [ ( I − K k H ) ( x k − x k ′ ) − K k v k ] T ] = ( I − K k H ) E [ ( x k − x k ′ ) ( x k − x k ′ ) T ] ( I − K k H ) T − K k E [ v k v k T ] K k T \begin{array}{l}
P_k = E(e_ke_k^T) \\
= E[[(I - K_kH)(x_k - x_k^{'}) - K_kv_k][(I - K_kH)(x_k - x_k^{'}) - K_kv_k]^T] \\
=(I - K_kH)E[(x_k - x_k^{'})(x_k - x_k^{'})^T](I - K_kH)^T - K_kE[v_kv_k^T]K_k^T
\end{array} P k = E ( e k e k T ) = E [ [ ( I − K k H ) ( x k − x k ′ ) − K k v k ] [ ( I − K k H ) ( x k − x k ′ ) − K k v k ] T ] = ( I − K k H ) E [ ( x k − x k ′ ) ( x k − x k ′ ) T ] ( I − K k H ) T − K k E [ v k v k T ] K k T e k , P k e_k, P_k e k , P k
P k = ( I − K k H ) P k ′ ( I − K k H ) T − K k R K k T P_k =(I - K_kH)P_k^{'}(I - K_kH)^T - K_kRK_k^T P k = ( I − K k H ) P k ′ ( I − K k H ) T − K k R K k T P k ′ P_k^{'} P k ′ 這裏就得到協方差關係式，由於直接處理矩陣不好處理，所以，通過展開，再求跡（tr）然後求導，令導數等於零，求得極小值點，再回代入原方程。 P k = ( 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 + R ) K k T \begin{array}{l}
P_k =(I - K_kH)P_k^{'}(I - K_kH)^T - K_kRK_k^T \\
=P_k^{'}-K_kHP_k^{'}-P_k^{'}H^TK_k^T+K_k(HP_k^{'}H^T+R)K_k^{T}
\end{array} P k = ( 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 + R ) K k T T [ P k ] = T [ P k ′ ] − 2 T [ K k H P k ′ ] + T [ K k ( H P K ′ H T + R ) K k T ] T[P_k] = T[P_k^{'}] - 2T[K_kHP_k^{'}] + T[K_k(HP_K^{'}H^T+R)K_k^{T}] T [ P k ] = T [ P k ′ ] − 2 T [ K k H P k ′ ] + T [ K k ( H P K ′ H T + R ) K k T ] d T [ P k ] d K k = − 2 ( H P k ′ ) T + 2 K k ( H P k ′ H T + R ) = 0 \frac{dT[P_k]}{dK_k} = - 2(HP_k^{'})^T + 2K_k(HP_k^{'}H^T+R) = 0 d K k d T [ P k ] = − 2 ( H P k ′ ) T + 2 K k ( H P k ′ H T + R ) = 0 ⇒ 2 K k ( H P K ′ H T + R ) = 2 H P k ′ \Rightarrow 2K_k(HP_K^{'}H^T+R) = 2HP_k^{'} ⇒ 2 K k ( H P K ′ H T + R ) = 2 H P k ′ ⇒ K k = ( H P k ′ ) T ( H P k ′ H T + R ) − 1 \Rightarrow K_k = (HP_k^{'})^T(HP_k^{'}H^T+R)^{-1} ⇒ K k = ( H P k ′ ) T ( H P k ′ H T + R ) − 1 將K k K_k K k P k P_k P k K k K_k K k ⇒ P k = P k ′ − K k H P k ′ − 2 ( H P k ′ ) T K k T \Rightarrow P_k = P_k^{'}-K_kHP_k^{'}-2(HP_k^{'})^TK_k^{T} ⇒ P k = P k ′ − K k H P k ′ − 2 ( H P k ′ ) T K k T ⇒ P k = ( I − K k H ) P k ′ \Rightarrow P_k = (I -K_kH )P_k^{'} ⇒ P k = ( I − K k H ) P k ′
推到這裏，我們已經得到卡爾曼濾波方程中的兩個關係式，但是P k ′ P_k^{'} P k ′ ∵ \because ∵ ∴ \therefore ∴ x k + 1 ′ = Φ x ^ k x_{k+1}^{'} =\Phi \hat x_k x k + 1 ′ = Φ x ^ k ∵ P k + 1 ′ = E [ e k + 1 ′ e k + 1 T ′ ] \because P_{k+1}^{'} = E[e_{k+1}^{'}e_{k+1}^{T'}] ∵ P k + 1 ′ = E [ e k + 1 ′ e k + 1 T ′ ] ∵ e k + 1 ′ = x k + 1 − x k + 1 ′ = Φ x k + w k − Φ x ^ k = Φ e k + w k \because e_{k+1}^{'} = x_{k+1} - x_{k+1}^{'} = \Phi x_k + w_k - \Phi \hat x_k = \Phi e_k + w_k ∵ e k + 1 ′ = x k + 1 − x k + 1 ′ = Φ x k + w k − Φ x ^ k = Φ e k + w k ∴ P k + 1 ′ = E [ ( Φ e k + w k ) ( Φ e k + w k ) T ] = Φ E [ e k e k T ] Φ T + E ( w k w k T ) = Φ P k Φ T + Q \begin{array}{l}
\therefore P_{k+1}^{'} = E[(\Phi e_k + w_k )(\Phi e_k + w_k )^T] \\
= \Phi E[e_ke_k^T]\Phi ^T + E(w_kw_k^T) \\
= \Phi P_k \Phi ^T + Q
\end{array} ∴ P k + 1 ′ = E [ ( Φ e k + w k ) ( Φ e k + w k ) T ] = Φ E [ e k e k T ] Φ T + E ( w k w k T ) = Φ P k Φ T + Q ⇒ P k + 1 ′ = Φ P k Φ T + Q \Rightarrow P_{k+1}^{'} = \Phi P_k \Phi ^T + Q ⇒ P k + 1 ′ = Φ P k Φ T + Q P k ′ P_k^{'} P k ′
至此，推導出了卡爾曼濾波方程當中的四個 ，還缺少一個最優估計方程，也就是已知條件的最後一個。
爲什麼有這部分？已知條件的最後一個是直接給出的，這裏解釋關係式是怎麼來的。χ 2 = ∑ i = 1 k ( ( z k − h ( a , x ) 2 σ i 2 ) \chi^2 = \sum \limits_{i=1}^{k} (\frac{(z_k -h(a,x)^2}{\sigma _i^2}) χ 2 = i = 1 ∑ k ( σ i 2 ( z k − h ( a , x ) 2 ) z i 測 量 值 （ 相 當 於 z k ） ， h ( a , x ) 表 示 參 數 爲 x 的 模 型 h ( 相 當 於 觀 測 方 程 模 型 ) , 在 σ i 是 和 測 量 值 有 關 的 誤 差 ( 相 當 於 v k ) z_i 測量值（相當於z_k），h(a, x) 表示參數爲x的模型h(相當於觀測方程模型),在\sigma _i是和測量值有關的誤差(相當於v_k) z i 測 量 值 （ 相 當 於 z k ） ， h ( a , x ) 表 示 參 數 爲 x 的 模 型 h ( 相 當 於 觀 測 方 程 模 型 ) , 在 σ i 是 和 測 量 值 有 關 的 誤 差 ( 相 當 於 v k ) χ 2 = ∑ i = 1 k ( 1 σ i σ i ( z k − h ( a i , x ) 2 ) \chi^2 = \sum \limits_{i=1}^{k} (\frac{1}{\sigma _i\sigma _i}(z_k -h(a_i,x)^2) χ 2 = i = 1 ∑ k ( σ i σ i 1 ( z k − h ( a i , x ) 2 ) R R R R R R χ k 2 = [ z k − h ( a , x k ) ] R − 1 [ z k − h ( a i , x ] T \chi_k^2 = [z_k -h(a,x_k)]R^{-1}[z_k -h(a_i,x]^T χ k 2 = [ z k − h ( a , x k ) ] R − 1 [ z k − h ( a i , x ] T χ k − 1 2 = ∑ k κ ( z k − h ( a , x ) R − 1 ( z k − h ( a , x ) T ≈ χ m i n 2 + ( x k − 1 − x ^ k − 1 ) R − 1 ( x k − 1 − x ^ k − 1 ) T \begin{array}{cc}
\chi_{k-1}^2 = \sum \limits_{k}^{\kappa} (z_k -h(a,x)R^{-1}(z_k -h(a,x)^T \\
\approx \chi_{min}^2 + (x_{k-1} -\hat x_{k-1})R^{-1}(x_{k-1} -\hat x_{k-1})^T
\end{array} χ k − 1 2 = k ∑ κ ( z k − h ( a , x ) R − 1 ( z k − h ( a , x ) T ≈ χ m i n 2 + ( x k − 1 − x ^ k − 1 ) R − 1 ( x k − 1 − x ^ k − 1 ) T χ 2 = χ k 2 + χ k − 1 2 = [ z k − h ( a , x k ) ] R − 1 [ z k − h ( a i , x ) ] T + χ m i n 2 + ( x k − 1 − x ^ k − 1 ) R − 1 ( x k − 1 − x ^ k − 1 ) T \begin{array}{cc}
\chi^2 = \chi_k^2 + \chi_{k-1}^2
=[z_k -h(a,x_k)]R^{-1}[z_k -h(a_i,x)]^T + \chi_{min}^2 + (x_{k-1} -\hat x_{k-1})R^{-1}(x_{k-1} -\hat x_{k-1})^T
\end{array} χ 2 = χ k 2 + χ k − 1 2 = [ z k − h ( a , x k ) ] R − 1 [ z k − h ( a i , x ) ] T + χ m i n 2 + ( x k − 1 − x ^ k − 1 ) R − 1 ( x k − 1 − x ^ k − 1 ) T h ( a , x k ) = h ( a , ( x ^ k + Δ x ) ) w h e r e x k = x ^ k + Δ x h(a,x_k) = h(a,(\hat x_k + \Delta x)) where x_k = \hat x_k + \Delta x h ( a , x k ) = h ( a , ( x ^ k + Δ x ) ) w h e r e x k = x ^ k + Δ x ⇒ h ( a , ( x ^ k + Δ x ) ) = h ( x ^ ) + Δ x ∇ x h ( x ^ ) \Rightarrow h(a,(\hat x_k + \Delta x)) = h(\hat x) + \Delta x \nabla _x h(\hat x) ⇒ h ( a , ( x ^ k + Δ x ) ) = h ( x ^ ) + Δ x ∇ x h ( x ^ ) h 和 Δ x k h和\Delta x_k h 和 Δ x k d ( χ 2 ) d x = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 ∇ x h ( a , x k ) T R − 1 [ z k − h ( a , x k ) ] = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 ∇ x h ( a , x k ) T R − 1 [ z k − h ( a , x ^ ) + ( x k − x ^ k ) ∇ x h ( a , x ^ k ) ] \begin{array}{cc}
\frac {d(\chi ^2)}{dx} = 2P_{k-1}^{'-1}(x_{k-1}-\hat x_{k-1}) - 2\nabla _xh(a,x_k)^TR^{-1}[z_k-h(a,x_k) ] \\
=2P_{k-1}^{'-1}(x_{k-1}-\hat x_{k-1}) - 2\nabla xh(a,x_k)^TR^{-1}[z_k-h(a,\hat x) + (x_k- \hat x_k) \nabla _x h(a,\hat x_k) ]
\end{array} d x d ( χ 2 ) = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 ∇ x h ( a , x k ) T R − 1 [ z k − h ( a , x k ) ] = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 ∇ x h ( a , x k ) T R − 1 [ z k − h ( a , x ^ ) + ( x k − x ^ k ) ∇ x h ( a , x ^ k ) ] ∇ x h ( a , x k ) = ∇ x h ( a , x ^ k ) = H \nabla _x h(a,x_k) = \nabla _x h(a,\hat x_k) = H ∇ x h ( a , x k ) = ∇ x h ( a , x ^ k ) = H d ( χ 2 ) d x = 0 = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 H T R − 1 [ z k − h ( a , x ^ ) + ( x k − x ^ k ) H ] = 2 ( P k − 1 ′ − 1 − H T R − 1 H ) ( x k − x ^ k ) − 2 H T R − 1 [ z k − h ( a , x ^ ) ] \begin{array}{cc}
\frac {d(\chi ^2)}{dx} =0 = 2P_{k-1}^{'-1}(x_{k-1}-\hat x_{k-1}) - 2H^TR^{-1}[z_k-h(a,\hat x) + (x_k- \hat x_k) H ] \\
=2(P_{k-1}^{'-1} - H^TR^{-1}H)(x_k- \hat x_k) - 2H^TR^{-1}[z_k-h(a,\hat x)]
\end{array} d x d ( χ 2 ) = 0 = 2 P k − 1 ′ − 1 ( x k − 1 − x ^ k − 1 ) − 2 H T R − 1 [ z k − h ( a , x ^ ) + ( x k − x ^ k ) H ] = 2 ( P k − 1 ′ − 1 − H T R − 1 H ) ( x k − x ^ k ) − 2 H T R − 1 [ z k − h ( a , x ^ ) ] ( x k − x ^ k ) = ∇ x = ( P k − 1 ′ − 1 − H T R − 1 H ) ( x k − x ^ k ) − 1 H T R − 1 [ z k − h ( a , x ^ ) ] (x_k- \hat x_k) = \nabla x =(P_{k-1}^{'-1} - H^TR^{-1}H)(x_k- \hat x_k)^{-1}H^TR^{-1}[z_k-h(a,\hat x)] ( x k − x ^ k ) = ∇ x = ( P k − 1 ′ − 1 − H T R − 1 H ) ( x k − x ^ k ) − 1 H T R − 1 [ z k − h ( a , x ^ ) ] K k = ( P k − 1 ′ − 1 − H T R − 1 H ) H T R − 1 K_k =(P_{k-1}^{'-1} - H^TR^{-1}H)H^TR^{-1} K k = ( P k − 1 ′ − 1 − H T R − 1 H ) H T R − 1 x k = x ^ k + ∇ x = x ^ k + K k [ z k − h ( a , x ^ ) ] x_k = \hat x_k + \nabla x = \hat x_k + K_k[z_k-h(a,\hat x)] x k = x ^ k + ∇ x = x ^ k + K k [ z k − h ( a , x ^ ) ] 已知，我們推出了一個線性問題可以表達爲y = a x + b y = ax + b y = a x + b y = a x + b y = ax + b y = a x + b x k = x ^ k + K k [ z k − h ( a , x ^ ) ] x_k = \hat x_k + K_k[z_k-h(a,\hat x)] x k = x ^ k + K k [ z k − h ( a , x ^ ) ] x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) \hat x_k =\hat x_k^{'} + K_k(z_k - H\hat x_k^{'}) x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) 注意：這裏的K k K_k K k K k 值 K_k值 K k 值 K k K_k K k
卡爾曼濾波的核心點就是狀態、協方差，一共分爲5步，反覆迭代進行。
五步方程：
(1) X ^ k ∣ k − 1 = F k − 1 X ^ k − 1 + G k − 1 u k − 1 \hat{X}_{k|k-1} = F_{k-1}\hat{X}_{k-1} + G_{k-1}u_{k-1} X ^ k ∣ k − 1 = F k − 1 X ^ k − 1 + G k − 1 u k − 1
(2) P k ∣ k − 1 = F k − 1 P k − 1 F k − 1 T + G k − 1 u k − 1 P_{k|k-1} = F_{k-1}P_{k-1}F_{k-1}^T + G_{k-1}u_{k-1} P k ∣ k − 1 = F k − 1 P k − 1 F k − 1 T + G k − 1 u k − 1
(3) K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_k = P_{k|k-1}H_{k}^T(H_{k}P_{k|k-1}H_{k}^T+R_{k})^{-1} K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1
(4) X ^ k = X ^ x ∣ k − 1 + K k ( z k − H k X ^ x ∣ k − 1 ) \hat{X}_{k} = \hat{X}_{x|k-1} + K_{k}(z_{k}-H_{k}\hat{X}_{x|k-1}) X ^ k = X ^ x ∣ k − 1 + K k ( z k − H k X ^ x ∣ k − 1 )
(5) P k = ( I − K k H k ) P k ∣ k − 1 P_{k} = (I-K_kH_k) P_{k|k-1} P k = ( I − K k H k ) P k ∣ k − 1
爲了更好地理解卡爾曼濾波，下面引入一個簡單的實例來進行說明，牛頓力學模型：
假設：小球 做自由落體運動，忽略阻力，
目標：濾波器KF由初始位置的不確定性來估計小球的位置
符號說明：
符號
h h h g g g t t t Δ t \Delta t Δ t k k k k − 1 k-1 k − 1
說明
高度
重力加速度
時刻
微小時間長度
當前計數
上一個計數
根據基本的牛頓力學定律，經過Δ t \Delta t Δ t h ˙ ( t ) = h ˙ ( t − Δ t ) − g Δ t \dot h(t) = \dot h(t - \Delta t)-g\Delta t h ˙ ( t ) = h ˙ ( t − Δ t ) − g Δ t h ( t ) = h ( t − Δ t ) + h ˙ ( t − Δ t ) Δ t − 1 2 g ( Δ t ) 2 h(t)=h(t-\Delta t)+\dot h(t-\Delta t)\Delta t-\frac{1}{2}g(\Delta t)^2 h ( t ) = h ( t − Δ t ) + h ˙ ( t − Δ t ) Δ t − 2 1 g ( Δ t ) 2
以上是該問題的連續性表述，將該問題離散化（這個離散化的思路有點意思），假設t = k Δ t t=k\Delta t t = k Δ t h ˙ ( k Δ t ) = h ˙ ( k Δ t − Δ t ) − g Δ t \dot h(k\Delta t) = \dot h(k\Delta t - \Delta t)-g\Delta t h ˙ ( k Δ t ) = h ˙ ( k Δ t − Δ t ) − g Δ t h ( k Δ t ) = h ( k Δ t − Δ t ) + h ˙ ( k Δ t − Δ t ) Δ t − 1 2 g ( Δ t ) 2 h(k\Delta t)=h(k\Delta t-\Delta t)+\dot h(k\Delta t-\Delta t)\Delta t-\frac{1}{2}g(\Delta t)^2 h ( k Δ t ) = h ( k Δ t − Δ t ) + h ˙ ( k Δ t − Δ t ) Δ t − 2 1 g ( Δ t ) 2
變量形式的替換不一一說明，按照位置對應即可。 h ˙ k = h ˙ k − 1 − g Δ t \dot h_k = \dot h_{k-1}-g\Delta t h ˙ k = h ˙ k − 1 − g Δ t h k = h k − 1 + h ˙ k − 1 Δ t − 1 2 g ( Δ t ) 2 h_k=h_{k-1}+\dot h_{k-1}\Delta t-\frac{1}{2}g(\Delta t)^2 h k = h k − 1 + h ˙ k − 1 Δ t − 2 1 g ( Δ t ) 2
轉換成矩陣表示：[ h k h ˙ k ] = [ 1 Δ t 0 1 ] [ h k h ˙ k ] + [ − 1 2 ( Δ t ) 2 − Δ t ] g \left[\begin{matrix}h_k \\\dot h_k \\\end{matrix} \right]= \left[\begin{matrix}1 & \Delta t \\0 &1 \\\end{matrix} \right]\left[\begin{matrix}h_k \\\dot h_k \\\end{matrix} \right] + \left[\begin{matrix} -\frac{1}{2}(\Delta t)^2 \\ -\Delta t \\\end{matrix} \right]g [ h k h ˙ k ] = [ 1 0 Δ t 1 ] [ h k h ˙ k ] + [ − 2 1 ( Δ t ) 2 − Δ t ] g
將對應位置的變量換一種表達形式：X k = F k − 1 X k 1 + G k − 1 u k − 1 （ 狀 態 方 程 ） X_k = F_{k-1}X_{k_1} + G_{k-1}u_{k-1}（狀態方程） X k = F k − 1 X k 1 + G k − 1 u k − 1 （ 狀 態 方 程 ）
此外，還需建立一個觀測方程： 觀測值爲輸出狀態值加上觀測噪聲y k = [ 1 0 ] h k + v k y_k = \left[\begin{matrix}1 & 0 \\\end{matrix}\right] h_k + v_k y k = [ 1 0 ] h k + v k y k = H k X k + v k y_k = H_kX_k + v_k y k = H k X k + v k
這個時候，回到卡爾曼濾波問題，思考一下想要使用卡爾曼濾波需要哪些變量？
圖1是估計值與真實值之間的差值變化情況，隨着時間推移，估計值越來越接近真實值，最終在真實值附近波動。圖2是測量值、真實值、估計值的一個情況對比。Python代碼部分將4中的實例執行了一次。
import numpy as np
import matplotlib. pyplot as plt
iterations = 1000
delta_t = 0.001
time = np. linspace( 0 , 1 , iterations)
time = np. mat( time)
g = 9.80665
z = [ 100 - 0.5 * g* ( delta_t* i) ** 2 for i in range ( iterations) ]
z_watch = np. mat( z)
noise = np. round ( np. random. normal( 0 , 1 , iterations) , 2 )
noise_mat = np. mat( noise)
z_mat = z_watch + noise_mat
y = [ ]
x_mat = np. mat( [ [ 105 , ] , [ 0 , ] ] )
p_mat = np. mat( [ [ 10 , 0 ] , [ 0 , 0.01 ] ] )
f_mat = np. mat( [ [ 1 , delta_t] , [ 0 , 1 ] ] )
g_mat = np. mat( [ [ - 0.5 * delta_t** 2 ] , [ - delta_t] ] )
q_mat = np. mat( [ [ 0.0 , 0 ] , [ 0 , 0.0 ] ] )
h_mat = np. mat( [ 1 , 0 ] )
r_mat = np. mat( [ 4 ] )
for i in range ( iterations) :
x_predict = f_mat * x_mat + g_mat * g
p_predict = f_mat * p_mat * f_mat. T + q_mat
kalman = p_predict * h_mat. T / ( h_mat * p_predict * h_mat. T + r_mat)
x_mat = x_predict + kalman * ( z_mat[ 0 , i] - h_mat * x_predict)
p_mat = ( np. eye( 2 ) - kalman * h_mat) * p_predict
y. append( x_mat[ 0 ] . tolist( ) [ 0 ] [ 0 ] )
y = np. mat( y)
error = y - z_watch
y = y. A
error = error. A
time = time. A
z_mat = z_mat. A
z_watch = z_watch. A
plt. plot( time[ 0 , : ] , z_mat[ 0 , : ] , label = 'Measured' )
plt. plot( time[ 0 , : ] , z_watch[ 0 , : ] , 'g' , label= 'True' )
plt. plot( time[ 0 , : ] , y[ 0 , : ] , 'r' , label = 'Estimated' )
plt. xlabel( 'h(m)' )
plt. ylabel( 'time(s)' )
plt. legend( loc= 'lower right' )
plt. figure( 2 )
plt. plot( time[ 0 , : ] , error[ 0 , : ] , label = 'Errors' )
plt. ylabel( 'error(m)' )
plt. xlabel( 'time(s)' )
plt. legend( loc= 'lower right' )
plt. show( )
