Kalman filter公式推导

Kalman滤波是一种基于测量结果更新预测值，以获得对系统状态的更精准估计的算法。下图给出了Kalman滤波算法的基本步骤（摘自Wikipedia），本文将对Kalman filter算法中所涉及到的几条公式进行推导。

如假定所研究的系统为时序离散动力学体系，那么 k k k时刻系统状态 x k x_{k} xk​应由下式决定：

x k = F k x k − 1 + B k u k + w k                             ( 1 ) x_{k} = F_{k} x_{k - 1} + B_{k} u_{k} + w_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~ (1) xk​=Fk​xk−1​+Bk​uk​+wk​                           (1)

其中， F k F_{k} Fk​为系统状态转移矩阵， B k B_{k} Bk​则为控制输入矩阵， u k u_{k} uk​是系统的输入向量， w k w_{k} wk​则表示过程噪声( w k ∼ N ( 0 , Q k ) w_{k} \sim \mathscr{N} (0, Q_{k}) wk​∼N(0,Qk​))。

实际上，我们所得到的系统状态应被表示为 z k z_{k} zk​，它其实是由观测者对体系的测量所获得的结果，即

z k = H k x k + v k                             ( 2 ) z_{k} = H_{k} x_{k} + v_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~ (2) zk​=Hk​xk​+vk​                           (2)

其中， H k H_{k} Hk​表示状态空间向观测空间映射的状态观测矩阵， v k v_{k} vk​则为测量噪声( v k ∼ N ( 0 , R k ) v_{k} \sim \mathscr{N} (0, R_{k}) vk​∼N(0,Rk​))。

一般来说，如系统的状态演化符合Markov过程，那么基于 k k k时刻之前的状态，我们可以对该时刻的系统状态进行估计，由式(1)可得：

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k                        ( 3 ) \hat{x}_{k | k - 1} = F_{k} \hat{x}_{k-1 | k-1} + B_{k} u_{k}~~~~~~~~~~~~~~~~~~~~~~(3) x^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​                      (3)

其中， x ^ k ∣ k − 1 \hat{x}_{k | k - 1} x^k∣k−1​为 k k k时刻的先验状态估计值， x ^ k − 1 ∣ k − 1 \hat{x}_{k - 1 | k - 1} x^k−1∣k−1​则为 k − 1 k - 1 k−1时刻的后验状态估计值。

由此，在系统状态估计过程中的模型预测误差可被定义为：

e r r k p r e d = x ^ k ∣ k − x ^ k ∣ k − 1 err^{pred}_{k} = \hat{x}_{k | k} - \hat{x}_{k | k-1} errkpred​=x^k∣k​−x^k∣k−1​

而根据式(2)，该过程中总的误差可被定义为：

e r r k a l l = e r r k p r e d + e r r k m e a s u r e = z k − H k x ^ k ∣ k − 1 err^{all}_{k} = err^{pred}_{k} + err^{measure}_{k} = z_{k} - H_{k} \hat{x}_{k | k - 1} errkall​=errkpred​+errkmeasure​=zk​−Hk​x^k∣k−1​

这样，我们将Kalman增益系数 K k K_{k} Kk​表示为 k k k时刻，模型预测误差在总误差中的比重，即

K k = e r r k p r e d e r r k a l l = x ^ k ∣ k − x ^ k ∣ k − 1 z k − H k x ^ k ∣ k − 1 K_{k} = \dfrac{err^{pred}_{k}}{err^{all}_{k}} = \dfrac{\hat{x}_{k | k} - \hat{x}_{k | k-1}}{z_{k} - H_{k} \hat{x}_{k | k - 1}} Kk​=errkall​errkpred​​=zk​−Hk​x^k∣k−1​x^k∣k​−x^k∣k−1​​

将式(2)代入上式可得：

x ^ k ∣ k = x ^ k ∣ k − 1 + K k [ H k ( x k − x ^ k ∣ k − 1 ) + v k ] \hat{x}_{k | k} = \hat{x}_{k | k - 1} + K_{k} [H_{k} (x_{k} - \hat{x}_{k | k - 1}) + v_{k}] x^k∣k​=x^k∣k−1​+Kk​[Hk​(xk​−x^k∣k−1​)+vk​]

由此可得：

