本章将介绍含有高斯随机变量的线性系统状态估计问题中的一些经典结论，包括重要的卡尔曼滤波器。我们将从离散时间的批量（batch）优化问题开始讨论，这可以导出随后非线性情况下的一些重要的结论，作为后文的铺垫。从批量式处理过程中，我们将导出递归式（recursive）算法的流程。最后，我们再讨论最重要的卡尔曼滤波器。

当然，全部的这部分内容比较多，这里将会分为三篇文章进行论述。本文主要讨论的是：离散时间的批量估计。

问题定义

对于离散时间线性时变系统，

定义运动模型：

$$x_k=A_{k-1}{x}_{k-1}+v_k+w_k$$

其中，$k=1,2,...,K$。

定义观测模型：

$$y_k=C_k{x}_k+n_k$$

其中，$k=0,1,...,K$。各变量的含义如下：

• 系统状态：$x_k\in R^N$
• 初始状态：$x_0\in R^N\sim N({\check{x}}_0,{\check{P}}_0)$
• 输入：$v_k\in R^N$
• 过程噪声：$w_k\in R^N\in N(0,Q_k)$
• 测量：$y_k\in R^M$
• 测量噪声：$n_k\in R^M\sim N(0,R_k)$

这些变量中，除了$v_k$为确定性变量之外，其他的都是随机变量。噪声和初始状态一般假设为互不相关的，并且在各个时刻与自己也互不相关。矩阵$A_k\in R^{N\ast N}$称为转移矩阵，$C_k\in R^{M\ast N}$称为观测矩阵。

尽管想知道整个系统在所有时刻的状态，然而仅仅知道以下的几个变量，并且要根据它们来估计状态${\hat{x}}_k$：

• 初始状态${\check{x}}_0$，以及相应的初始协方差矩阵${\check{P}}_0$
• 输入量$v_{0:K}$，以及它的噪声协方差$Q_k$
• 观测数据$y_{0:K}$，以及它的噪声协方差$R_k$

状态估计问题是指：在时间点$k$上，基于初始的状态信息${\check{x}}_0$、一系列观测数据$y_{0:K}$、一系列输入$v_{1:K}$，以及系统的运动和观测模型，来计算系统的真实状态的估计值${\hat{x}}_k$。

为了求解更加困难的状态估计问题，先讨论批量的线性高斯系统（linear-Gaussian,LG）状态估计问题。所谓批量，就是可以一次性使用所有的数据，来推算所有时刻的状态。但是这种方式无法在实时场景下使用，因为不可以用未来的信息来估计过去时刻的状态。因此，还需要用到递归式的方法。

为了显示各种方法之间的联系，从两个不同的途径着手解决LG系统的估计问题。

• 贝叶斯推断：从状态的先验概率密度函数出发，通过初始状态、输入、运动方程和观测数据，计算状态的后验概率密度函数
• 最大后验估计：利用优化理论，寻找给定信息下（初始状态、输入、观测）的最大后验估计

尽管两种方法的本质上是不同的，但是对于LG系统，它们将给出相同的结论。

原因是：在LG系统中，贝叶斯后验概率正好也是高斯的，所以优化方法会找到高斯分布的最大值（也就是它的模），这也正好是高斯的均值。细化一点讲：贝叶斯推断计算出该状态的后验概率密度函数，然后计算出其均值和方差来描述它；最大后验估计则是寻找最大后验估计，并且找到达到最大值时的状态。此时，前者为均值，后者为模。两者相等。

这一点非常重要，在步入非线性、非高斯之后，一个分布的均值和模不再重合，这使得两种方法给出不同的答案。

最大后验估计

在批量估计中，最大后验估计MAP的目标是求解这样的一个问题：

$$\hat{x}=argmax(p(x|v,y))$$

即：在给定先验信息和所有时刻的运动$v$、观测$y$的情况下，推测出所有时刻的最优状态下的$\hat{x}$。定义几个宏观的变量：

$$\begin{array}{rl}x& =x_{0:K}=(x_0,x_1,...,x_K)\\ v& =({\check{x}}_0,v_{1:K})=({\check{x}}_0,v_1,v_2,...,v_K)\\ y& =y_{0:K}=(y_0,y_1,...,y_K)\end{array}$$

这里相当于省略变量的下标，表示所有的时刻。需要注意的是：${\check{x}}_0$表示初值，这里将它和输入量放在一起，表明先验的信息。

利用贝叶斯公式重写MAP公式：

$$\begin{array}{rl}\hat{x}& =argmax(p(x|v,y))\\ & =argmax(\frac{p(y|v,x)p(x|v)}{p(y|v)})\\ & =argmax(p(y|x)p(x|v))\end{array}$$

这里将分母去掉，因为它和$x$无关。同时省略$p(y|v,x)$中的$v$，因为$y$和$x$相关，和$v$不相关。

