卡尔曼滤波器

欢迎访问我的博客首页。

---

1. 滤波器思想

---

  卡尔曼滤波器是一种最优化递归数据处理算法 Optimal Recursive Data Processing Algorithm。

1.1 最优估计

---

  比较精确地测量某个距离的长度时，我们通常多次测量并取其平均值作为真实值的估计值。假设每次的测量值是 $x_i$，则第 $k$ 次的估计值

$$\begin{array}{rl}{\hat{x}}_k& =\frac{1}{k}\sum _{i=1}^k{x}_i\\ & =\frac{1}{k}\cdot \frac{k-1}{k-1}\cdot \sum _{i=1}^{k-1}{x}_i+\frac{1}{k}\cdot x_k\\ & =\frac{k-1}{k}\cdot (\frac{1}{k-1}\cdot \sum _{i=1}^{k-1}{x}_i)+\frac{1}{k}\cdot x_k\\ & =\frac{k-1}{k}\cdot {\hat{x}}_{k-1}+\frac{1}{k}\cdot x_k\\ & ={\hat{x}}_{k-1}+\frac{1}{k}\cdot (x_k-{\hat{x}}_{k-1}).\end{array}$$

从上式可以看出，第 $k$ 次的估计值 ${\hat{x}}_k$ 有三个量决定：第 $k-1$ 次的估计值 ${\hat{x}}_{k-1}$、第 $k$ 次的系数 $\frac{1}{k}$、第 $k$ 次的测量值 $x_k$。上式是一个递归算法，第 $k$ 次的状态仅与第 $k-1$ 次有关，而与更前面的状态无关。这就是卡尔曼滤波器的思想。

  可以看出，$k$ 越大，估计值越趋于稳定，即，与第 $k$ 次的测量值相关性越小。

1.2 数据融合

---

  假设两个称的标准差分别是 ${\sigma }_1=2g$、${\sigma }_2=4g$，使用它们称得一个物体的质量分别是 $z_1=30g$、$z_2=32g$。请估计这个物体的重量。

  题设包含的数学思想是：称对物体的真实质量估计满足正态分布 $N(z,{\sigma }^2)$。数据融合：令 $\hat{z}=z_1+k(z_2-z_1)$。$\hat{z}$ 是两个正态分布的线性组合，因此也是正态分布。下面，我们需要用 $k$ 求出 ${\sigma }_{\hat{z}}$，然后找出令 ${\sigma }_{\hat{z}}$ 最小的 $k$，即可得出 $\hat{z}$ 的无偏估计。

  根据方差的定义

$${\sigma }_{\hat{z}}^2=D(\hat{z})=D[z_1+k(z_2-z_1)]=D[(1-k)z_1+k{z}_2]=(1-k{)}^{2}D(z_1)+k^{2}D(z_2).$$

这是关于 $k$ 的一元二次函数，极值点就是最值点，因此我们对 $k$ 求导

$$\frac{\mathrm{d}{\sigma }_{\hat{z}}^2}{\mathrm{d}k}=-2(1-k)D(z_1)+2kD(z_2).$$

令导数为 0 得 $k=\frac{D(z_1)}{D(z_1)+D(z_2)}=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}=0.2$。因此 $\hat{z}=30.4g$。

2. 卡尔曼滤波器

---

  线性高斯系统可以用状态方程和观测方程表示为

