【状态估计】线性高斯系统的状态估计——离散时间的递归滤波

前两篇文章介绍了离散时间的批量估计、离散时间的递归平滑，本文着重介绍离散时间的递归滤波。

前两篇位置：【状态估计】线性高斯系统的状态估计——离散时间的批量估计、【状态估计】线性高斯系统的状态估计——离散时间的递归平滑。

离散时间的递归滤波算法

批量优化的方案及其对应的平滑算法方案，是LG问题下能找到的最好的方法了。它利用了所有能用的数据，来估计所有时刻的状态。不过这个方法有一个致命的问题：无法在线运行，因为它需要用未来时刻的信息估计过去的状态。为了在实时场合下使用，当前时刻的状态只能由它之前时间的信息决定，而卡尔曼滤波则是对这样一个问题的传统解决方案。

之前讲述了如何从Cholesky分解推导出卡尔曼滤波，实际上并不需要这么复杂，接下来介绍几种推导卡尔曼滤波的方法。

通过MAP推导卡尔曼滤波

假设已经有了$k-1$时刻的前向估计：

{ x ^ k − 1 , P ^ k − 1 } \{\hat x_{k-1},\hat P_{k-1}\} {x^k−1​,P^k−1​}

这两个量是根据初始时刻到$k-1$时刻的数据推导出来的。目标是计算：

{ x ^ k , P ^ k } \{\hat x_k,\hat P_k\} {x^k​,P^k​}

其中，需要用到直到$k$时刻的数据。实际上没有必要再从初始时刻开始，而是简单地用$k-1$时刻的状态，以及$k$时刻的$v_k$、$y_k$就能估计出$k$时刻的状态了：

{ x ^ k − 1 , P ^ k − 1 , v k , y k } − − > { x ^ k , P ^ k } \{\hat x_{k-1},\hat P_{k-1},v_k,y_k\}-->\{\hat x_k,\hat P_k\} {x^k−1​,P^k−1​,vk​,yk​}−−>{x^k​,P^k​}

为了推导这个过程，定义：

$$z=\left[\begin{array}{c}{\hat{x}}_{k-1}\\ v_k\\ y_k\end{array}\right]$$

$$H=\left[\begin{array}{cc}1& \\ -A_{k-1}& 1\\ & C_k\end{array}\right]$$

$$W=\left[\begin{array}{ccc}{\hat{P}}_{k-1}& & \\ & Q_k& \\ & & R_k\end{array}\right]$$

$$\hat{x}=\left[\begin{array}{c}{\hat{x}}_{k-1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]$$

其中，${\hat{x}}_{k-1}^{{}^{\prime }}$表示使用了直到$k$时刻的数据计算的$k-1$时刻的状态估计，而${\hat{x}}_{k-1}$表示使用了直到$k-1$时刻的数据计算的$k-1$时刻的状态估计，两者之间相差一个$k$时刻的后向估计。

通常MAP的最优解$\hat{x}$写成：

$$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z$$

将上面的定义代入：

$$\left[\begin{array}{cc}{\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}& -A_{k-1}^T{Q}_k^{-1}\\ -Q_k^{-1}{A}_{k-1}& Q_k^{-1}+C_k^T{R}_k^{-1}{C}_k\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{k-1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k\\ Q_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k\end{array}\right]$$

由于并不关心${\hat{x}}_{k-1}^{{}^{\prime }}$的实际值，因此可以将它边缘化，等式两侧左乘：

$$\left[\begin{array}{cc}1& 0\\ Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}& 1\end{array}\right]$$

这是一元线性方程的行操作，于是变成：

$$\left[\begin{array}{cc}{\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}& -A_{k-1}^T{Q}_k^{-1}\\ 0& Q_k^{-1}-Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}{A}_{k-1}^T{Q}_k^{-1}+C_k^T{R}_k^{-1}{C}_k\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{k-1}^{{}^{\prime }}\\ {\hat{x}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k\\ Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}({\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k)+Q_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k\end{array}\right]$$

因此，只需要关注${\hat{x}}_k$：

$$(Q_k^{-1}-Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}{A}_{k-1}^T{Q}_k^{-1}+C_k^T{R}_k^{-1}{C}_k){\hat{x}}_k=Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}({\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k)+Q_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k$$

根据SMW恒等式：

$$Q_k^{-1}-Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}{A}_{k-1}^T{Q}_k^{-1}=(Q_k+A_{k-1}{\hat{P}}_{k-1}{A}_{k-1}^T{)}^{-1}$$

