机器人学中的状态估计学习笔记（二）第三章线性高斯系统的状态估计

3.1 离散时间的批量估计问题

3.1.1 问题定义

  在离散时间线性时变系统中，定义运动和观测模型如下：$$运动方程：x_k=A_{k-1}{x}_{k-1}+v_k+w_k,k=1,...,K$$$$观测方程：y_k=C_k{x}_k+n_k,k=0,...,K$$其中k为时间下标，最大值为K。各变量的含义如下：$$系统状态：x_k\in {\mathbb{R}}^N$$$$初始状态：x_0\in {\mathbb{R}}^N\sim N({\check{x}}_0,{\check{P}}_0))$$$$输入：v_k\in {\mathbb{R}}^N$$$$过程噪声：w_k\in {\mathbb{R}}^N\sim N(0,Q_k)$$$$测量：y_k\in {\mathbb{R}}^M$$$$测量噪声：n_k\in {\mathbb{R}}^M\sim N(0,R_k)$$其中除了v_k为确定性变量之外，其他的变量都是随机变量。噪声和初始状态一般假设互不相关的，并且在各个时刻与自己也互不相关。矩阵$A_k\in {\mathbb{R}}^{N\times N}$称为转移矩阵，$C_k\in {\mathbb{R}}^{M\times N}$称为观测矩阵。
  一般情况只知道以下几个变量，并根据它们来估计状态 ${\hat{x}}_k$:

1. 初始状态${\check{x}}_0$，以及相应的初始协方差矩阵${\check{P}}_0$。
2. 输入量v_k，通常来自控制器，是已知的；它的噪声协方差矩阵是Q_k。
3. 观测数据y_k,meas是观测变量y_k的一次实现，它的协方差为R_k。

  状态估计问题是指：在k个（一个或多个）时间点上，基于初始的状态信息${\check{x}}_0$、一系列观测数据y_0：K，meas、一系列输入v_1：K，以及系统的运动模型和观测模型，来计算系统的真实状态的估计值 ${\hat{x}}_k$
  为了显示各个方法之间的联系，从两个不同的途径着手解决批量LG系统的估计问题：

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

  以上两种方法本质上是不同的，但在LG系统中，它们将给出同样的结论。主要原因在于，在LG系统中，贝叶斯后验概率正好是高斯的。所以优化方法会找到高斯分布的最大值（也就是它的模），这也正是高斯的均值。但在非线性非高斯系统中，一个分布的均值和模将不再重合，使得这两类方法给出不同的答案。

3.1.2 最大后验估计

  在批量估计中，MAP的目标是求解这样一个问题：$$\hat{x}=arg\underset{x}{\max }p(x|v,y)$$也就是说，我们希望在给定先验信息和所有时刻的运动 $v$、观测 $y$ 的情况下，推测出所有时刻的最优状态$\hat{x}$。为此定义几个宏观变量：$$x=x_{0:K}=(x_0,...,x_K),v=({\check{x}}_0,v_{1:K})=(\check{x},v_1,...,v_K)$$$$y=y_{0:K}=(y_0,...,y_K)$$  首先，用贝叶斯公式重写MAP估计：$$\hat{x}=arg\underset{x}{\max }\frac{p(y|x,v)p(x|v)}{p(y|v)}=arg\underset{x}{\max }p(y|x)p(x|v)$$由于分母与x无关，将其略去。同时省略p(y|x,v)中的v，因为如果x已知，v不会影响观测数据。
  接下来要做出一个重要的假设：对于所有时刻k=0，…，K，所有的噪声项$w_k$和$n_k$之间是无关的。这使得我们可以用贝叶斯公式对p(y|x)进行因子分解：$$p(y|x)=\prod _{k=0}^{K}p(y_k|x_k)$$另一方面，贝叶斯定理又允许我们将p(y|v)分解为：$$p(x|v)=p(x_0|{\check{x}}_0)\prod _{k=1}^{K}p(x_k|x_{k-1},v_k)$$在线性系统中，高斯密度函数可展开为：

  然后对等式两侧各取对数：

在这个过程中：

