对于卡尔曼滤波的一点理解

前言

   这篇博客主要讲的是我个人对于卡尔曼滤波的一点理解，所以并不会细致地讲解卡尔曼滤波的原理及公式推导。

   关于详细的卡尔曼讲解的博客，可以移步《卡尔曼滤波–从推导到应用》。这篇博客我觉得讲得很细致，公式推导也很详细，适合系统的学习。

   以下将是我的个人理解。

卡尔曼滤波到底解决什么问题

   想要搞清楚这个问题，我们就不得不提两个方程了：

$$状态转换方程：\qquad x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1} \qquad (1) \\ 测量方程：\qquad z_k = Hx_k + v_k \qquad (2) \\ w_{k-1} \sim N(0, Q)\\ v_{k} \sim N(0, R)$$

   我在学习卡尔曼滤波的时候，花了很多时间在理解这两个方程上，并且由于一些资料不够明确，导致走了很多弯路，所以想在这里完整地讲述一下我对这两个方程的理解：

• 假设 $w_k, v_k$ 服从多元高斯分布，并且两者相互独立。

   对于状态转换方程（1）:

• 这个方程其实是一个系统的状态模型，它表示了一个系统在 $k$ 时刻的状态 $x_k$ 由 $k-1$ 时刻的状态 $x_{k-1}$ 以及 $k-1$ 时刻的输入 $u_{k-1}$ 所决定。

• A,B通常都是常数矩阵，并且$A \in R^{n \times n}, B \in R^{n \times m}$ 。

• $x_k \in \Bbb R^n$ 代表着这个状态通过 $n$ 个属性来表示。比如要描述一个行驶小车的状态，我们可能需要 $x_k^T = \{位移值，速度值，方向值\}$ ， 此时 $n = 3$, 也就是说我们用三行列向量来表示该系统的状态。

• $u_{k-1} \in R^m$ 代表着我们的输入量，在刚才那个描述小车的例子里，很可能 $u_{k-1}^T = \{加速度， 偏移角度\}$。

• $w_{k-1}$ 则代表了 $k-1$ 时刻的系统噪声。为什么需要这个量呢，回想一下高中物理中，平常我们分析物体运动的时候，总会假设受恒力作用或者是匀加速运动，一切都是在理想条件下发生的。假如没有 $w_{k-1}$，我们的模型可能仅仅适用于理想情况下，而引入$w_{k-1}$，我们的模型就能适用于实际情况中。还是刚才的小车例子，$w_{k-1}$ 就代表着在 $k - 1$ 时刻到 $k$ 时刻系统内部发生了细微的变化，导致$x_k \ne Ax_{k-1} + Bu_{k-1}$ 。

   通过对状态转换方程的分析，其实我们也就知道了，假使给我们一个确切的$x_{k-1}$,由于噪声的存在，并且我们不知道$k-1$ 时刻的系统噪声值 $w_{k-1}$,所以我们并不能根据这个模型以及这个给定的$x_{k-1}$ 计算出 $x_k$, 我们仅仅只能计算出根据模型得到的预测值 $\hat x^- = Ax_{k-1} + Bu_{k-1}$ 。

  那为什么我们不以这个预测量作为 $k$时刻的状态呢？假如我们这么做的话，也就是说之后 $k+1$ 时刻的状态也将是这样预测出来，可能一开始预测值相较于真实值而言还比较接近。但随着递推的增多，初始的$w_{k-1}$ 经过不断的放大（与A相乘），最后的误差将变得非常大。这也就是我们需要另一个测量方程的原因。

   对于测量方程（2）：

• $H \in R^{n \times n}$ ，类似于状态转换方程中的A和B，通常也是常数矩阵。

• 测量噪声 $v_k$ 的存在也很好理解，在测量中由于仪器失误或者偏差，总会造成误差。

• 有一点需要注意的是：$z_k$ 是我们能够得到的值，即我们的测量值，但其本身就不准确，因为受到了测量噪声的影响。

   我们介绍完了两个方程，就可以真正地说到卡尔曼滤波的作用了。其本质作用用一句话概括就是：用测量值不断修正预测值，使得通过两者结合得到的状态的估计值接近系统状态的真实值。

   那么这个估计值 $\hat x_k$ 又是什么呢？

$$\hat x_k = \hat x_k^- + K_k(z_k - \hat z_k) = \hat x_k^- + K_k(z_k - H\hat x_k^-) \qquad (3)$$

   从（3）式 可以看出，$\hat x_k^-$ 是预测值, 可以通过我们的模型预测得到， $H$ 已知，是常数矩阵，$z_k$ 是实际的测量值，可通过测量得到，所以只需要求得$K_k$ 我们就可以得到 $k$ 时刻的估计值 $\hat x_k$ 。

  先介绍几个概念：

  估计值和真实值的协方差矩阵 $P_k$ :

$$P_k = \{e_k e_k^T\} = \{(x_k - \hat x_k)(x_k - \hat x_k)^T\}$$

   预测值和真实值的协方差矩阵 $P_k^-$ ：

$$P_k^- = \{e_k^- {e_l^-}^T\} = \{(x_k - \hat x_k^-)(x_k - \hat x_k^-)^T\}$$

  接下来我们来看几个公式，关于公式的推导，可以看上文我推荐的博客，这里仅仅写出结果：

$$P_{k+1}^- = AP_kA^T + Q \qquad (4)\\ K_k = P_k^- H^T(HP_k^- H^T + R)^{-1} \qquad (5) \\ P_k =(I - K_k H)P_k^- \qquad I为单位矩阵 \qquad (6)$$

卡尔曼滤波具体过程

1. 首先建立最开始的状态变换方程和测量方程，并确定$A,B,H,Q,R$矩阵。

2. 然后假设0时刻的状态估计值为$\hat x_0$，并且确定此时的$P_0$,这两个初值都可以人工设定。我们的首要任务是计算出1时刻的状态估计值$\hat x_1$ 。

3. 根据状态方程，可计算1时刻的预测值 $\hat x_1^- = A \hat x_0 + B u_0$ ，并且根据测量直接得到1时刻的测量值 $z_1$ 。

4. 根据（4），$P_1^- = AP_0A^T + Q$ 。

5. 根据（5），$K_1 = P_1^- H^T(HP_1^- H^T + R)^{-1}$ ,计算出1时刻的卡尔曼增益 $K_1$ 。

6. 根据（3），$\hat x_1 = \hat x_1^- + K_1(z_1 - H\hat x_1^-)$ 计算得到1时刻的估计值$\hat x_1$ 。

7. 根据（6），$P_1 =(I - K_1 H)P_1^-$ 为下一次迭代做准备。

8. 重复2-7步，可以迭代递推出之后的所有估计值。