接下来做出一个重要的假设：对于所有时刻$k=0,1,...,K$，所有的噪声项$w_k$和$n_k$之间总是无关的。这使得可以用贝叶斯公式对$p(y|x)$进行因子分解：

$$p(y|x)=\prod _{k=0}^{K}p(y_k|x_k)$$

原因：$y_k$只与$x_k$有关。

同时，也可以用贝叶斯公式对$p(x|v)$进行因子分解：

$$p(x|v)=p(x_0|{\check{x}}_0)\prod _{k=1}^{K}p(x_k|x_{k-1},v_k)$$

原因：$x_k$只与$x_{k-1}$、$v_k$有关。

在线性系统中，高斯密度函数可以展开为：

$$p(x_0|{\check{x}}_0)=\frac{1}{\sqrt{(2\pi {)}^N}det{\check{P}}_0}exp(-\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0))$$

$$p(x_k|x_{k-1},v_k)=\frac{1}{\sqrt{(2\pi {)}^N}det{Q}_k}exp(-\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k))$$

$$p(y_k|x_k)=\frac{1}{\sqrt{(2\pi {)}^N}det{R}_k}exp(-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k)))$$

需要注意：必须保证${\check{P}}_0$、$Q_k$、$R_k$是可逆的。事实上，他们通常是正定的，因而也必然是可逆的。因此：

$$ln(p(y|x)p(x|v))=ln(p(x_0|{\check{x}}_0))+\sum _{k=1}^{K}ln(p(x_k|x_{k-1},v_k))+\sum _{k=0}^{K}ln(p(y_x|x_k))$$

去掉与$x$无关的一些项，即一些常量，整理一下：

$$\begin{array}{rl}& ln(p(y|x)p(x|v))\\ =& -\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0)+\\ & \sum _{k=1}^K(-\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k))+\\ & \sum _{k=0}^K(-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k))\end{array}$$

定义下面这些变量：

$k=0$时：

$$J_{v,k}=\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0)$$

$k=1,2,...,K$时：

$$J_{v,k}=\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k)$$

$k=0,1,...,K$时：

$$J_{y,k}=\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k)$$

这些都是平方马氏距离，据此可以定义整体的目标函数$J(x)$。

通过最小化这个目标函数，可以求解出自变量$x$的值：

$$J(x)=\sum _{k=0}^K(J_{v,k}(x)+J_{y,k}(x))$$

$$\hat{x}=argmin(J(x))$$

这是一个无约束的优化问题，对于状态变量$x$本身并没有任何约束。

由于$J_{v,k}(x)$和$J_{y,k}(x)$都是$x_k$的二次形式，因此还可以进一步简化。即：将所有的数据排成一列，提升形式。那么可以把所有时刻的状态组成一个向量$x$，并把所有时候已知的数据组成一个向量$z$：

$$z=\left[\begin{array}{c}{\check{x}}_0\\ v_1\\ v_2\\ ⋮\\ v_K\\ y_0\\ y_1\\ ⋮\\ y_K\end{array}\right]$$

$$x=\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_K\end{array}\right]$$

然后定义以下的块矩阵：

$$H=\left[\begin{array}{cccc}1& & & \\ -A_0& 1& & \\ & \ddots & \ddots & \\ & & -A_{K-1}& 1\\ C_0& & & \\ & C_1& & \\ & & \ddots & \\ & & & C_K\end{array}\right]$$

$$W=\left[\begin{array}{cccccccc}{\check{P}}_0& & & & & & & \\ & Q_1& & & & & & \\ & & \ddots & & & & & \\ & & & Q_K& & & & \\ & & & & R_0& & & \\ & & & & & R_1& & \\ & & & & & & \ddots & \\ & & & & & & & R_K\end{array}\right]$$

根据这些定义，可以将目标函数写成：

$$J(x)=\frac{1}{2}(z-Hx{)}^T{W}^{-1}(z-Hx)$$

这正是$x$的二次形式。同时有：

$$p(z|x)=\eta exp(-\frac{1}{2}(z-Hx{)}^T{W}^{-1}(z-Hx))$$

其中，$\eta$是归一化因子。

因为$J(x)$刚好是一个抛物面，可以解析地找到它的最小值，只需要让目标函数相对于自变量的偏导数为零：

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

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

该方程的解$\hat{x}$是经典的批量最小二乘法的解，同时也等价于传统估计理论中的固定区间平滑算法。

批量最小二乘法的求解利用了矩阵求伪逆的方法。从计算角度上说，并不会真的去计算$H^T{W}^{-1}H$的逆，它是一种特殊的对角块结构，可以利用稀疏的求解算法来更高效地求解。

贝叶斯推断

现在看一看如何计算全贝叶斯后验概率$p(x|v,y)$，而不是简单的最大化它。