在式（3.8）中出现了一些与x无关的项，可以把它们忽略掉。因此，定义下面这些量：

这些都是平方马氏距离，所谓马氏距离表示点与一个分布之间的距离。它是一种有效的计算两个未知样本集的相似度的方法。与欧氏距离不同的是，它考虑到各种特性之间的联系（例如：一条关于身高的信息会带来一条关于体重的信息，因为两者是有关联的），并且是尺度无关的，即独立于测量尺度。对于一个均值为μ，协方差矩阵为Σ的多变量向量，其马氏距离为$\sqrt{(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$。然后我们可以定义整体的目标函数$J(x)$。通过最小化这个目标函数，我们来求解自变量x的值：$$J(x)=\sum _{k=0}^K(J_{v,k}(x)+J_{y,k}(x))$$从优化的角度来看，我们寻求下面这个优化问题的最优解：$$\hat{x}=arg\underset{x}{\min }J(x)$$为了寻找最优状态估计，我们可以根据已有的数据计算最大似然估计。这个问题是一个无约束的优化问题，对于状态变量x本身并没有任何约束。
  因为式（3.9）中所有的项都是x的二次形式，我们还可以进一步简化问题。把所有数据排成一列，称为提升形式。那么可以把所有时刻的状态组成一个向量x，并把所有时刻已知的数据组成一个向量$z$：$$z=\left[\begin{array}{c}{\check{x}}_0\\ v_1\\ ⋮\\ \underline{v_K}\\ y_0\\ y_1\\ ⋮\\ y_K\end{array}\right],x=\left[\begin{array}{c}{x}_0\\ ⋮\\ x_K\end{array}\right]$$然后，定义以下的块矩阵：
该式只显示了非零块，分割线用以区分矩阵的不同部分，例如先验信息、输入v、测量y以及提升形式的z。根据上面的定义，我们把目标函数写成：$$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)$刚好是一个抛物面，可以解析地找到它的最小值。只需让目标函数相对于自变量的偏导数为0即可：

式（3.16）的解 $\hat{x}$ 是经典的批量最小二乘法的解。同时也等价于传统估计理论中的固定区间平滑算法批量最小二乘法的求解利用了矩阵求伪逆的方法。从计算角度来说，我们并不会真的去计算$H^T{W}^{-1}H$的逆（即使它是个稠密矩阵）。由于$H^T{W}^{-1}H$会有一种特殊的对角块结构，因此可以利用稀疏的求解算法来更高效地求解。
  批量线性高斯系统的一种直观的解释，是把它看成一个弹簧-重物系统，如下图所示。最小二乘中的每一项代表了每个弹簧的能量，它和重物的位置有关。而最小二乘的最优解，就是整个系统处于最低能量的状态。

3.1.3 贝叶斯推断

  在线性高斯系统中，全贝叶斯后验概率$p(x|v,y)$的计算并不是简单地最大化它。需要从先验的概率密度函数出发，然后再用测量的数据来更新它。
  在这个情况下，可以用初始状态和输入来建立状态的先验估计：$p(x|v)$。用运动方法来建立先验：$$x_k=A_{k-1}{x}_{k-1}+v_k+w_k$$在提升形式中，可以写成：$$x=A(v+w)$$这里$w$也是初始状态和运动噪声的提升形式，同时，

是转移矩阵的提升形式，可见它是下三角的。于是，提升之后的均值为：$$\check{x}=E[x]=E[A(v+w)]=Av$$提升的协方差为：$$\check{P}=E[(x-E[x])(x-E[x]{)}^T]=AQ{A}^T$$其中$Q=E[w{w}^T]=diag(P_0,Q_1,...,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$$其中n是提升之后的测量噪声，且$$C=diag(C_0,C_1,...,C_K)$$是提升后的观测矩阵。于是，提升之后的状态、观测联合概率密度函数可写成：

这里$R=E[n{n}^T]=diag(R_0,R_1,...,R_K)$。这个式子因式分解：$$p(x,y|v)=p(x|v,y)p(y|v)$$我们只关心第一个因子，它表示了全贝叶斯后验概率。这一项根据上一节的式（2.53）$p(x|y)=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y)，{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$可以重写成：

利用SMW恒等式，可将上式转换成：

事实上，基于这个等式就能实现一个贝叶斯估计器，因为它代表了全贝叶斯后验概率，但这种做法并不高效。
  为显示此方法与之前讲的优化方法的联系，整理均值项，让他成为$\hat{x}$的线性函数：

可见，左侧出现了先验的协方差的逆。代入$\check{x}=Av$和${\check{P}}^{-1}=(AQ{A}^T{)}^{-1}=A^{-T}{Q}^{-1}{A}^{-1}$,可以将它重写成：

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

它仍是个下三角矩阵，但形式非常稀疏（仅有主对角线和下方的一块非零）。如果定义：

就可以把系统写成：$$(H^T{W}^{-1}H)x=H^T{W}^{-1}z$$这和之前讨论的最优化方法的解完全一致。
  在线性高斯系统中，贝叶斯推断给出了与优化方法（最大后验估计）一致的解，本质上是因为线性高斯系统的贝叶斯后验概率仍是高斯的，而高斯函数的均值和模是一样的。

3.1.4 存在性、唯一性与能观性

  从基本的线性代数可以看出，当且仅当$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$可逆？再从基本的线性代数来看，因为$dimx=N(K+1)$，所以可逆的充要条件是：$$rank(H^T{W}^{-1}H)=N(K+1)$$又因为$W^{-1}$是实对称且正定的，所以可以仅检测：$$rank(H^{T}H)=rank(H^T)=N(K+1)$$即要求$H^T$有N(K+1)个线性无关的行/列向量。
  接下来，分为情况两种进行讨论：
1.初始状态有先验信息：${\check{x}}_0$；
2.初始状态没有先验信心。

情况1：有先验

  将$H^T$展开，我们需要检测秩的矩阵为：

这是一个阶梯形矩阵。显然它每一行都是线性无关的，这意味着该矩阵是满秩的。所以，我们能够得到一个唯一解 $\hat{x}$ ，只须：$$\check{P_0}>0,Q_k>0$$这里>0意味着矩阵是正定的（因此也是可逆的）。直观上说，因为先验已经给出了系统的一个解，我们只是用观测来修正它。这只是充分但不是必要的条件。

情况2：没有先验知识

  $H^T$中每一列中的块都表示了一部分有关系统的信息，而第一列的块表达了我们对初始状态信息的了解程度。因此，去掉第一列之后，变为：

这个矩阵有K+1个行（每块大小为N）。将最上一行移到最小（这不会改变矩阵的秩），得：

那么，除去最后一行，这个矩阵仍是阶梯形矩阵。同理，在不影响秩得前提下，我们可以用$A_0^T$乘第一行，然后加到最后一行上；再用$A_0^T{A}_1^T$乘第二行，加到最后一行上…最后用$A_0^T{A}_1^T...A_{K-1}^T$乘倒数第二行，加到最后一行，得到：

检查最终的表达式，可见左下角块为0，同时，左上角块是阶梯形矩阵，所以是满秩的（每一行有一个领头的非零块）。于是，对$H^T$的秩判定，归结于右下角部分是否为秩N：

如果进一步假设系统是时不变的，那么对所有的k，有$A_k=A,C_k=C$，并且做一个不那么严格的假设：K >>N，之后还能进一步简化这个条件。
  为了做这件事，需要引入线性代数中的卡莱-哈密顿定理。因为A是N×N的，它的特征方程最多有N项，因此A的大于等于N次以上的高阶项必能写成$1,A,...,A^{N-1}$的一种线性组合。展开来说，对于$\forall k\ge N$，能够找到一组不全为零的系数$a_0,a_1,...,a_{N-1}$，使得
又因为矩阵的行秩和列秩是一样的，因而：

定义能观性矩阵$\mathcal{O}$ 为：

那么秩条件就简化为：$$rank\mathcal{O}=N$$这就是能观性矩阵的判别条件。在此指明了系统能观性和$H^T{W}^{-1}H$可逆性之间的联系。总而言之，使得解是存在且唯一的条件是：$$Q_k>0,R_k>0,rank\mathcal{O}=N$$ 这里大于0的意思是矩阵正定（因而可逆）。这些仍是充分条件而非必要条件。

解释

  仍然回到弹簧-质点模型，来理解能观性方面的问题。如下图展现了一些例子。当我们有初始状态时（顶上的图例），系统总是能观的，因为你没法在不改变（至少一个）弹簧长度的条件下，移动任意一个（或多个）重物。也就是说，这样一个系统有一个唯一的最小能量状态。对于中间的例子，结论也是一样，即使我们不知道初始状态。最下面的例子则是不能观的，因为我们可以把所有重物任意左移或右移，同时不改变弹簧储存的能量。这样一个系统的最小能量就是不唯一的。

3.1.5 MAP的协方差

  回到式$\hat{x}=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}z$，$\hat{x}$表示了状态真值x最可能的估计。那么另一个重要的问题是说，我们对$\hat{x}$的置信度如何？也就是说，我们可以用高斯形式对最小二乘解作另一种解释：$$\underset{协方差的逆}{\underbrace{(H^T{W}^{-1}H)}}\underset{均值}{\underbrace{\hat{x}}}=\underset{信息向量}{\underbrace{H^T{W}^{-1}z}}$$等式右侧称为信息向量。为了说明它的含义，用贝叶斯法则重写式子$$p(z|x)=\eta exp(-\frac{1}{2}(z-Hx{)}^T{W}^{-1}(z-Hx))$$为：$$p(x|z)=\beta exp(-\frac{1}{2}(Hx-z{)}^T{W}^{-1}(Hx-z))$$这里 $\beta$ 是归一化因子。然后把式子$$\hat{x}=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}z$$代入并作一些简化，易得：$$p(x|\hat{x})=\kappa exp(-\frac{1}{2}(x-\hat{x}{)}^T(H^T{W}^{-1}H)(x-\hat{x}))$$这里$\kappa$也是一个归一化因子。从该式可以看出，$N(\hat{x},\hat{P})$是最小二乘算法对x的高斯估计，其中均值即最优解，而协方差$\hat{P}=(H^T{W}^{-1}H{)}^{-1}$。
  另一种处理方式是直接对最优解取期望。注意：$$x-\underset{E[x]}{\underbrace{(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}z}}=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}\underset{s}{\underbrace{(Hx-z)}}z$$其中$$s=\left[\begin{array}{c}w\\ n\end{array}\right]$$那么就有：$$\hat{p}=E[(x-E[x])(x-E[x]{)}^T]$$$$=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}\underset{W}{\underbrace{E[s{s}^T]}}{W}^{-1}H(H^T{W}^{-1}H{)}^{-1}$$$$=(H^T{W}^{-1}H{)}^{-1}$$仍能得到上述的结论。