$$\{\begin{array}{rlr}{\mathbf{x}}_k& ={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{\epsilon }}_k,& {\mathbf{\epsilon }}_k\sim N(0,{\mathbf{Q}}_k),\\ {\mathbf{z}}_k& ={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{\delta }}_k,& {\mathbf{\delta }}_k\sim N(0,{\mathbf{R}}_k).\end{array}\text{\hspace{1em}(2.1)}$$

其中，第一个公式称为状态方程或运动方程，$\mathbf{x}$ 称为状态向量，$\mathbf{A}$ 称为状态矩阵，$\mathbf{B}$ 称为控制矩阵，$\mathbf{u}$ 称为控制向量或读数向量，$\mathbf{\epsilon }$ 称为过程噪声。第二个公式称为观测方程，$\mathbf{C}$ 称为观测矩阵，$\mathbf{\delta }$ 称为观测噪声。

2.1 先验估计与后验估计

---

  我们可以分别根据状态方程和观测方程得到 $k$ 时刻的状态

$$\{\begin{array}{rl}{\hat{\mathbf{x}}}_k^{-}& ={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k,\\ {\hat{\mathbf{x}}}_k^M& ={\mathbf{C}}_k^{-1}{\mathbf{z}}_k.\end{array}\text{\hspace{1em}(2.2)}$$

其中，${\hat{\mathbf{x}}}_k^{-}$ 是先验估计，${\hat{\mathbf{x}}}_k^M$ 是测量值。计算这两个值时没有考虑噪声，因此它们是不准确的。所以我们进行数据融合

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+{\mathbf{G}}_k({\mathbf{C}}_k^{-1}{\mathbf{z}}_k-{\hat{\mathbf{x}}}_k^{-}).\text{\hspace{1em}(2.3a)}$$

现在，我们已经利用先验估计 ${\hat{\mathbf{x}}}_k^{-}$ 和测量值 ${\mathbf{C}}_k^{-1}{\mathbf{z}}_k$ 求得后验估计 ${\hat{\mathbf{x}}}_k$。

2.2 卡尔曼增益

---

  更常见的做法是令 ${\mathbf{G}}_k={\mathbf{K}}_k{\mathbf{C}}_k$，把上式写成

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\hat{\mathbf{x}}}_k^{-}).\text{\hspace{1em}(2.3b)}$$

公式中的 ${\mathbf{K}}_k$ 称为卡尔曼增益矩阵（Kalman Gain Matrix）。

  现在求解 ${\mathbf{K}}_k$。我们令 ${\mathbf{e}}_k={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k$。因为 ${\mathbf{e}}_k$ 是均值为 $0$ 的正态分布的线性组合，所以它也是均值为 $0$ 的正态分布，我们假设 ${\mathbf{e}}_k\sim N(0,{\mathbf{P}}_k)$。为了让 ${\hat{\mathbf{x}}}_k$ 尽可能地接近 ${\mathbf{x}}_k$，我们需要让 ${\mathbf{e}}_k$ 各维度的方差之和最小，也就是让其协方差矩阵 ${\mathbf{P}}_k$ 的迹最小。

  由

$$\{\begin{array}{rl}{\mathbf{x}}_k-{\hat{\mathbf{x}}}_k& ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-{\mathbf{K}}_k{\mathbf{z}}_k+{\mathbf{K}}_k{\mathbf{C}}_k{\hat{\mathbf{x}}}_k^{-}\\ & ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-{\mathbf{K}}_k({\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{\delta }}_k)+{\mathbf{K}}_k{\mathbf{C}}_k{\hat{\mathbf{x}}}_k^{-}\\ & ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{x}}_k-{\mathbf{K}}_k{\mathbf{\delta }}_k+{\mathbf{K}}_k{\mathbf{C}}_k{\hat{\mathbf{x}}}_k^{-}\\ & =({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-{\mathbf{K}}_k{\mathbf{C}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-{\mathbf{K}}_k{\mathbf{\delta }}_k\\ & =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k)({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-{\mathbf{K}}_k{\mathbf{\delta }}_k\\ & =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{e}}_k^{-}-{\mathbf{K}}_k{\mathbf{\delta }}_k,\\ ({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k{)}^{\mathrm{T}}& ={\mathbf{e}}_k^{-\mathrm{T}}(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})-{\mathbf{\delta }}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}.\end{array}$$

知

$$\begin{array}{rl}{\mathbf{e}}_k\cdot {\mathbf{e}}_k^{\mathrm{T}}& =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{e}}_k^{-}\cdot {\mathbf{e}}_k^{-\mathrm{T}}(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})\\ & -(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{e}}_k^{-}\cdot {\mathbf{\delta }}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}\\ & -{\mathbf{K}}_k{\mathbf{\delta }}_k\cdot {\mathbf{e}}_k^{-\mathrm{T}}(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})\\ & +{\mathbf{K}}_k{\mathbf{\delta }}_k\cdot {\mathbf{\delta }}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}.\end{array}$$

