本文在参考文献的基础上添加了自己的理解，若有不当之处，敬请指正 卡尔曼滤波以前接触过，但是没有仔细推导，这次参考文献仔细推导实现，也是第一次完全的通过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( )