x k − x ^ k ∣ k = ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k x_{k} - \hat{x}_{k | k} = (I - K_{k} H_{k}) (x_{k} - \hat{x}_{k | k - 1}) - K_{k} v_{k} xk​−x^k∣k​=(I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​

因此，系统的真实状态 x k x_{k} xk​与后验状态估计值 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​之间的协方差可以表示为：

P k ∣ k = c o v ( x k − x ^ k ∣ k ) = c o v [ ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k ] P_{k | k} = \rm{cov} (x_{k} - \hat{x}_{k | k}) = \rm{cov} [(I - K_{k} H_{k}) (x_{k} - \hat{x}_{k | k - 1}) - K_{k} v_{k}] Pk∣k​=cov(xk​−x^k∣k​)=cov[(I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​]

考虑到协方差矩阵的计算公式，上式可被改写为：

P k ∣ k = ( I − K k H k ) c o v ( x k − x ^ k ∣ k − 1 ) ( I − K k H k ) T + K k c o v ( v k ) K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k | k} = (I - K_{k} H_{k}) \rm{cov} (x_{k} - \hat{x}_{k | k - 1}) (I - K_{k} H_{k})^{T} + K_{k} \rm{cov} (v_{k}) K^{T}_{k} = (I - K_{k} H_{k}) P_{k | k - 1} (I - K_{k} H_{k})^{T} + K_{k} R_{k} K^{T}_{k} Pk∣k​=(I−Kk​Hk​)cov(xk​−x^k∣k−1​)(I−Kk​Hk​)T+Kk​cov(vk​)KkT​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​

展开上式，我们可得：

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k H k P k ∣ k − 1 H k T K k T + K k R k K k T                                   ( 4 ) P_{k | k} = P_{k | k - 1} - K_{k} H_{k} P_{k | k - 1} - P_{k | k - 1} H^{T}_{k} K^{T}_{k} + K_{k} H_{k} P_{k | k - 1} H^{T}_{k} K^{T}_{k} + K_{k} R_{k} K^{T}_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~(4) Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​HkT​KkT​+Kk​Hk​Pk∣k−1​HkT​KkT​+Kk​Rk​KkT​                                 (4)

显然，我们期望 x k = x ^ k ∣ k x_{k} = \hat{x}_{k | k} xk​=x^k∣k​，即协方差 P k ∣ k P_{k | k} Pk∣k​关于 K k K_{k} Kk​的导数为零：

∂ P k ∣ k ∂ K k = − 2 H k P k ∣ k − 1 + 2 ( H k P k ∣ k − 1 H k T + R k ) K k T = 0 ⟹ K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1     ( 5 ) \dfrac{\partial P_{k | k}}{\partial K_{k}} = - 2 H_{k} P_{k | k - 1} + 2 (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k}) K^{T}_{k} = 0 \Longrightarrow K_{k} = P_{k | k - 1} H^{T}_{k} (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k})^{-1}~~~(5) ∂Kk​∂Pk∣k​​=−2Hk​Pk∣k−1​+2(Hk​Pk∣k−1​HkT​+Rk​)KkT​=0⟹Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1   (5)

将式(5)代入式(4)，可以得到：

P k ∣ k = ( I − K k H k ) P k ∣ k − 1                                 ( 6 ) P_{k | k} = (I - K_{k} H_{k}) P_{k | k - 1}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~(6) Pk∣k​=(I−Kk​Hk​)Pk∣k−1​                               (6)

此外，将式(1)与式(3)的等号两边相减可得：

x k − x ^ k ∣ k − 1 = F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) + w k x_{k} - \hat{x}_{k | k - 1} = F_{k} (x_{k - 1} - \hat{x}_{k - 1 | k - 1}) + w_{k} xk​−x^k∣k−1​=Fk​(xk−1​−x^k−1∣k−1​)+wk​

于是，

P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) = c o v [ F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) ] + c o v ( w k ) = F k P k − 1 ∣ k − 1 F k T + Q k       ( 7 ) P_{k | k - 1} = \rm{cov} (x_{k} - \hat{x}_{k | k - 1}) = \rm{cov} [F_{k} (x_{k - 1} - \hat{x}_{k - 1 | k - 1})] + \rm{cov} (w_{k}) = F_{k} P_{k - 1 | k - 1} F^{T}_{k} + Q_{k}~~~~~(7) Pk∣k−1​=cov(xk​−x^k∣k−1​)=cov[Fk​(xk−1​−x^k−1∣k−1​)]+cov(wk​)=Fk​Pk−1∣k−1​FkT​+Qk​     (7)

至此，我们便得到了Kalman滤波算法中所涉及的各公式的推导过程。根据本文一开始引用的图，我们可以给出Kalman滤波算法的基本步骤：

（1）已知系统的状态转移矩阵 F k F_{k} Fk​、控制输入矩阵 B k B_{k} Bk​、输入向量 u k u_{k} uk​以及过程噪声的协方差矩阵 Q k Q_{k} Qk​，在对应于 k − 1 k - 1 k−1时刻的 P k − 1 ∣ k − 1 P_{k - 1 | k - 1} Pk−1∣k−1​和 x ^ k − 1 ∣ k − 1 \hat{x}_{k - 1 | k - 1} x^k−1∣k−1​的基础上，可获得对 k k k时刻的先验状态估计值：

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k \hat{x}_{k | k -1} = F_{k} \hat{x}_{k - 1 | k - 1} + B_{k} u_{k} x^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​

利用式(7)，我们可以得到：

P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k | k - 1} = F_{k} P_{k - 1 | k - 1} F^{T}_{k} + Q_{k} Pk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

（2）基于式(5)和式(6)，我们可以得到 k k k时刻的Kalman增益系数：

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^{T}_{k} (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k})^{-1} Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1

以及协方差

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k | k} = (I - K_{k} H_{k}) P_{k | k - 1} Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

（3）在获得测量值 z k z_{k} zk​后，后验状态估计值 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​可由Kalman增益系数的定义式算得：

x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat{x}_{k | k} = \hat{x}_{k | k - 1} + K_{k} (z_{k} - H_{k} \hat{x}_{k | k - 1}) x^k∣k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)

（4）由此，我们得到 k k k时刻对应的 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​和 P k ∣ k P_{k | k} Pk∣k​。重复上述(1)~(3)步，便可获得 k + 1 k + 1 k+1时刻的相应值。