在这种情况下，可以用初始状态和输入来建立状态的先验估计：$p(x|v)$。用运动方程来建立先验：

$$x_k=A_{k-1}{x}_{k-1}+v_k+w_k$$

可以得到：

$$\begin{array}{rl}{x}_0& ={\check{x}}_0+w_0\\ x_1& =A_0{x}_0+v_1+w_1=A_0({\check{x}}_0+w_0)+v_1+w_1\\ x_2& =A_1{x}_1+v_2+w_2=A_1(A_0({\check{x}}_0+w_0)+v_1+w_1)+v_2+w_2\\ x_k& =...\end{array}$$

在提升形式中，可以写成：

$$x=A(v+w)$$

其中，

$$A=\left[\begin{array}{cccccc}1& & & & & \\ A_0& 1& & & & \\ A_1{A}_0& A_1& 1& & & \\ ⋮& ⋮& ⋮& \ddots & & \\ A_{K-2}...A_0& A_{K-2}...A_1& A_{K-2}...A_2& \dots & 1& \\ A_{K-1}...A_0& A_{K-1}...A_1& A_{K-1}...A_2& \dots & A_{K-1}& 1\end{array}\right]$$

$$v=\left[\begin{array}{c}{\check{x}}_0\\ v1\\ v2\\ ⋮\\ v_K\end{array}\right]$$

$$w=\left[\begin{array}{c}{w}_0\\ w1\\ ⋮\\ w_K\end{array}\right]$$

于是，提升之后的均值为：

$$\check{x}=E[x]=E[A(v+w)]=Av$$

提升的协方差为：

$$\begin{array}{rl}\check{P}& =E[(x-E[x])(x-e[x]{)}^T]\\ & =E[(Aw)(Aw{)}^T]\\ & =AE[w{w}^T]A^T\\ & =AQ{A}^T\end{array}$$

其中，$Q=diag({\check{P}}_0,Q_1,Q_2,...,Q_K)$。

那么，先验就可以简洁的写成：

$$p(x|v)=N(\check{x},\check{P})=N(Av,AQ{A}^T)$$

再看看观测，观测模型为：

$$y_k=C_k{x}_k+n_k$$

写成提升形式：

$$y=Cx+n$$

其中，

$$C=\left[\begin{array}{cccc}{C}_0& & & \\ & C_1& & \\ & & \ddots & \\ & & & C_K\end{array}\right]$$

$$n=\left[\begin{array}{c}{n}_0\\ n_1\\ ⋮\\ n_K\end{array}\right]$$

于是，提升之后的状态、观测联合概率密度函数可写成：

$$p(x,y|v)=N(\left[\begin{array}{c}\check{x}\\ C\check{x}\end{array}\right],\left[\begin{array}{cc}\check{P}& \check{P}{C}^T\\ C\check{P}& C\check{P}{C}^T+R\end{array}\right])$$

其中，$R=E[n{n}^T]=diag(R_0,R_1,...R_K)$。

下面的几个推导过程需要一定的概率论基础知识，不太熟悉的可以参考文章：【状态估计】概率论基础。

由于联合概率密度，总可以将其分解成两个因子的乘积（条件概率乘以边缘概率）。那么：这个式子可以因式分解：

$$p(x,y|v)=p(x|v,y)p(y|v)$$

第一个因子表示的就是全贝叶斯后验概率，根据高斯推断，可以得到：

$$p(x|v,y)=N(\check{x}+\check{P}{C}^T(C\check{P}{C}^T+R{)}^{-1}(y-C\check{x}),\check{P}-\check{P}{C}^T(C\check{P}{C}^T+R{)}^{-1}C\check{P})$$

根据SMW恒等式，可以转换为：

