卡尔曼滤波(Kalman Filter)概念介绍及详细公式推导

卡尔曼滤波

卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

​ 卡尔曼滤波的算法是二步骤的程序。在估计步骤中，卡尔曼滤波会产生有关目前状态的估计，其中也包括不确定性。只要观察到下一个量测（其中一定含有某种程度的误差，包括随机噪声）。会通过加权平均来更新估计值，而确定性越高的量测加权比重也越高。算法是迭代的，可以在实时控制系统中执行，只需要目前的输入量测、以往的计算值以及其不确定性矩阵，不需要其他以往的资讯。

应用实例

​ 卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、机器视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。

​ 例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。

基本动态系统模型

​ 卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

​ 为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵$F_k$, $H_k$, $Q_k$, $R_k$，有时也需要定义$B_k$，如下。

​ 卡尔曼滤波模型假设$k+1$时刻的真实状态是从$k$时刻的状态演化而来，符合状态方程：
$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$
其中

• $F_k$是作用在$x_{k-1}$上的状态变换模型（矩阵或向量）。
• $B_k$是作用在控制器向量$u_k$上的输入-控制模型。
• $w_k$是过程噪声，并假定其符合均值为零，协方差矩阵为$Q_k$的多元正态分布。

​ $w_k\sim N(0,Q_k)$

其中 c o v { w ( k ) } = Q ( k ) cov\{w(k)\}=Q(k) cov{w(k)}=Q(k)表示协方差矩阵。

​ 时刻k，对真实状态$x_k$的一个测量$z_k$满足下式，即观测方程：
$$z_k=H_k{x}_k+v_k$$
其中$H_k$是观测模型，它把真实状态空间映射成观测空间，$v_k$是观测噪声，其均值为零，协方差矩阵为$R_k$且服从正态分布。
$$v_k\sim N(0,R_k)$$
其中 c o v { v ( k ) } = R ( k ) cov\{v(k)\}=R(k) cov{v(k)}=R(k)表示协方差矩阵。

​ 初始状态以及每一时刻的噪声 { x 0 , w 1 , … , w k , v 1 , … , v k } \{x_0,w_1,…,w_k,v_1,…,v_k\} {x0​,w1​,…,wk​,v1​,…,vk​}都认为是互相独立的。

算法逻辑

​ 卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

​ 卡尔曼滤波主要分为预测和更新两部分，需要构建了系统的状态方程和观测方程，且已知系统的初始状态。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

​ 卡尔曼滤波器的状态由以下两个变量表示：

• ${\hat{X}}_{k|k}=E(X_k|Y_1,Y_2,\dots ,Y_k)$表示在时刻k的状态的估计。
• ${\hat{X}}_{k|k-1}=E(X_{k-1}|Y_1,Y_2,\dots ,Y_{k-1})$表示已知过去k-1个时刻的状态，对k时刻状态的预测。
• ${\hat{P}}_{k|k}$，后验估计误差协方差矩阵，度量估计值的精确程度。

​ 则预测和更新(Prediction-Correction)过程如下：
$${\hat{X}}_{k-1|k-1}\stackrel{Prediction}{\to }{\hat{X}}_{k|k-1}\stackrel{Correction}{\to }{\hat{X}}_{k|k}$$

1. 预测${\hat{X}}_{k-1|k-1}\Rightarrow {\hat{X}}_{k|k-1}$

​ 预测步骤中，根据上一时刻的状态和控制量，预测当前时刻的状态。这个预测值是一个估计值，因为它还没有考虑当前时刻的观测值。预测值的误差协方差矩阵是通过上一时刻的误差协方差矩阵和系统噪声协方差矩阵计算得到的。
$$\{\begin{array}{l}{\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1}+B_k{u}_k\\ \\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k\end{array}$$

2. 更新${\hat{X}}_{n|n-1}\Rightarrow {\hat{X}}_{n|n}$

​ 更新步骤中，根据当前时刻的观测值和预测值，计算出当前时刻的状态估计值。这个估计值是一个更加准确的估计值，因为它已经考虑了当前时刻的观测值。状态估计值的误差协方差矩阵是通过预测步骤中计算得到的误差协方差矩阵、观测噪声协方差矩阵和卡尔曼增益计算得到的。
$$\{\begin{array}{l}{K}_k=P_{k|k-1}{H}_k^T(H_k{P}_{k|k-1}{H}_k^T+R_k{)}^{-1}\\ \\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H_k{\hat{x}}_{k|k-1})\\ \\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}$$
​ 以上五个公式为卡尔曼滤波的核心公式。更新步骤更加简洁便于理解的形式为，首先计算以下三个量：
$$\{\begin{array}{l}{\hat{y}}_k=z_k-H_k{\hat{x}}_{k|k-1}(测量残差)\\ \\ S_k=H_k{P}_{k|k}{H}_k^T+R_k(测量残差协方差)\\ \\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}(最优卡尔曼增益)\end{array}$$
​ 然后用它们来更新滤波器变量：
$$\{\begin{array}{l}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\hat{y}}_k(更新的状态估计)\\ \\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}(更新的协方差估计)\end{array}$$
使用上述公式计算$P_{k|k}$仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。