定义：

$${\check{P}}_k=Q_k+A_{k-1}{\hat{P}}_{k-1}{A}_{k-1}^T$$

$${\hat{P}}_k=({\check{P}}_k^{-1}+C_k^T{R}_k^{-1}{C}_k{)}^{-1}$$

原式可化简为：

$$\begin{array}{rl}{\hat{P}}_k^{-1}{\hat{x}}_k& =Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}({\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}-A_{k-1}^T{Q}_k^{-1}{v}_k)+Q_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k\\ & =Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}{\hat{P}}_{k-1}^{-1}{\hat{x}}_{k-1}+(Q_k^{-1}-Q_k^{-1}{A}_{k-1}({\hat{P}}_{k-1}^{-1}+A_{k-1}^T{Q}_k^{-1}{A}_{k-1}{)}^{-1}{A}_{k-1}^T{Q}_k^{-1})v_k+C_k^T{R}_k^{-1}{y}_k\\ & ={\check{P}}_k^{-1}{A}_{k-1}{\hat{x}}_{k-1}+{\check{P}}_k^{-1}{v}_k+C_k^T{R}_k^{-1}{y}_k\\ & ={\check{P}}_k^{-1}(A_{k-1}{\hat{x}}_{k-1}+v_k)+C_k^T{R}_k^{-1}{y}_k\\ & ={\check{P}}_k^{-1}{\check{x}}_k+C_k^T{R}_k^{-1}{y}_k\end{array}$$

梳理一下整个过程：

预测：

$$\begin{array}{rl}{\check{x}}_k& =A_{k-1}{\hat{x}}_{k-1}+v_k\\ {\check{P}}_k& =A_{k-1}{\hat{P}}_{k-1}{A}_{k-1}^T+Q_k\end{array}$$

更新：

$$\begin{array}{rl}{\hat{P}}_k& =({\check{P}}_k^{-1}+C_k^T{R}_k^{-1}{C}_k{)}^{-1}\\ {\hat{P}}_k^{-1}{\hat{x}}_k& ={\check{P}}_k^{-1}{\check{x}}_k+C_k^T{R}_k^{-1}{y}_k\end{array}$$

这是逆协方差形式（信息形式）的卡尔曼滤波。为了得到经典形式的卡尔曼滤波，需要定义卡尔曼增益$K_k$：

$$K_k={\hat{P}}_k{C}_k^T{R}_k^{-1}$$

经过化简，更新：