$$p(x|v,y)=N(({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}({\check{P}}^{-1}\check{x}+C^T{R}^{-1}y),({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1})$$

因而，

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

为显示此方法与MAP方法的联系，对后验均值项进行整理：

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

带入$\check{x}=Av$、$\check{P}=AQ{A}^T$，得到：

$$(A^{-T}{Q}^{-1}{A}^{-1}+C^T{R}^{-1}C)\hat{x}=A^{-T}{Q}^{-1}v+C^T{R}^{-1}y$$

这里需要计算$A^{-1}$，不过它有一个很好看的形式：

$$A^{-1}=\left[\begin{array}{ccccc}1& & & & \\ -A_0& 1& & & \\ & -A_1& 1& & \\ & & \ddots & \ddots & \\ & & & -A_{K-1}& 1\end{array}\right]$$

如果定义：

$$z=\left[\begin{array}{c}v\\ y\end{array}\right]$$

$$H=\left[\begin{array}{c}{A}^{-1}\\ C\end{array}\right]$$

$$W=\left[\begin{array}{cc}Q& \\ & R\end{array}\right]$$

就可以将其写成：

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

这和之前MAP的方法的解完全一致。

重申一遍，在LG系统中，贝叶斯推断给出了和MAP方法一致的解，本质上是因为LG系统的贝叶斯后验概率仍然是高斯的，而高斯函数的均值和模是一样的。

存在性、唯一性与能观性

LG系统中，无论是贝叶斯推断还是MAP方法，最终的解都是$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z$。那么，什么时候该式有唯一解呢？

线性代数理论中可知，当且仅当$H^T{W}^{-1}H$可逆时，$\hat{x}$存在且唯一。即：

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

那么，$H^T{W}^{-1}H$什么时候可逆呢？

线性代数理论中可知，由于$\hat{x}$的维数 $dim\hat{x}=N(K+1)$，因此可逆的充要条件为：

$$rank(H^T{W}^{-1}H)=N(K+1)$$

又因为，一般情况下，假设$W^{-1}$实对称且正定，因此可以得到：

$$rank(H^T{W}^{-1}H)=rank(H^{T}H)=rank(H^T)$$

即：要求$H^T$有$N(K+1)$个线性无关的行/列向量。

接下来，情况分为两种，需要分类讨论：

• 对初始状态有先验知识：${\check{x}}_0$

• 对初始状态没有先验知识

有先验知识

将$H^T$展开：

$$rank(H^T)=rank\left[\begin{array}{cccccccc}1& -A_0^T& & & C_0^T& & & \\ & \ddots & \ddots & & & \ddots & & \\ & & 1& -A_{K-1}^T& & & C_{K-1}^T& \\ & & & 1& & & & C_K^T\end{array}\right]$$

这是一个阶梯型的矩阵。很显然，它的每一行都是线性无关的，因而它是满秩的，因此，对初始状态${\check{x}}_0$有先验知识，总是可以得到一个唯一解$\hat{x}$。

没有先验知识

$H^T$中每一列的块都表示了一部分有关系统的信息，而第一列表示对初始状态的信息。因此，没有先验知识的情况下，即去掉第一列：

$$rank(H^T)=rank\left[\begin{array}{ccccccccc}-A_0^T& & & & C_0^T& & & & \\ 1& -A_1^T& & & & C_1& & & \\ & \ddots & \ddots & & & & \ddots & & \\ & & 1& -A_{K-1}^T& & & & C_{K-1}^T& \\ & & & 1& & & & & C_K^T\end{array}\right]$$

该矩阵有$K+1$行，为了判断其秩，将其最上一行移到最下，得：

$$rank(H^T)=rank\left[\begin{array}{ccccccccc}1& -A_1^T& & & & C_1& & & \\ & \ddots & \ddots & & & & \ddots & & \\ & & 1& -A_{K-1}^T& & & & C_{K-1}^T& \\ & & & 1& & & & & C_K^T\\ -A_0^T& & & & C_0^T& & & & \end{array}\right]$$

除了最后一行，矩阵的剩下部分是阶梯型矩阵，因此，通过初等行变换，对最后一行进行化简操作：

$$rank(H^T)=rank\left[\begin{array}{ccccccccc}1& -A_1^T& & & & C_1& & & \\ & \ddots & \ddots & & & & \ddots & & \\ & & 1& -A_{K-1}^T& & & & C_{K-1}^T& \\ & & & 1& & & & & C_K^T\\ & & & & C_0^T& A_0^T{C}_1^T& \dots & A_0^T{A}_1^T...A_{K-2}^T{C}_{K-1}^T& A_0^T{A}_1^T...A_{K-1}^T{C}_K^T\end{array}\right]$$

可以看出，对$H^T$的秩的判断，归结于右下角部分的秩是否为$N$：

$$rank\left[\begin{array}{ccccc}{C}_0^T& A_0^T{C}_1^T& \dots & A_0^T{A}_1^T...A_{K-2}^T{C}_{K-1}^T& A_0^T{A}_1^T...A_{K-1}^T{C}_K^T\end{array}\right]=N$$

假设系统为时不变系统，即对于所有的$k$，$A_k=A$，$C_k=C$。并且假设$K\gg N$，定义能观性矩阵$O$为：

$$O=\left[\begin{array}{c}C\\ CA\\ ⋮\\ C{A}^{N-1}\end{array}\right]$$

秩条件就可以转换为：

$$rank(O)=N$$

注：这边的证明过程略。

也就是说，对初始状态${\check{x}}_0$有先验知识，当$K\gg N$时，才可以得到一个唯一解$\hat{x}$。

MAP的协方差

在MAP方法中，只得到了最大的贝叶斯后验概率，并没有得到其协方差。然而，在贝叶斯推断中，得到了贝叶斯后验概率的均值，及其协方差。

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

其中，$\hat{x}$为均值，$H^T{W}^{-1}H$为协方差的逆，$H^T{W}^{-1}z$称为信息向量。