卡尔曼滤波算法

---

1 基础知识

[1] 卡尔曼增益最后会变成定值吗？
[2] 如何通俗并尽可能详细解释卡尔曼滤波？
[3] 卡尔曼滤波增益综述报告
[4] Understanding the Basis of the Kalman Filter
[5] 奇异值的物理意义是什么？
[6] 矩阵的奇异值与特征值有什么相似之处与区别之处？
[7] 欧拉角与万向节死锁

---

2 卡尔曼滤波算法简介

  Kalman Filter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。广泛应用于包含Radar、计算机视觉等在内的工程应用领域，在控制理论和控制系统工程中也是一个非常重要的课题。本文介绍了卡尔曼滤波增益的由来，以及它在卡尔曼滤波理论中的作用，着重介绍了卡尔曼滤波增益的理论意义。由卡尔曼滤波增益可以更深入的理解卡尔曼滤波，把它更好地应用于实际中。
  
  1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems，在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态，甚至在即使并不知道模型的确切性质时，也能估计将来的状态。
  
  其基本思想是以最小均方误差为最佳估计准则，采用利用前一时刻信号与噪声的状态空间模型获取的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
  
  对于解决很大部分的问题，它是最优、效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航、控制、传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别、图像分割、图像边缘检测等等。

---

3 卡尔曼滤波直观描述

  Kalman Filter算法运用场景。问题描述：存在两个功能相同但性能不同的传感器，那么应该如何融合两个传感器的测量值。解决办法：加权平均，但是无法很好的确定加权平均参数。卡尔曼滤波算法就是解决该问题的方法。
  
  形式化描述：存在一个传感器和一个对应的模型，如何解决传感器测量值与模型计算值的加权平均融合？假设模型是简化的，即$x_{k+1}=f(w_{1}x_{k}+(1-w_{1})\hat x_{k})$，其中$x_{k}(s_{1},s_{2},...,s_{n})$，$s_{i}$是影响模型的状态变量，现假设状态分量受到的不确定因素都服从正态分布。在$k$时刻，模型估计值为$x_{k}$，传感器测量值为$\hat x_{k}$，Kalman Filter算法能够确定概率最大化下的综合值$\overline x_{k}=w_{1}x_{k}+(1-w_{1})\hat x_{k})$，即确定$w_{1}$，同时此时算出的综合值也服从正太分布。定义Kalman增益为$\overline x_{k}$的均值，定义步长$\Delta t=t_{k+1}-t_{k}$。
  
  假设存在2个传感器$S_{1}, S_{2}$，对应的测量值为$\hat x^{1},\hat x^{2}$，传感器测量噪音协防差为$\Sigma^{1}, \Sigma^{2}$，1个状态估计模型$M$(假设状态转移为一步转移)，对应的估计值为$x$，状态转移协防差矩阵为$\Sigma$。Kalman Filter算法求解步骤如下：
  
  （1）状态估计模型初始值为$x_{0}$
  （2）考虑第$k$步，状态估计模型估计值为$x_{k}$，测量值为$\hat x_{k}^{1},\hat x_{k}^{2}$，Kalman增益$\overline x_{k}$为估计值与测量值的融合。针对该问题，需要进行两次融合：传感器测量融合、传感器测量与状态估计模型融合，融合过程中的加权参数由彼此的协防差确定。在融合值概率最大情况下求得Kalman增益，同时融合状态也服从高斯分布。
  （3）进入$k+1$步，利用状态估计模型求$x_{k+1}$，即$x_{k+1}=f(\overline x_{k})$。

---

4 卡尔曼滤波算法

  状态转移方程或离散随机差分方程如下：

$$x_{k}=A_{n \times n}x_{k-1}+B_{n}u_{k-1}+w_{k-1} \tag{1}$$
  测量方程如下：
$$z_{k}=H_{m \times n}x_{k}+v_{k} \tag{2}$$
  随机信号 $w_{k}$和 $v_{k}$分别表示过程激励噪声和观测噪声，假设它们为相互独立且是高斯白噪声，即：