3. 卡尔曼滤波过程

公式推导

1. 符号定义

状态方程
$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$
其中$u_k$为输入信号，$x_{k-1}$为状态变量，$w_k$表示过程噪声。其中 c o v { w ( k ) } = Q ( k ) cov\{w(k)\}=Q(k) cov{w(k)}=Q(k)。

观测方程
$$z_k=H_k{x}_k+v_k$$
其中$z_k$为观测变量，$v_k$表示观测噪声。其中 c o v { v ( k ) } = R ( k ) cov\{v(k)\}=R(k) cov{v(k)}=R(k)。

状态估计误差
$${\tilde{x}}_{k|k-1}=x_k-{\hat{x}}_{k|k-1}$$
其中${\tilde{x}}_{k|k-1}$表示估计在第$k-1$时刻对$x_k$的估计。在第k-1时刻的时候，对k时刻的任何值，都只能是估计（预测未来值）。

状态估计方程
$${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1}+B_k{u}_k$$
观测估计误差
$${\tilde{e}}_k=z_k-{\hat{z}}_{k|k-1}$$
观测估计方程
$${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$$
基于上述公式，可定义两个重要的误差协方差矩阵：

状态误差协方差矩阵
P k ∣ k − 1 = c o v { x ~ k ∣ k − 1 } P_{k|k-1}=cov\{\tilde{x}_{k|k-1}\} Pk∣k−1​=cov{x~k∣k−1​}

观测误差协方差矩阵
S K = c o v { e ~ k } S_{K}=cov\{\tilde{e}_{k}\} SK​=cov{e~k​}
卡尔曼滤波的目的是得到一个基于误差能够不断修正的迭代估计表达式，其具体形式应该如下：
$${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\tilde{e}}_k$$
即基于误差对预测值进行修正。这里的$K_k$就是卡尔曼增益。因此，我们需要推导出所用变量的迭代式，并求得卡尔曼增益。

2. 推导

推导误差协方差矩阵

状态误差协方差矩阵：

P k ∣ k − 1 = c o v { x k − x ^ k ∣ k − 1 } = c o v { F k x k − 1 + B k u k + w k − F k x ^ k − 1 − B k u k } = c o v { F k ( x k − 1 − x ^ k − 1 ) + w k } = c o v { F k ( x k − 1 − x ^ k − 1 ) } + c o v { w k } = F k c o v { x k − 1 − x ^ k − 1 } F k T + Q k = F k P k ∣ k F k T + Q k \begin{align}P_{k|k-1}&=cov\{x_k-\hat{x}_{k|k-1}\}\\ &=cov\{F_kx_{k-1}+B_ku_k+w_k-F_k\hat{x}_{k-1}-B_{k}u_{k}\}\\ &=cov\{F_k(x_{k-1}-\hat{x}_{k-1})+w_k\}\\ &=cov\{F_k(x_{k-1}-\hat{x}_{k-1})\}+cov\{w_k\}\\ &=F_kcov\{x_{k-1}-\hat{x}_{k-1}\}F^T_k+Q_k\\ &=F_kP_{k|k}F^T_k+Q_k \end{align} Pk∣k−1​​=cov{xk​−x^k∣k−1​}=cov{Fk​xk−1​+Bk​uk​+wk​−Fk​x^k−1​−Bk​uk​}=cov{Fk​(xk−1​−x^k−1​)+wk​}=cov{Fk​(xk−1​−x^k−1​)}+cov{wk​}=Fk​cov{xk−1​−x^k−1​}FkT​+Qk​=Fk​Pk∣k​FkT​+Qk​​​
这里我们得到了$P_{k|k}$ 到$P_{k|k-1}$的一个递推形式，$P_{k|k}$的标记形式是为了更方便后面的推导，即为了得到
$$\begin{array}{r}{P}_{k-1|k-1}\to P_{k|k-1}\to P_{k|k}\end{array}$$
同理可得观测误差协方差矩阵的递推形式：
$$\begin{array}{r}{S}_k=H_k{P}_{k|k-1}{H}_k^T+R_k\end{array}$$
能够发现，$S_k$和$S_{k-1}$没有直接的联系，这就是为什么不把$S_k$写成$S_{k|k-1}$的形式。

因此，目前还缺少$P_{k|k-1}\to P_{k|k}$的递推式。

推导后验协方差矩阵

由定义可知，误差协方差$P_{k|k}$的定义如下，代入上述定义逐行推导化简即可。
P k ∣ k = c o v { x ~ k ∣ k } = c o v { x k − x ^ k ∣ k } = c o v { x k − ( x ^ k ∣ k − 1 + K k e ~ k ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( z k − z ^ k ∣ k − 1 ) ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( H k x k + v k − H k x ^ k ∣ k − 1 ) ) } = c o v { ( x k − x ^ k ∣ k − 1 ) − K k H k ( x k − x ^ k ∣ k − 1 ) − K k v k } = c o v { ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k } = c o v { ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) } + c o v { K k v 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 \begin{align} P_{k|k}&=cov\{\tilde{x}_{k|k}\} \\&=cov\{x_k-\hat{x}_{k|k}\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k\tilde{e}_k)\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(z_{k}-\hat{z}_{k|k-1}))\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(z_{k}-H_k\hat{x}_{k|k-1}))\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(H_kx_k+v_k-H_k\hat{x}_{k|k-1}))\} \\&=cov\{(x_k-\hat{x}_{k|k-1})-K_kH_k(x_k-\hat{x}_{k|k-1})-K_kv_k\} \\&=cov\{(I-K_kH_k)(x_k-\hat{x}_{k|k-1})-K_kv_k\} \\&=cov\{(I-K_kH_k)(x_k-\hat{x}_{k|k-1})\}+cov\{K_kv_k\} \\&=(I-K_kH_k)cov\{(x_k-\hat{x}_{k|k-1})\}(I-K_kH_k)^T+K_kcov\{v_k\}K_k^T \\&=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T \end{align} Pk∣k​​=cov{x~k∣k​}=cov{xk​−x^k∣k​}=cov{xk​−(x^k∣k−1​+Kk​e~k​)}=cov{xk​−(x^k∣k−1​+Kk​(zk​−z^k∣k−1​))}=cov{xk​−(x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​))}=cov{xk​−(x^k∣k−1​+Kk​(Hk​xk​+vk​−Hk​x^k∣k−1​))}=cov{(xk​−x^k∣k−1​)−Kk​Hk​(xk​−x^k∣k−1​)−Kk​vk​}=cov{(I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​}=cov{(I−Kk​Hk​)(xk​−x^k∣k−1​)}+cov{Kk​vk​}=(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​​​
其中，测量误差$v_k$与其他项非相关，因此可以单独提出来。这一公式对任何卡尔曼增益$K_k$都成立，如果$K_k$是最优卡尔曼增益，则可以进一步简化。

最优卡尔曼增益的推导

卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计是$x_k-{\hat{x}}_{k|k}$

最小化这个矢量幅度平方的期望值$E[|X_k-{\hat{x}}_{k|k}{|}^2]$，这等同于最小化后验估计协方差矩阵$P_{k|k}$的迹。将上面方程中的项展开、抵消得到：
$$\begin{array}{rl}{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\\ & =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+R_k)K_k^T\\ & =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k{S}_k{K}_k^T\end{array}$$
当矩阵导数为0的时候得到$P_{k|k}$的迹（trace）的最小值：
$$\begin{array}{r}\frac{\mathrm{d}tr(P_{k|k})}{\mathrm{d}{K}_k}=-2(H_k{P}_{k|k-1}{)}^T+2K_k{S}_k=0\end{array}$$
此处须用到一个常用的式子，如下：[参考1][参考2][参考3]
$$\begin{array}{r}\frac{\mathrm{d}tr(BAC)}{\mathrm{d}A}=B^T{C}^T\end{array}$$
则可解得卡尔曼增益：
$$\begin{array}{r}{K}_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\end{array}$$
这个增益称为最优卡尔曼增益，在使用时得到最小均方误差。

后验误差协方差公式的化简

在最优卡尔曼增益公式两侧都乘以$S_k{K}_k^T$得到
$$\begin{array}{r}{K}_k{S}_k{K}_k^T=P_{k|k-1}{H}_k^T{K}_k^T\end{array}$$
根据后验误差协方差展开公式，有
$$\begin{array}{rl}{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{S}_k{K}_k^T\\ & =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}\end{array}$$
实际中由于计算简单，往往使用这个公式，但需要注意该公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。

参考文献

An Introduction to the Kalman Filter

可能是讲解最清楚的Kalman filter

【EKF】卡尔曼滤波原理

卡尔曼滤波wiki百科

卡尔曼滤波最完整公式推导

矩阵的迹(Trace)及相关性质证明

迹-wiki百科

【机器学习】汇总详解：矩阵的迹以及迹对矩阵求导