小结

  总的来说，离散时间批量估计的方法主要有最大后验估计和贝叶斯推断两种方法。由于线性高斯系统中贝叶斯后验概率仍是高斯的，所以高斯函数的均值和模都是一样，因此，该两种方法最终得到的解是一致的，都为$\hat{x}=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}z$。

3.2 离散时间的递归平滑算法

  本小节主要是介绍了两种递归平滑算法，分别是Cholesky平滑算法和Rauch-Tung-Striebel平滑算法。该两种方法都利用了上一节批量优化结论中国的稀疏结构$H^T{W}^{-1}H$（三对角块）来加速方程的求解。采用前向-后向的递归操作来完成。
  在Cholesky平滑算法中，利用上一节的核心等式$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z$，然后主要分为三个步骤：

1. 令$H^T{W}^{-1}H=L{L}^T$，先求解出L；
2. 令$d=L^T\hat{x},求Ld=H^T{W}^{-1}$，求解出d；
3. 最后根据$d=L^T\hat{x}$，求解 $\hat{x}$。

  以上前两个步骤为前向迭代过程，第三步为后向迭代过程。最终得到如下的递归方程：
  以上的六个递归方程等价于传统的Rauch-Tung-Striebel(RST)平滑算法；而5个前向迭代方程则等价于著名的卡尔曼滤波器。
  实际上，RTS平滑算法是比Cholesky平滑算法更为标准的方程形式，通过代数运算，得到更经典的表达形式。利用SMW等式来进行代数操作，并且在化简得过程中，令${\hat{P}}_{k,f}=I_k^{-1},{\hat{P}}_{k,f}^{-1}{\hat{x}}_{k,f}=q_k$，并定义卡尔曼增益$K_k={\hat{P}}_{k,f}{C}_k^T{R}_k^{-1}$，最终根据cholesky平滑算法的6个递归方程，通过一系列得代数操作，得到以下的RTS平滑算法：

  其中前五个前向迭代过程被称为经典的卡尔曼滤波器，更重要的是，以上6个公式表达的RTS平滑算法，能够高效且无近似地解决之前提到的批量优化问题。这件事情成立的原因正是由于优化算法左侧矩阵的特殊三对角块的稀疏结构，和批量解法本质上是一样的。
  总的来说，该两种平滑算法都利用了所有能用到的数据来估计所有时刻的状态。