$$p(w)\sim N(0,Q) \tag{3}$$
$$p(v)\sim N(0,R) \tag{4}$$
  实际系统中， 过程激励噪声协方差矩阵 Q 和观测噪声协方差矩阵 R 可 能会随每次迭代计算而变化，但本文假设它们是常数。
  定义：已知第 $k$ 步以前状态情况下第$k$ 步的先验状态估计为 $\hat{\overline x}_{k}$ (−代表先验，ˆ代表估计)；已知测量变量 $z_{k}$时第 $k$ 步的后验状态估计为$\hat x_{k}$；由此定义先验估计误差 $\overline e_{k}$和后验估计误差 $e_{k}$如下：
$$\overline e_{k}= {x}_{k}- \hat{\overline x}_{k} \tag{5}$$
$$e_{k}= {x}_{k}- \hat x_{k} \tag{6}$$
  那么先验估计误差的协方差 $\overline P_{k}$和后验估计的协方差 $P_{k}$为：
$$\overline P_{k}=E[{\overline e_{k}}{\overline e_{k}}^{T}] \tag{7}$$
$$P_{k}=E[{e_{k}}{e_{k}}^{T}] \tag{8}$$
  卡尔曼滤波器的表达式：由先验估计 $\overline P_{k}$和加权的测量变量 $z_{k}$及其预测 $Hx_{k}$之差的线性组合构成后验状态估计 $P_{k}$：
$$\hat x_{k}=\hat{\overline x}_{k}+K(z_{k}-H\hat{\overline x}_{k}) \tag{9}$$
  其中测量变量及其预测之差 $z_{k}-H\hat{\overline x}_{k}$被称为测量过程的革新或残余，残余反映了预测值和实际值之间的不一致程度；矩阵 $K_{m \times n}$叫做残余的增益或混合因数，作用是使(8)式中的后验估计误差协方差最小。
  
  在此忽略矩阵 $K$的详细推导，仅给出计算的大致步骤：
  （1）将(9)式代入$e_{k}$的定义式(6)，再将 $e_{k}$代入(8)式，求得后验估计的协方差 $P_{k}$；
  （2）将(8)式中的 $P_{k}$对 $K$求导并使其一阶导数为零，从而解得矩阵$K$的值；矩阵 $K$的一种表示形式如下：
$$K_{k}=\frac{\overline P_{k} H^{T}}{H \overline P_{k} H^{T}+R} \tag{10}$$
其中 $H$矩阵为常量，$P_{k}$与过程激励噪声的协方差矩阵 $Q$有关；$R$为测量噪声的协方差矩阵。

5 卡尔曼增益的物理意义

  
  取值范围：$K_{k} \in [0, H^{-1}]$，即：$\lim_{R\rightarrow 0}=H^{-1}$，$\lim_{\overline P_{k} \rightarrow 0}=0$，因此卡尔曼滤波增益是一个决定了最优估计组成比例的“调节器”。
  
  具体地：当$R\rightarrow 0$时$\lim_{R\rightarrow 0}=H^{-1}$，那么$\hat x_{k}=\hat{\overline x}_{k}+H^{-1}(z_{k}-H\hat{\overline x}_{k})$，则可以看出系统表现为完全取测量值作为状态的后验估计值，而系统的先验状态估计完全被抛弃；反之$\overline P_{k} \rightarrow 0$，那么$\lim_{\overline P_{k} \rightarrow 0}=0$，即：$Q=0$，则可以看出系统完全抛弃测量值，取先验估计值。

6 卡尔曼增益的理论意义

  被估计值系统的第$k+1$时刻的状态值${x}_{k}$的卡尔曼滤波值$\hat x_{k}$，就是${x}_{k}$的无偏最小方差估计。同时，滤波误差方差矩阵$P_{k}$是基于测量值$z_{1},z_{2},z_{3}$等等的${x}_{k}$的所有线性估计中最小的均方误差阵。
  
  对于一维的情况，测量噪声协方差矩阵$R$增大时，增益矩阵$K$变小。这表明，如果测量噪声越大，该增益取的越小，以减弱测量噪声对估计值的影响，从而使预测值所占最后的结果比重加大。
  
  由(10)式还可以看出，当$\overline P_{k}$或者$Q$矩阵变小时，$P_{k}$也变小（此处可以从推导公式中看出，本文省略），$Ｋ_{k}$矩阵也减小。从直观上看，这是自然的，因为 $Ｐ$变小表示估计值或者预测值比较好，又因为$Ｑ$变小表示状态转移随机波动减小。所以新的测量值对状态的估计值的矫正影响减弱，于是增益矩阵Ｋ应当变小。