NO.20.tip: 卡尔曼滤波

背景

  考虑在状态空间描述的线性系统：

$$\{\begin{array}{l}{y}_{k+1}=A_k{y}_k+B_k{u}_k+{\Gamma }_k{\xi }_k\\ w_k=C_k{y}_k+D_k{u}_k+{\eta }_k\end{array}\text{\hspace{1em}(1)}$$

式中 $A_k,B_k,{\Gamma }_k,C_k,D_k$ 分别是 $n\times n,n\times m,n\times p,q\times n,q\times m$ 阶常值矩阵（已知），且有 $1⩽m,p,q⩽n$，$\{u_k\}$ 是 $m$ 维向量序列（称为确定性输入序列）（已知），$\{{\xi }_k\}$ 和 $\{{\eta }_k\}$ 分别是已知均值、方差和协方差等统计信息的系统和观测噪声序列（未知）。因为同时有确定性输入 $\{u_k\}$ 和噪声序列 $\{{\xi }_k\}$ 及 $\{{\eta }_k\}$，该系统被称为线性确定性/随机系统。该系统能够分解成一个线性确定性系统：

$$\{\begin{array}{l}{z}_{k+1}=A_k{z}_k+B_k{u}_k\\ s_k=C_k{z}_k+D_k{u}_k\end{array}\text{\hspace{1em}(2)}$$

与一个线性（完全）随机系统的和。

$$\{\begin{array}{l}{x}_{k+1}=A_k{x}_k+{\Gamma }_k{\xi }_k\\ v_k=C_k{x}_k+{\eta }_k\end{array}\text{\hspace{1em}(3)}$$

式中 $w_k=s_k+v_k$，$y_k=z_k+x_k$。这样分解的好处是线性确定性系统的解 $z_k$ 能够由转换方程给出：

$$z_k=(A_{k-1}\cdots A_0)z_0+\sum _{i=1}^k(A_{k-1}\cdots A_{i-1})B_{i-1}{u}_{i-1}\text{\hspace{1em}(4)}$$

而通过求解式 (3) 描述的随机状态空间的 $x_k$ 的最优估计 ${\hat{x}}_k$，从而

$${\hat{y}}_k=z_k+{\hat{x}}_k\text{\hspace{1em}(5)}$$

为原始线性系统的状态向量 $y_k$ 的最优估计。当然，估计必须依赖于噪声序列的统计信息。在本章中，只考虑零均值高斯白噪声过程。

　假设：令 $\{{\xi }_k\}$ 和 $\{{\eta }_k\}$ 是零均值高斯白噪声序列。那么对于所有 $k$ 和 $l$，$\mathrm{Var}({\xi }_k)=Q_k$ 和 $\mathrm{Var}({\eta }_k)=R_k$ 是正定矩阵，且 $E({\xi }_k{\eta }_l^{\mathrm{T}})=0$。另假设初始状态 $x_0$ 与 ${\xi }_k,{\eta }_k$ 独立，即对于所有的 $k$，有 $E(x_0{\xi }_k^{\mathrm{T}})=0$ 和 $E(x_0{\eta }_k^{\mathrm{T}})=0$ 成立。 $■$

源与流

  在确定 $x_k$ 的最优估计 ${\hat{x}}_k$ 时，最优性是通过选择最优权值矩阵给出的最小二乘意义下的最小方差估计取得的。但是需要联合所有数据 $v_j(j=0,1,\cdots ,k)$ 的信息来确定 $x_k$ 的估计 ${\hat{x}}_k$。为了实现该思路，引入向量：

$${\bar{v}}_j=\left[\begin{array}{c}{v}_0\\ ⋮\\ v_j\end{array}\right],j=0,1,\cdots \text{\hspace{1em}(6)}$$

并从数据向量 ${\bar{v}}_k$ 中求得 ${\hat{x}}_k$。为了完成该过程，假设到当前时刻的所有系统矩阵 $A_j$ 非奇异。那么状态空间描述的线性随机系统可以写为：

$${\bar{v}}_j=H_{k,j}{x}_k+{\overline{ϵ}}_{k,j}\text{\hspace{1em}(7)}$$

式中：

$$H_{k,j}=\left[\begin{array}{c}{C}_0{\Phi }_{0k}\\ ⋮\\ C_j{\Phi }_{jk}\end{array}\right]\text{和}{\overline{ϵ}}_{k,j}=\left[\begin{array}{c}{ϵ}_{k,0}\\ ⋮\\ {ϵ}_{k,j}\end{array}\right]\text{\hspace{1em}(8)}$$

转移矩阵 ${\Phi }_{lk}$ 定义为：

$${\Phi }_{lk}=\{\begin{array}{ll}{A}_{l-1}\cdots A_k,& l>k\\ I,& l=k\end{array}\text{\hspace{1em}(9)}$$

当 $l<k$ 时，${\Phi }_{lk}={\Phi }_{kl}^{-1}$，且

$${ϵ}_{k,l}={\eta }_l-C_l\sum _{i=l+1}^k{\Phi }_{li}{\Gamma }_{i-1}{\xi }_{i-1}\text{\hspace{1em}(10)}$$

应用前面介绍的 ${\Phi }_{lk}$ 的逆变换特性，转移方程为：

$$x_k={\Phi }_{kl}{x}_l+\sum _{i=l+1}^k{\Phi }_{ki}{\Gamma }_{i-1}{\xi }_{i-1}\text{\hspace{1em}(11)}$$

该式可以轻易地从式 (3) 介绍的系统方程得到，有：

$$x_l={\Phi }_{lk}{x}_k-\sum _{i=l+1}^k{\Phi }_{li}{\Gamma }_{i-1}{\xi }_{i-1}\text{\hspace{1em}(12)}$$

可得：

$$H_{k,j}{x}_k+{\overline{ϵ}}_{k,j}=\left[\begin{array}{c}{C}_0{\Phi }_{0k}\\ ⋮\\ C_j{\Phi }_{jk}\end{array}\right]x_k+\left[\begin{array}{c}{\eta }_0-C_0\sum _{i=1}^k{\Phi }_{0i}{\Gamma }_{i-1}{\xi }_{i-1}\\ ⋮\\ {\eta }_j-C_j\sum _{i=j+1}^k{\Phi }_{ji}{\Gamma }_{i-1}{\xi }_{i-1}\end{array}\right]=\left[\begin{array}{c}{C}_0{x}_0+{\eta }_0\\ ⋮\\ C_j{x}_j+{\eta }_j\end{array}\right]=\left[\begin{array}{c}{v}_0\\ ⋮\\ v_j\end{array}\right]={\bar{v}}_j\text{\hspace{1em}(13)}$$

即式 (7)。

  使用最小二乘估计，权值为 $W_{k,j}=(\mathrm{Var}({\overline{ϵ}}_{k,j}){)}^{-1}$，这样通过使用数据 $v_0,\cdots ,v_j$，就可以得到 $x_k$ 的线性、无偏、最小方差最小二乘估计 ${\hat{x}}_{k|j}$。

定义：　(1) 对于 $j=k$，定义 ${\hat{x}}_k={\hat{x}}_{k|k}$，并称该估计过程为数字滤波过程；(2) 对于 $j<k$，定义 ${\hat{x}}_{k|j}$ 为 $x_k$ 的最优预测，并称该过程为数字预测过程；(3) 对于 $j>k$，定义 ${\hat{x}}_{k|j}$ 为 $x_k$ 的平滑估计，并且称该过程为数字平滑过程。 $■$

  卡尔曼滤波属于数字滤波。由于 ${\hat{x}}_k={\hat{x}}_{k|k}$ 是根据所有数据 $v_0,\cdots ,v_j$ 确定的，因为数据存储量和计算量随着时间增加，该方法不适用于 $k$ 值很大的实时问题。因此我们打算推导从“预测” ${\hat{x}}_{k|k-1}$ 得到 ${\hat{x}}_k={\hat{x}}_{k|k}$，及从估计 ${\hat{x}}_{k-1}={\hat{x}}_{k-1|k-1}$ 得到 ${\hat{x}}_{k|k-1}$ 的递推公式。在其中的每一步，由于只使用最新的数据信息，故只需用很小的数据存储量。这就是通常提到的卡尔曼滤波算法。

预测-校正公式

  为了实时计算 ${\hat{x}}_k$，本节将推导递推公式：

$$\{\begin{array}{l}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+G_k(v_k-C_k{\hat{x}}_{k|k-1})\\ {\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}\end{array}\text{\hspace{1em}(14)}$$

式中 $G_k$ 为卡尔曼增益矩阵。

  开始点是初始估计 ${\hat{x}}_0={\hat{x}}_{0|0}$，因为 ${\hat{x}}_0$ 是初始状态 $x_0$ 的无偏估计，可以使用常值向量 ${\hat{x}}_0=E(x_0)$。而在实际的卡尔曼滤波中，$G_k$ 也必须递推计算。这两个递推过程合起来称为卡尔曼滤波过程。

  选择权值矩阵：

$$W_{k,j}=(\mathrm{Var}({\overline{ϵ}}_{k,j}){)}^{-1}\text{\hspace{1em}(15)}$$

使用式 (7) 的 ${\bar{v}}_j$，使得 ${\hat{x}}_{k|j}$ 是 $x_k$ 的具有最小方差的（最优）最小二乘估计。易证：

$$W_{k,k-1}^{-1}=\left[\begin{array}{ccc}{R}_0& & 0\\ & \ddots & \\ 0& & R_{k-1}\end{array}\right]+\mathrm{Var}\left[\begin{array}{c}{C}_0\sum _{i=1}^k{\Phi }_{0i}{\Gamma }_{i-1}{\xi }_{i-1}\\ ⋮\\ C_{k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}{\xi }_{k-1}\end{array}\right]\text{\hspace{1em}(16)}$$

$$W_{k,k}^{-1}=\left[\begin{array}{cc}{W}_{k,k-1}^{-1}& 0\\ 0& R_k\end{array}\right]\text{\hspace{1em}(17)}$$

。所以，$W_{k,k-1}$ 和 $W_{k,k}$ 是正定的。

  在这里，假设矩阵 $(H_{k,j}^{\mathrm{T}}{W}_{k,j}{H}_{k,j}),j=k-1,k$，非奇异。

  由以上可知：

$${\hat{x}}_{k|j}=(H_{k,j}^{\mathrm{T}}{W}_{k,j}{H}_{k,j}{)}^{-1}{H}_{k,j}^{\mathrm{T}}{W}_{k,j}{\bar{v}}_j\text{\hspace{1em}(18)}$$

我们第一个目标是建立 ${\hat{x}}_{k|k-1}$ 与 ${\hat{x}}_{k|k}$ 的联系。为了实现该目标，注意到：

$$\begin{array}{rl}{H}_{k,k}^{\mathrm{T}}{W}_{k,k}{H}_{k,k}& =\left[\begin{array}{cc}{H}_{k,k-1}^{\mathrm{T}}& C_k^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}{W}_{k,k-1}& 0\\ 0& R_k^{-1}\end{array}\right]\left[\begin{array}{c}{H}_{k,k-1}\\ C_k\end{array}\right]\\ & =H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k\end{array}\text{\hspace{1em}(19)}$$

及

$$H_{k,k}^{\mathrm{T}}{W}_{k,k}{\bar{v}}_k=H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{\bar{v}}_{k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{v}_k\text{\hspace{1em}(20)}$$

应用式 (18) 和前面的两个方程，得：

$$\{\begin{array}{l}(H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k){\hat{x}}_{k|k-1}=H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{\bar{v}}_{k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k{\hat{x}}_{k|k-1}\\ (H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k){\hat{x}}_{k|k}=(H_{k,k}^{\mathrm{T}}{W}_{k,k}{H}_{k,k}){\hat{x}}_{k|k}=H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{\bar{v}}_{k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{v}_k\end{array}\text{\hspace{1em}(21)}$$

通过简单的减法可得：

$$(H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k)({\hat{x}}_{k|k}-{\hat{x}}_{k|k-1})=C_k^{\mathrm{T}}{R}_k^{-1}(v_k-C_k{\hat{x}}_{k|k-1})\text{\hspace{1em}(22)}$$

定义：

$$\begin{array}{rl}{G}_k& =(H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k{)}^{-1}{C}_k^{\mathrm{T}}{R}_k^{-1}\\ & =(H_{k,k}^{\mathrm{T}}{W}_{k,k}{H}_{k,k}{)}^{-1}{C}_k^{\mathrm{T}}{R}_k^{-1}\end{array}\text{\hspace{1em}(23)}$$

这样就得到：

$${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+G_k(v_k-C_k{\hat{x}}_{k|k-1})\text{\hspace{1em}(24)}$$

  因为 ${\hat{x}}_{k|k-1}$ 是一步预测，$(v_k-C_k{\hat{x}}_{k|k-1})$ 是实际数据和预测之间的误差，式 (24) 实际上是以卡尔曼滤波增益 $G_k$ 作为权值矩阵的“预测-校正”公式。为了完成递推过程，还需要一个从 ${\hat{x}}_{k-1|k-1}$ 到 ${\hat{x}}_{k|k-1}$ 的公式：

$${\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}\text{\hspace{1em}(25)}$$

为了证明该式，首先注意到：

$${\overline{ϵ}}_{k,k-1}={\overline{ϵ}}_{k-1,k-1}-H_{k,k-1}{\Gamma }_{k-1}{\xi }_{k-1}\text{\hspace{1em}(26)}$$

使得：

$$W_{k,k-1}^{-1}=W_{k-1,k-1}^{-1}+H_{k-1,k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^{\mathrm{T}}{\Phi }_{k-1,k}^{\mathrm{T}}{H}_{k-1,k-1}^{\mathrm{T}}\text{\hspace{1em}(27)}$$

  根据以上有：

$$\begin{array}{rl}{W}_{k,k-1}=& W_{k-1,k-1}-W_{k-1,k-1}{H}_{k-1,k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}(Q_{k-1}^{-1}\\ & +{\Gamma }_{k-1}^{\mathrm{T}}{\Phi }_{k-1,k}^{\mathrm{T}}{H}_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}{H}_{k-1,k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}{)}^{-1}\\ & \cdot {\Gamma }_{k-1}^{\mathrm{T}}{\Phi }_{k-1,k}^{\mathrm{T}}{H}_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}\end{array}\text{\hspace{1em}(28)}$$

  然后根据转换关系：

$$H_{k,k-1}=H_{k-1,k-1}{\Phi }_{k-1,k}\text{\hspace{1em}(29)}$$

有：

$$\begin{array}{rl}{H}_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}=& {\Phi }_{k-1,k}^{\mathrm{T}}\{I-H_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}{H}_{k-1,k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}(Q_{k-1}^{-1}\\ & +{\Gamma }_{k-1}^{\mathrm{T}}{\Phi }_{k-1,k}^{\mathrm{T}}{H}_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}{H}_{k-1,k-1}{\Phi }_{k-1,k}{\Gamma }_{k-1}{)}^{-1}\\ & \cdot {\Gamma }_{k-1}^{\mathrm{T}}{\Phi }_{k-1,k}^{\mathrm{T}}\}{H}_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}\end{array}\text{\hspace{1em}(30)}$$

则：

$$\begin{array}{rl}& (H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}){\Phi }_{k,k-1}(H_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}{H}_{k-1,k-1}{)}^{-1}\\ & \cdot H_{k-1,k-1}^{\mathrm{T}}{W}_{k-1,k-1}=H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}\end{array}\text{\hspace{1em}(31)}$$

结合式 (18)，当 $j=k-1$ 和 $k$ 时得到式 (25)。

  下一个目标是得到卡尔曼增益矩阵 $G_k$ 的递推公式。首先有：

$$G_k=P_{k,k}{C}_k^{\mathrm{T}}{R}_k^{-1}\text{\hspace{1em}(32)}$$

式中：

$$P_{k,k}=(H_{k,k}^{\mathrm{T}}{W}_{k,k}{H}_{k,k}{)}^{-1}\text{\hspace{1em}(33)}$$

且令：

$$P_{k,k-1}=(H_{k,k-1}^{\mathrm{T}}{W}_{k,k-1}{H}_{k,k-1}{)}^{-1}\text{\hspace{1em}(34)}$$

又因：

$$P_{k,k}^{-1}=P_{k,k-1}^{-1}+C_k^{\mathrm{T}}{R}_k^{-1}{C}_k\text{\hspace{1em}(35)}$$

可得：

$$P_{k,k}=P_{k,k-1}-P_{k,k-1}{C}_k^{\mathrm{T}}(C_k{P}_{k,k-1}{C}_k^{\mathrm{T}}+R_k{)}^{-1}{C}_k{P}_{k,k-1}\text{\hspace{1em}(36)}$$

可以证明：

$$G_k=P_{k,k-1}{C}_k^{\mathrm{T}}(C_k{P}_{k,k-1}{C}_k^{\mathrm{T}}+R_k{)}^{-1}\text{\hspace{1em}(37)}$$

因此：

$$P_{k,k}=(I-G_k{C}_k)P_{k,k-1}\text{\hspace{1em}(38)}$$

此外，还有：

$$P_{k,k-1}=A_{k-1}{P}_{k-1,k-1}{A}_{k-1}^{\mathrm{T}}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^{\mathrm{T}}\text{\hspace{1em}(39)}$$

  应用式 (38) 和式 (39) 及初始矩阵 $P_{0,0}$，可得 $P_{k-1,k-1},P_{k,k-1},G_k$ 和 $P_{k,k}(k=1,2,\cdots )$ 的递推计算方法。首先有：

$$\begin{array}{rl}{P}_{k,k-1}& =E(x_k-{\hat{x}}_{k|k-1})(x_k-{\hat{x}}_{k|k-1}{)}^{\mathrm{T}}\\ & =\mathrm{Var}(x_k-{\hat{x}}_{k|k-1})\end{array}\text{\hspace{1em}(40)}$$

还有：

$$P_{k,k}=E(x_k-{\hat{x}}_{k|k})(x_k-{\hat{x}}_{k|k}{)}^{\mathrm{T}}=\mathrm{Var}(x_k-{\hat{x}}_{k|k})\text{\hspace{1em}(41)}$$

特别地，当 $k=0$ 时，有：

$$P_{0,0}=E(x_0-E{x}_0)(x_0-E{x}_0{)}^{\mathrm{T}}=\mathrm{Var}(x_0)\text{\hspace{1em}(42)}$$

  最后，联合上面得到的所有结果，得到式 (3) 所示的状态空间描述的线性随机系统的卡尔曼滤波过程：

$$\{\begin{array}{l}{P}_{0,0}=\mathrm{Var}(x_0)\\ P_{k,k-1}=A_{k-1}{P}_{k-1,k-1}{A}_{k-1}^{\mathrm{T}}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^{\mathrm{T}}\\ G_k=P_{k,k-1}{C}_k^{\mathrm{T}}(C_k{P}_{k,k-1}{C}_k^{\mathrm{T}}+R_k{)}^{-1}\\ P_{k,k}=(I-G_k{C}_k)P_{k,k-1}\\ {\hat{x}}_{0|0}=E(x_0)\\ {\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+G_k(v_k-C_k{\hat{x}}_{k|k-1})\\ k=1,2,\cdots \end{array}\text{\hspace{1em}(43)}$$

总结

  现在考虑具有确定性控制输入 $\{u_k\}$ 的常规线性确定性/随机系统。考虑状态空间模型：

$$\{\begin{array}{l}{x}_{k+1}=A_k{x}_k+B_k{u}_k+{\Gamma }_k{\xi }_k\\ v_k=C_k{x}_k+D_k{u}_k+{\eta }_k\end{array}\text{\hspace{1em}(44)}$$

式中 $\{u_k\}$ 是 $m$ 维向量序列 $(1⩽m⩽n)$。

  将确定性解叠加到式 (43) 上，则可得到该系统的卡尔曼滤波过程：

$$\{\begin{array}{l}{P}_{0,0}=\mathrm{Var}(x_0)\\ P_{k,k-1}=A_{k-1}{P}_{k-1,k-1}{A}_{k-1}^{\mathrm{T}}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^{\mathrm{T}}\\ G_k=P_{k,k-1}{C}_k^{\mathrm{T}}(C_k{P}_{k,k-1}{C}_k^{\mathrm{T}}+R_k{)}^{-1}\\ P_{k,k}=(I-G_k{C}_k)P_{k,k-1}\\ {\hat{x}}_{0|0}=E(x_0)\\ {\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+G_k(v_k-D_k{u}_k-C_k{\hat{x}}_{k|k-1})\\ k=1,2,\cdots \end{array}\text{\hspace{1em}(45)}$$