3.3 离散时间的递归滤波算法

  为了解决上文介绍的批量优化和平滑算法无法在线运行的问题，因为它需要用到未来时刻的信息估计过去的状态，因此为了在实时场合使用，当前的状态只能由之前时间的信息决定，而卡尔曼滤波就是对这样的问题的传统解决方案。在上一节的RTS平滑算法的前向迭代过程中已经引出了卡尔曼滤波。本节将介绍其他几种推导卡尔曼滤波的方法。

  首先对上一节批量优化得到的结果分解成前向递归估计和后向递归估计，为了实现这一结果需要将$z,H,W$按不同时刻来重新排列，最终得到等式：

其中：


通过化简${\hat{P}}_k^{-1}$和$q_k$分为了前向过程和后向过程，前向过程仅用k时刻以前的H和W块，后向过程仅用到k+1至K时刻的H与W块。
  于是，分别得到“前向”和“后向”的估计器，分别计算${\hat{x}}_{k,f}$和${\hat{x}}_{k,b}$:

进一步的化简得到：

  最终通过高斯密度函数的归一化积，得到：

  然后是采用MAP方法、贝叶斯推断方法分别将前向的高斯估计${\hat{x}}_{k,f}$推导出递归滤波器器，也就是卡尔曼滤波器。
  在MAP方法中采用了消元法将不关心的变量边缘化掉，并通过SMW恒等式变换和一系列的化简最终求解出递归滤波如下：

  在贝叶斯推断方法中，则以更加简洁的方式推出卡尔曼滤波。通过联合高斯推断推导出均值和协方差，最终求得：

  这与MAP给出的更新步骤的方程式完全一致的。根本原因在于采用的是线性模型，而噪声和先验也都是高斯的。在这些条件下，后验概率亦是高斯的，于是它的均值和模也是一样的。但在非线性模型中就不能保证这个性质了。
  然后是从增益最优化的角度来看卡尔曼滤波，选取合适的$K_k$值，正确地衡量修正部分的权重。

并定义状态的误差为${\hat{e}}_k={\hat{x}}_k-x_k$，以及代价函数$J(K_k)=\frac{1}{2}trE[{\hat{e}}_k{\hat{e}}_k^T]$。
最终求解$K_k$为：$$K_k={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+R_k{)}^{-1}$$关于卡尔曼滤波的要点如下：
1.对于高斯噪声的线性系统，卡尔曼滤波器是最优线性无偏估计。这意味着它给出的解的协方差矩阵正位于克拉美罗下界处；
2.必须有初始状态：{ ${\check{x}}_0,{\check{P}}_0$};
3.协方差部分与均值部分可以独立地递推。有时可以计算一个固定的$K_k$，用于所有时刻的均值修正。这种做法称为固定状态的卡尔曼滤波；
4.实现当中我们必须用实际的$y_{k,meas}$，它们来自传感器的实际读数；
5.从后向过程一样能推导出类似的滤波方法，这种方法沿着时间反向递归。