$$\begin{array}{rl}{K}_k& ={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{-1}\\ {\hat{P}}_k& =(1-K_k{C}_k){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-C_k{\check{x}}_k)\end{array}$$

其中，$y_k-C_k{\check{x}}_k$称为更新量，指的是实际与期望观测量的误差，而卡尔曼增益则是这部分更新量对估计值的权重。

通过贝叶斯推断推导卡尔曼滤波

使用贝叶斯推断方法还能够以更简洁的方式推出卡尔曼滤波。假设$k-1$时刻的高斯先验为：

$$p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$$

首先，对于预测部份，考虑最近时刻的输入$v_k$，来计算$k$时刻的先验：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=N({\check{x}}_k,{\check{P}}_k)$$

其中：

$${\check{x}}_k=E[x_k]=E[A_{k-1}{x}_{k-1}+v_k+w_k]=A_{k-1}{\hat{x}}_{k-1}+v_k$$

$$\begin{array}{rl}{\check{P}}_k& =E[(x_k-E[x_k])(x_k-E[x_k]{)}^T]\\ & =A_{k-1}E[(x_{k-1}-{\hat{x}}_{k-1})(x_{k-1}-{\hat{x}}_{k-1}{)}^T]A_{k-1}^T+E[w_k{w}_k^t]\\ & =A_{k-1}{\hat{P}}_{k-1}{A}_{k-1}^T+Q_k\end{array}$$

然后，对于更新部分，将状态与最新一次测量（即$k$时刻）写成联合高斯分布的形式：

$$p(x_k,y_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=N(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right])=N(\left[\begin{array}{c}{\check{x}}_k\\ C_k{\check{x}}_k\end{array}\right],\left[\begin{array}{cc}{\check{P}}_k& {\check{P}}_k{C}_k^T\\ C_k{\check{P}}_k& C_k{\check{P}}_k{C}_k^T+R_k\end{array}\right])$$

根据高斯推断，可以得到：

$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y_k-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$

代入之前的结果，有：

$$\begin{array}{rl}{K}_k& ={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{-1}\\ {\hat{P}}_k& =(1-K_k{C}_k){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-C_k{\check{x}}_k)\end{array}$$

这与MAP给出的更新步骤的方程是完全一致的。

重申一遍，这件事情的根本在于使用了线性模型，且噪声和先验也都是高斯的。在这些条件下，后验概率也是高斯的，于是它的均值和模正巧是一样的。然而在使用非线性模型之后就不能保证这个性质了。

从增益最优化的角度来看卡尔曼滤波

通常来说，卡尔曼滤波是线性高斯系统下的最优解。因此，也可以从其他的角度来看卡尔曼滤波的最优特性。下面介绍其中的一个：

假设有一个估计器，形式如下：

$${\hat{x}}_k={\check{x}}_k+K_k(y_k-C_k{\check{x}}_k)$$

但是此时并不知道如何选取$K_k$的值，才能正确地衡量修正部分的权重。如果定义状态的误差为（估计值 - 真值）：

$${\hat{e}}_k={\hat{x}}_k-x_k$$

那么有：

$$\begin{array}{rl}{\hat{P}}_k& =E[{\hat{e}}_k{\hat{e}}_k^T]\\ & =E[({\hat{x}}_k-x_k)({\hat{x}}_k-x_k{)}^T]\\ & =E[({\check{x}}_k+K_k(C_k{x}_k+n_k-C_k{\check{x}}_k)-x_k)({\check{x}}_k+K_k(C_k{x}_k+n_k-C_k{\check{x}}_k)-x_k{)}^T]\\ & =E[((1-K_k{C}_k)({\check{x}}_k-x_k)+K_k{n}_k)((1-K_k{C}_k)({\check{x}}_k-x_k)+K_k{n}_k{)}^T]\\ & =(1-K_k{C}_k)E[({\check{x}}_k-x_k)({\check{x}}_k-x_k{)}^T](1-K_k{C}_k{)}^T+K_{k}E[C_k{C}_k^T]K_k^T\\ & =(1-K_k{C}_k){\check{P}}_k(1-K_k{C}_k{)}^T+K_k{R}_k{K}_k^T\\ & ={\check{P}}_k-K_k{C}_k{\check{P}}_k-{\check{P}}_k{C}_k^T{K}_k^T+K_k(C_k{\check{P}}_k{C}_k^T+R_k)K_k^T\end{array}$$

接下来最小均方差开始正式登场了。由于协方差矩阵的对角线元素就是方差，这样一来，把协方差矩阵的对角线元素求和，用$tr$来表示这种算子，它的学名叫矩阵的迹。

于是，可以由它定义出一个代价函数：

$$\begin{array}{rl}J(K_k)& =tr(E[{\hat{e}}_k{\hat{e}}_k^T])\\ & =tr({\check{P}}_k)-2tr(K_k{C}_k{\check{P}}_k)+tr(K_k(C_k{\check{P}}_k{C}_k^T+R_k)K_k^T)\end{array}$$

最小均方差就是使得$J(K_k)$最小，对未知量$K_k$求导，令导函数等于0：

$$\frac{dJ(K_k)}{d{K}_k}=-2(C_k{\check{P}}_k{)}^T+2K_k(C_k{\check{P}}_k{C}_k^T+R_k)$$

因此：

$$K_k={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{-1}$$

这正是卡尔曼增益的通常表达式。

关于卡尔曼滤波的讨论

以下是卡尔曼滤波的要点：

1. 对于高斯噪声的线性系统，卡尔曼滤波器是最优线性无偏估计；
2. 必须有初始状态： { x ˇ 0 , P ˇ 0 } \{\check x_0,\check P_0\} {xˇ0​,Pˇ0​}；
3. 协方差部分与均值部分可以独立地递推。有时甚至可以计算一个固定的$K_k$，用于所有时刻的均值修正，这种做法称为固定状态的卡尔曼滤波。