在第一个公式中，我们记 ${\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}={\mathbf{e}}_k^{-}$。

  为了求 ${\mathbf{P}}_k$ 的迹，我们先求 ${\mathbf{P}}_k$。由附录 4.2 知

$$\begin{array}{rl}{\mathbf{P}}_k& =E({\mathbf{e}}_k\cdot {\mathbf{e}}_k^{\mathrm{T}})\\ & =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k)E({\mathbf{e}}_k^{-}\cdot {\mathbf{e}}_k^{-\mathrm{T}})(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})\\ & -(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k)E({\mathbf{e}}_k^{-}\cdot {\mathbf{\delta }}_k^{\mathrm{T}}){\mathbf{K}}_k^{\mathrm{T}}\\ & -{\mathbf{K}}_{k}E({\mathbf{\delta }}_k\cdot {\mathbf{e}}_k^{-\mathrm{T}})(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})\\ & +{\mathbf{K}}_{k}E({\mathbf{\delta }}_k\cdot {\mathbf{\delta }}_k^{\mathrm{T}}){\mathbf{K}}_k^{\mathrm{T}}\\ & =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_k^{-}(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})\\ & -0\\ & -0\\ & +{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & =({\mathbf{P}}_k^{-}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-})(\mathbf{I}-{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-({\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{)}^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\end{array}\text{\hspace{1em}(2.4)}$$

于是

$$\mathrm{tr}({\mathbf{P}}_k)=\mathrm{tr}({\mathbf{P}}_k^{-})-2\mathrm{tr}({\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-})+\mathrm{tr}({\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}})+\mathrm{tr}({\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}).$$

这里用到的公式有：协方差矩阵 ${\mathbf{P}}_k^{-}$ 是对称矩阵；对称矩阵的性质 $(\mathbf{A}\mathbf{B}{)}^{\mathrm{T}}={\mathbf{B}}^{\mathrm{T}}{\mathbf{A}}^{\mathrm{T}}$；迹的性质 $\mathrm{tr}(\mathbf{A})=\mathrm{tr}({\mathbf{A}}^{\mathrm{T}})$。在第一个公式中，我们令 ${\mathbf{P}}_k^{-}=E({\mathbf{e}}_k^{-}\cdot {\mathbf{e}}_k^{-\mathrm{T}})$。

  接下来我们要找使 ${\mathbf{P}}_k$ 的迹 $\mathrm{tr}({\mathbf{P}}_k)$ 最小的 ${\mathbf{K}}_k$，于是我们求导并使之为 $0$：

$$0=\begin{array}{r}\frac{\mathrm{d}\mathrm{tr}({\mathbf{P}}_k)}{\mathrm{d}{\mathbf{K}}_k}=0-2{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}+2{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+2{\mathbf{K}}_k{\mathbf{R}}_k.\end{array}$$

得

$${\mathbf{K}}_k=\frac{{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}}{{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k}=\frac{{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}}{{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k}.\text{\hspace{1em}(2.5)}$$

这里用到的公式有：$\frac{\mathrm{d}\mathrm{tr}(\mathbf{A}\mathbf{X})}{\mathrm{d}\mathbf{X}}=\frac{\mathrm{d}\mathrm{tr}(\mathbf{X}\mathbf{A})}{\mathrm{d}\mathbf{X}}={\mathbf{A}}^{\mathrm{T}}$；$\frac{\mathrm{d}\mathrm{tr}(\mathbf{A}\mathbf{X}\mathbf{B}{\mathbf{X}}^{\mathrm{T}})}{\mathrm{d}\mathbf{X}}=\mathbf{A}\mathbf{X}\mathbf{B}+{\mathbf{A}}^{\mathrm{T}}\mathbf{X}{\mathbf{B}}^{\mathrm{T}}$。

2.3 先验误差的方差和误差的方差

---

  欲求 ${\mathbf{K}}_k$ 先求先验误差的方差 ${\mathbf{P}}_k^{-}$。我们求 ${\mathbf{P}}_k$ 时令 ${\mathbf{P}}_k^{-}=E({\mathbf{e}}_k^{-}\cdot {\mathbf{e}}_k^{-\mathrm{T}})$，而

$$\begin{array}{rl}{\mathbf{e}}_k^{-}& ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}\\ & =({\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{\epsilon }}_k)-({\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k)\\ & ={\mathbf{A}}_k({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})+{\mathbf{\epsilon }}_k\\ & ={\mathbf{A}}_k{\mathbf{e}}_{k-1}+{\mathbf{\epsilon }}_k.\end{array}$$

其中 ${\mathbf{\epsilon }}_k\sim N(0,{\mathbf{Q}}_k)$。于是

$$\begin{array}{rl}{\mathbf{P}}_k^{-}& =E({\mathbf{e}}_k^{-}\cdot {\mathbf{e}}_k^{-\mathrm{T}})\\ & =E[({\mathbf{A}}_k{\mathbf{e}}_{k-1}+{\mathbf{\epsilon }}_k)({\mathbf{e}}_{k-1}^{\mathrm{T}}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{\epsilon }}_k^{\mathrm{T}})]\\ & =E({\mathbf{A}}_k{\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\mathrm{T}}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{A}}_k{\mathbf{e}}_{k-1}{\mathbf{\epsilon }}_k^{\mathrm{T}}+{\mathbf{\epsilon }}_k{\mathbf{e}}_{k-1}^{\mathrm{T}}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{\epsilon }}_k{\mathbf{\epsilon }}_k^{\mathrm{T}})\\ & =E({\mathbf{A}}_k{\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\mathrm{T}}{\mathbf{A}}_k^{\mathrm{T}})+E({\mathbf{A}}_k{\mathbf{e}}_{k-1}{\mathbf{\epsilon }}_k^{\mathrm{T}})+E({\mathbf{\epsilon }}_k{\mathbf{e}}_{k-1}^{\mathrm{T}}{\mathbf{A}}_k^{\mathrm{T}})+E({\mathbf{\epsilon }}_k{\mathbf{\epsilon }}_k^{\mathrm{T}})\\ & ={\mathbf{A}}_{k}E({\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\mathrm{T}}){\mathbf{A}}_k^{\mathrm{T}}+0+0+E({\mathbf{\epsilon }}_k{\mathbf{\epsilon }}_k^{\mathrm{T}})\\ & ={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{Q}}_k.\end{array}$$

  可以看出，欲求先验误差的方差 ${\mathbf{P}}_k^{-}$，需先求上一轮误差的方差 ${\mathbf{P}}_{k-1}$。因此我们利用公式 (2.4) 给出求误差的方差的公式

$$\begin{array}{rl}{\mathbf{P}}_k& ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{K}}_k({\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k){\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+\frac{{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}}{{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k}({\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k){\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{P}}_k^{-\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}+{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{K}}_k^{\mathrm{T}}\\ & ={\mathbf{P}}_k^{-}-{\mathbf{K}}_k{\mathbf{C}}_k{\mathbf{P}}_k^{-}\\ & =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_k^{-}.\end{array}$$

2.4 估计过程

---

  现在我们总结一下使用卡尔曼滤波器进行状态估计的过程。第一步求先验估计

$${\hat{\mathbf{x}}}_k^{-}={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k$$

第二步求先验误差的方差（协方差矩阵）

$${\mathbf{P}}_k^{-}={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{Q}}_k$$

第三步计算卡尔曼增益

$${\mathbf{K}}_k=\frac{{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}}{{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k}.$$

第四步计算后验估计

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\hat{\mathbf{x}}}_k^{-}).$$

第五步求误差的方差（协方差矩阵）

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_k^{-}.$$

这五个步骤的前两步称为预测，后三步称为校正。

3. 扩展卡尔曼滤波器

---

  非线性高斯系统可以用状态方程和观测方程表示为

$$\{\begin{array}{rlr}{\mathbf{x}}_k& =f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{\epsilon }}_k,& {\mathbf{\epsilon }}_k\sim N(0,{\mathbf{Q}}_k),\\ {\mathbf{z}}_k& =h({\mathbf{x}}_k)+{\mathbf{\delta }}_k,& {\mathbf{\delta }}_k\sim N(0,{\mathbf{R}}_k).\end{array}\text{\hspace{1em}(3.1)}$$

3.1 线性化

---

  使用泰勒公式展开成线性方程

$$\{\begin{array}{rlr}{\mathbf{x}}_k& =f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)+{\mathbf{\epsilon }}_k+{\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}\cdot ({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}),& {\mathbf{\epsilon }}_k\sim N(0,{\mathbf{Q}}_k),\\ {\mathbf{z}}_k& =h({\tilde{\mathbf{x}}}_k)+{\mathbf{\delta }}_k+{\frac{\partial h}{\partial x_k}|}_{{\tilde{\mathbf{x}}}_k}\cdot ({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k),& {\mathbf{\delta }}_k\sim N(0,{\mathbf{R}}_k).\end{array}\text{\hspace{1em}(3.2a)}$$

我们记

$$\{\begin{array}{rl}{\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}& =A_k\\ {\frac{\partial h}{\partial x_k}|}_{{\tilde{\mathbf{x}}}_k}& =C_k\end{array}$$

则公式 (3.2a) 可以写成

$$\{\begin{array}{rlr}{\mathbf{x}}_k& =f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)+{\mathbf{\epsilon }}_k+{\mathbf{A}}_k\cdot ({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}),& {\mathbf{\epsilon }}_k\sim N(0,{\mathbf{Q}}_k),\\ {\mathbf{z}}_k& =h({\tilde{\mathbf{x}}}_k)+{\mathbf{\delta }}_k+{\mathbf{C}}_k\cdot ({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k),& {\mathbf{\delta }}_k\sim N(0,{\mathbf{R}}_k).\end{array}\text{\hspace{1em}(3.2b)}$$

现在我们已经把非线性高斯系统转化成与公式 (2.2) 一样的线性高斯系统，因此估计过程可以直接使用第 2 节的推导结果。

3.2 估计过程

---

  类比卡尔曼滤波，我们可以写出扩展卡尔曼滤波的五个公式

$$\{\begin{array}{rl}{\hat{\mathbf{x}}}_k^{-}& =f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)\\ {\mathbf{P}}_k^{-}& ={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+{\mathbf{Q}}_k\\ {\mathbf{K}}_k& =\frac{{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}}{{\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{R}}_k}\\ {\hat{\mathbf{x}}}_k& ={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k[{\mathbf{z}}_k-h({\hat{\mathbf{x}}}_k^{-})]\\ {\mathbf{P}}_k& =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_k^{-}.\end{array}$$

3.3 例子

---

# encoding=utf-8
import math
import numpy as np
import matplotlib.pyplot as plt


# 运动方程。
def motion_model(x, u):
    B = np.array([[DT * math.cos(x[2, 0]), 0],
                  [DT * math.sin(x[2, 0]), 0],
                  [0.0, DT],
                  [0.0, 0.0]])
    x = x + B @ u
    return x


# 观测方程。
def observation_model(x):
    H = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])
    z = H @ x
    return z


# 运动学方程中的雅克比矩阵。
def jacob_f(x, u):
    yaw = x[2, 0]
    v = u[0, 0]
    jF = np.array([
        [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
        [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0]])
    return jF


# 观测方程中的雅克比矩阵。
def jacob_h():
    jH = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])
    return jH


def observation(xTrue, xd, u):
    # 计算无噪声的五元状态，仅用于绘图。
    xTrue = motion_model(xTrue, u)
    # 观测值。
    z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
    # add noise to input
    ud = u + INPUT_NOISE @ np.random.randn(2, 1)
    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud


# 扩展卡尔曼滤波过程
def ekf_estimation(xEst, PEst, z, u):
    """
    滤波的五个公式。
    :param xEst: 上一轮的估计值 hat x_{k - 1}.
    :param PEst: 误差的方差 P_{k - 1}。
    :param z:    观测值 z_k。
    :param u:    带噪声的控制向量 u_k。
    :return:     估计值 hat x_k 和误差的方差 P_k。
    """

    # 1.预测。
    # hat x_k^- = f(hat x_{k - 1}, u_k)
    xPred = motion_model(xEst, u)
    # P_k^- = A_k P_{k - 1} A_k^T + Q_k
    jF = jacob_f(xEst, u)
    PPred = jF @ PEst @ jF.T + Q  # 预测方差
    # 2.更新。
    # K_k = P_k^- H_k^T / (H_k P_k^- H_k^T + R_k)
    jH = jacob_h()
    K = PPred @ jH.T @ np.linalg.inv(jH @ PPred @ jH.T + R)
    # hat x_k = hat x_k^- + K_k(z_k - h(hat x_k^-))
    xEst = xPred + K @ (z - observation_model(xPred))
    # P_k = (I - K_k H_k) P_k^-
    PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
    # 3.。
    return xEst, PEst


# 绘制协方差的椭圆
def plot_covariance_ellipse(xEst, PEst):
    # 从 4x4 的 PEst 矩阵取左上角的 2x2 矩阵。
    Pxy = PEst[0:2, 0:2]
    # 计算特征值和特征向量。
    eigval, eigvec = np.linalg.eig(Pxy)

    if eigval[0] >= eigval[1]:
        bigind = 0
        smallind = 1
    else:
        bigind = 1
        smallind = 0

    t = np.arange(0, 2 * math.pi + 0.1, 0.1)
    a = math.sqrt(eigval[bigind])
    b = math.sqrt(eigval[smallind])
    x = [a * math.cos(it) for it in t]
    y = [b * math.sin(it) for it in t]
    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
    rot = np.array([[math.cos(angle), math.sin(angle)],
                    [-math.sin(angle), math.cos(angle)]])
    fx = rot @ (np.array([x, y]))
    px = np.array(fx[0, :] + xEst[0, 0]).flatten()
    py = np.array(fx[1, :] + xEst[1, 0]).flatten()
    plt.plot(px, py, "--r")


def main():
    print(__file__ + " start!!")

    time = 0.0

    # State Vector [x y yaw v]'
    xEst = np.zeros((4, 1))
    xTrue = np.zeros((4, 1))
    PEst = np.eye(4)

    xDR = np.zeros((4, 1))  # Dead reckoning

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((2, 1))

    while SIM_TIME >= time:
        time += DT
        # 线速度 m/s，角速度 rad/s。
        u = np.array([[1], [0.1]])

        xTrue, z, xDR, ud = observation(xTrue, xDR, u)
        # 扩展卡尔曼滤波器的五个方程。
        xEst, PEst = ekf_estimation(xEst, PEst, z, ud)

        # store data history
        hxEst = np.hstack((hxEst, xEst))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))
        hz = np.hstack((hz, z))

        plt.cla()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect('key_release_event',
                                     lambda event: [exit(0) if event.key == 'escape' else None])
        plt.plot(hz[0, :], hz[1, :], ".g")  # 观测数据
        plt.plot(hxTrue[0, :].flatten(), hxTrue[1, :].flatten(), "-y")  # 真实
        plt.plot(hxDR[0, :].flatten(), hxDR[1, :].flatten(), "-k")  # 黑色
        plt.plot(hxEst[0, :].flatten(), hxEst[1, :].flatten(), "-r")  # 红色
        plot_covariance_ellipse(xEst, PEst)
        plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)


if __name__ == '__main__':
    # EKF 状态方程的协方差矩阵
    Q = np.diag([
        0.1,  # variance of location on x-axis
        0.1,  # variance of location on y-axis
        np.deg2rad(1.0),  # variance of yaw angle
        1.0  # variance of velocity
    ]) ** 2  # predict state covariance

    # 观测方程的协方差矩阵
    R = np.diag([1.0, 1.0]) ** 2  # Observation x,y position covariance

    # 仿真参数
    INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
    GPS_NOISE = np.diag([0.5, 0.5]) ** 2

    DT = 0.1  # time tick [s]
    SIM_TIME = 100.0  # simulation time [s]
    main()

4. 附录

---

4.1 正态分布

---

  正态分布又叫高斯分布。均值为 $\mu$ 方差为 ${\sigma }^2$ 的一维正态分布的概率密度函数是

$$p(x)=(2\pi {\sigma }^2{)}^{-\frac{1}{2}}\exp [-\frac{1}{2}\frac{(x-{\mu }^2)}{{\sigma }^2}].$$

当随机变量 $\mathbf{x}$ 是多维矢量时，称为多元正态分布。多元正态分布的概率密度函数是

$$p(\mathbf{x})=\det (2\pi \mathbf{\Sigma }{)}^{-\frac{1}{2}}\exp [-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })].$$

其中，方差 $\mathbf{\Sigma }$ 由随机变量 $\mathbf{x}$ 各分量的协方差组成

$$\mathbf{\Sigma }=\left[\begin{array}{cccc}\mathrm{Cov}(x_1,x_1)& \mathrm{Cov}(x_1,x_2)& \cdots & \mathrm{Cov}(x_1,x_n)\\ \mathrm{Cov}(x_2,x_1)& \mathrm{Cov}(x_2,x_2)& \cdots & \mathrm{Cov}(x_2,x_n)\\ ⋮& ⋮& \ddots & ⋮\\ \mathrm{Cov}(x_n,x_1)& \mathrm{Cov}(x_n,x_2)& \cdots & \mathrm{Cov}(x_n,x_n)\end{array}\right].$$

4.2 均值为零的随机变量的方差

---

  均值为零的一维随机变量 $x\sim N(0,{\sigma }^2)$ 的方差

$${\sigma }^2=D(x)=E[x-E(x){]}^2=E(x^2).$$

  均值为零的多维随机变量 $\mathbf{x}\sim N(0,\mathbf{\Sigma })$ 的协方差矩阵

$$\mathbf{\Sigma }=E(\mathbf{x}{\mathbf{x}}^{\mathrm{T}}).$$

证明：因为 $\mathbf{x}$ 的各分量相互独立，所以

$$\mathrm{Cov}(x_i,x_j)=E(x_i{x}_j)-E(x_i)E(x_j)=E(x_i{x}_j)=x_i{x}_j.$$

于是

$$\mathbf{\Sigma }=\left[\begin{array}{cccc}{x}_1{x}_1& x_1{x}_2& \cdots & x_1{x}_n\\ x_2{x}_1& x_2{x}_2& \cdots & x_2{x}_n\\ ⋮& ⋮& \ddots & ⋮\\ x_n{x}_1& x_n{x}_2& \cdots & x_n{x}_n\end{array}\right]=E(\mathbf{x}{\mathbf{x}}^{\mathrm{T}}).$$

5. 参考

---

1. 卡尔曼滤波器，B 站，2020。
2. 扩展卡尔曼滤波器，CSDN，2023。