【状态估计】偏差，匹配和外点

我们都知道：对于状态的估计可能是有偏差的，特别是在运动模型或观测模型是非线性的情况下。在简单的立体相机的例子中，我们看到MAP方法相比于全贝叶斯方法来说是有偏差的。同时，我们也看到批量ML方法对于真实值来说也是有偏差的，还推导了用来量化该偏差的表达式。

不幸的是，偏差的来源不仅仅是这些。

在众多状态估计的方法中，我们都假设输入或者测量是满足零地均值的高斯分布的。实际上，我们的输入和测量也可能受到其它未知偏差的影响。如果忽视它们，我们的估计也会出现偏差。典型的例子是加速度计，它的偏差与温度有关，并且会随着时间的推移而而变化。

状态估计问题中另一个十分重要的部分就是确定测量与模型之间的对应关系问题。例如，当我们测量距离一个路标的距离时，需要假设正在观测的是哪一个路标，这是非常重要的。天体跟踪器是一个很好的例子。当我们看到光点的时候，如何确定这个光点对应星图中的哪一个星星呢？测量与它对应的模型或映射的配对过程被称为建立匹配（determining corresspondences）或数据关联（data association）。

最后，尽管我们尽力消除偏差的影响，并找到正确的匹配关系，但还存在着一些问题。比如测量的外点，会对我们的测量产生不好影响，使算法一直在调整某个在预设的噪声模型看来完全不可能产生的数据上。如果我们没有正确地检测和别除外点，很多状态估计的方法都会面临灾难性的失败。

这本文中，我们将研究怎么处理不太完美的输入或测量。我们将介绍一些用于处理偏差，确定匹配关系，检测和剔除外点的经典方法。

处理输入和测量的偏差

在本节中，我们将研究偏差对于输入和测量的影响。我们会看到输入和测量的偏差都是可以处理的，而且输入偏差会比测量偏差更易于处理。在接下来的讨论中，我们将利用具有非零均值高斯噪声的、线性时不变的运动模型和观测模型来进行研究，但是其中的许多概念也可以拓展到非线性系统中。

偏差对于卡尔曼滤波器的影响

为了展示输入、测量的偏差，讨论下非零均值高斯噪声的情况下，对卡尔曼滤波器的影响。假设：

$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B(u_k+\bar{u})+w_k\\ y_k& =C{x}_k+\bar{y}+n_k\end{array}$$

其中，$\bar{u}$为输入的偏差，$\bar{y}$为测量的偏差。在这里，仍然假设运动和观测模型都受到零均值高斯噪声影响：

$$\begin{array}{r}{w}_k\sim N(0,Q)\\ n_k\sim N(0,R)\end{array}$$

并且噪声之间是相互独立的，即对于所有的$k\ne l$：

$$E[w_k{w}_l]=E[n_k{n}_l]=E[w_k{n}_l]=E[n_k{w}_l]=0$$

在卡尔曼滤波器中可以获得状态估计：

$$\begin{array}{rl}{\check{x}}_k& =A{\hat{x}}_{k-1}+B{u}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-C{\check{x}}_k)\end{array}$$

定义估计误差：

$$\begin{array}{r}{\check{e}}_k={\check{x}}_k-x_k\\ {\hat{e}}_k={\hat{x}}_k-x_k\end{array}$$

在这种情况下，构造的误差动态过程方程如下：

$$\begin{array}{rl}{\check{e}}_k& =A{\hat{e}}_{k-1}-(B\bar{u}+w_k)\\ {\hat{e}}_k& =(1-K_{k}C){\check{e}}_k+K_k(\bar{y}+n_k)\end{array}$$

其中，${\hat{e}}_0={\hat{x}}_0-x_0$。

为了让估计是无偏和一致的，需要保证对于所有的$k=1,2,...,K$，有：

$$\begin{array}{r}E[{\hat{e}}_k]=E[{\check{e}}_k]=0\\ E[{\hat{e}}_k{\hat{e}}_k^T]={\hat{P}}_k\\ E[{\check{e}}_k{\check{e}}_k^T]={\check{P}}_k\end{array}$$

在$\bar{u}=\bar{y}=0$的情况下，这些条件是成立的。

那么在不满足零偏置条件的情况下，会发生什么变化？假设：

$$\begin{array}{rl}E[{\hat{e}}_0]& =0\\ E[{\hat{e}}_0{\hat{e}}_0^T]& ={\hat{P}}_0\end{array}$$

虽然这个初始条件是另一个可能引入偏差的地方。在$k=1$时，有：

$$\begin{array}{rl}E[{\check{e}}_1]& =AE[{\hat{e}}_0]-(B\bar{u}+E[w_1])\\ & =-B\bar{u}\\ E[{\hat{e}}_1]& =(1-K_{1}C)E[{\check{e}}_1]+K_1(\bar{y}+E[n_1])\\ & =-(1-K_{1}C)B\bar{u}+K_1\bar{y}\end{array}$$

在$\bar{u}\ne 0$或$\bar{y}\ne 0$时，$k=1$就已经为有偏估计了。

预测误差的协方差为：

$$\begin{array}{rl}E[{\check{e}}_1{\check{e}}_1^T]& =E[(A{\hat{e}}_0-(B\bar{u}+w_1))(A{\hat{e}}_0-(B\bar{u}+w_1){)}^T]\\ & =E[(A{\hat{e}}_0-w_1)(A{\hat{e}}_0-w_1{)}^T]+(-B\bar{u})(-B\bar{u}{)}^T\\ & ={\check{P}}_1+E[{\check{e}}_1]E[{\check{e}}_1{]}^T\end{array}$$

因此，卡尔曼滤波器对真实的误差不确定性是欠估计（低估）的，从而导致不一致性。

同理，最终估计误差的协方差为：

$$\begin{array}{rl}E[{\hat{e}}_1{\hat{e}}_1^T]& =E[((1-K_{1}C){\check{e}}_1+K_1(\bar{y}+N_1))((1-K_{1}C){\check{e}}_1+K_1(\bar{y}+N_1){)}^T]\\ & =E[((1-K_{1}C){\check{e}}_1+K_1{n}_1)((1-K_{1}C){\check{e}}_1+K_1{n}_1{)}^T]+(-(1-K_{1}C)B\bar{u}+K_1\bar{y})(-(1-K_{1}C)B\bar{u}+K_1\bar{y})\\ & ={\hat{P}}_1+E[{\hat{e}}_1]E[{\hat{e}}_1{]}^T\end{array}$$

可以发现，卡尔曼滤波器对协方差的估计仍然是过度确信（高估）的，因此也是不一致的。有趣的是，不管偏差的正负性如何，卡尔曼滤波器都是过度确信的。并且，很容易看出，随着$k$的增大，偏差带来的影响将会无限制地增长。

对卡尔曼滤波做如下的修正：

$$\begin{array}{rl}{\check{P}}_k& =A{\hat{P}}_{k-1}{A}^T+Q\\ {\check{x}}_k& =A{\hat{x}}_{k-1}+B{u}_k+B\bar{u}\\ K_k& ={\check{P}}_k{C}^T(C{\check{P}}_k{C}^T+R{)}^{-1}\\ {\hat{P}}_k& =(1-K_{k}C){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-C{\check{x}}_k-\bar{y})\end{array}$$

通过上面的修正，重新恢复了一个无偏的和一致的估计。但这里的问题是，为了有效抑制偏差带来的影响，必须知道偏差的准确值。但大多数情况下，并没有办法知道其精确值（偏差有时甚至会随着时间而变化）。因此，在估计问题中，应该尝试把偏差也估计出来。

未知的输入偏差

假设测量偏差$\bar{y}=0$，但输入偏差$\bar{u}\ne 0$，因此除了估计系统状态$x_k$，还需要估计输入偏差。即需要估计的增广状态为：

$$x_k^{{}^{\prime }}=\left[\begin{array}{c}{x}_k\\ {\bar{u}}_k\end{array}\right]$$

其中，为了使偏差作为状态的一部分，令偏差为时间的函数，所以需要为偏差定义一个运动模型。一个典型的模型为：

$${\bar{u}}_k={\bar{u}}_{k-1}+s_k$$

其中，$s_k\sim N(0,W)$，这是有偏差的布朗运动模型。由于影响运动的内部偏差是服从零均值高斯分布的，在某些意义上，可以简单地利用一个积分器来处理。此时，增广运动模型为：

$$\begin{array}{rl}{x}_k^{{}^{\prime }}& =\left[\begin{array}{cc}A& B\\ 0& 1\end{array}\right]x_{k-1}^{{}^{\prime }}+\left[\begin{array}{c}B\\ 0\end{array}\right]u_k+\left[\begin{array}{c}{w}_k\\ s_k\end{array}\right]\\ & =A^{{}^{\prime }}{x}_{k-1}^{{}^{\prime }}+B^{{}^{\prime }}{u}_k+w_k^{{}^{\prime }}\end{array}$$

注意到：

$$\begin{array}{rl}{w}_k^{{}^{\prime }}& \sim N(0,Q^{{}^{\prime }})\\ Q^{{}^{\prime }}& \sim \left[\begin{array}{cc}Q& 0\\ 0& W\end{array}\right]\end{array}$$

于是又变成了无偏估计系统的形式。在这种情况下，观测模型为：

$$\begin{array}{rl}{y}_k& =\left[\begin{array}{cc}C& 0\end{array}\right]x_k^{{}^{\prime }}+n_k\\ & =C^{{}^{\prime }}{x}_k^{{}^{\prime }}+n_k\end{array}$$

一个至关重要的问题是，该增广状态构建的滤波器会不会收敛到正确的结果。上面的技巧是否真实有效？对于线性高斯批量估计（没有初始状态的先验信息），解存在且唯一的条件为：

$$\begin{array}{rl}Q& >0\\ R& >0\\ rank(O)& =N\end{array}$$

当系统偏差为0时，即$\bar{u}=0$，我们假设上述条件成立。定义：

$$O^{{}^{\prime }}=\left[\begin{array}{c}{C}^{{}^{\prime }}\\ C^{{}^{\prime }}{A}^{{}^{\prime }}\\ ⋮\\ C^{{}^{\prime }}(A^{{}^{\prime }}{)}^{N+U-1}\end{array}\right]$$

其中，$dim({\bar{u}}_k)=U$。

为了证明增广状态构建的批量估计问题中解的存在性和唯一性，需要证明：

$$rank(Q^{{}^{\prime }})=N+U$$

一般情况下，这个条件不一定会成立。

未知的测量偏差

假设输入偏差$\bar{u}=0$，但测量偏差$\bar{y}\ne 0$。因此需要估计的增广状态为：

$$x_k^{{}^{\prime }}=\left[\begin{array}{c}{x}_k\\ {\bar{y}}_k\end{array}\right]$$

其中，偏差仍然是时间的函数，假设一个随机游走的噪声模型：

$${\bar{y}}_k={\bar{y}}_{k-1}+s_k$$

其中，$s_k\in N(0,W)$。在这种偏差运动模型下，增广状态系统的运动模型为：

$$\begin{array}{rl}{x}_k^{{}^{\prime }}& =\left[\begin{array}{cc}A& 0\\ 0& 1\end{array}\right]x_{k-1}^{{}^{\prime }}+\left[\begin{array}{c}B\\ 0\end{array}\right]u_k+\left[\begin{array}{c}{w}_k\\ s_k\end{array}\right]\\ & =A^{{}^{\prime }}{x}_{k-1}^{{}^{\prime }}+B^{{}^{\prime }}{u}_k+w^{{}^{\prime }}\end{array}$$

注意到：

$$\begin{array}{rl}{w}_k^{{}^{\prime }}& \sim N(0,Q^{{}^{\prime }})\\ Q^{{}^{\prime }}& \sim \left[\begin{array}{cc}Q& 0\\ 0& W\end{array}\right]\end{array}$$

观测模型为：

$$\begin{array}{rl}{y}_k& =\left[\begin{array}{cc}C& 1\end{array}\right]x_k^{{}^{\prime }}+n_k\\ & =C^{{}^{\prime }}{x}_k^{{}^{\prime }}+n_k\end{array}$$

同样，此时的估计问题还不一定是能观的。

小结

无论是运动还是测量方程，都可以把偏差引入到状态中，形成增广的状态估计问题。大部分情况下，引入偏差后的问题是不完全能观的。

对于不完全能观的情况下，对应的批量估计：

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

左侧的矩阵存在零空间，使得它无法直接求逆（但可以求广义逆）。此时，线性方程组有无穷多组解。

可以引入偏差量的初始估计，并在求解过程中保持零空间不变，来计算这种情况下的状态估计。

数据关联

数据关联问题就是找出每一个测量信息对应于模型中的哪一部分。事实上，几乎所有的估计技术，特别是机器人技术，都采用某种形式的模型或地图来确定机器人的状态，如机器人的位置/方向。

一些常见例子是：

1. GPS卫星定位。在固联于地球的参考坐标系中，假设GPS卫星的位置（随时间的函数）是已知的（通过其轨道参数计算），地面上的GPS接收器可以测量自己距离多颗卫星的距离（利用飞行时间原理以及卫星传递的时间信息）。在这个例子里，很容易知道GPS接收器获得的距离信息是相对于哪颗卫星的，因为每颗卫星发送的时间戳中带有唯一的编码信息。
2. 使用天体观测确定姿态。星敏感器使用天空中所有最亮星星绘制而成的星图（或星表）来确定传感器指向的方位。因此可以将自然世界看作为地图（提前绘制的），而这个例子中数据关联问题，就是确定现在看到的星星是哪一颗，要比GPS例子要难得多。只有提前生成星表，这个系统才能实用。

数据关联技术本质上分为两类：外部关联和内部关联。

外部数据关联

在外部数据关联中，需要用到基于模型和测量的专业知识。这些专业知识对于估计问题来说是外部的。从估计问题的角度来看，数据关联的任务已经完成，因此外部数据关联有时被称为已知的数据关联。

例如，一些观测物体被涂上了不同的颜色，而通过立体相机观测到的这些物体，可以利用颜色信息来进行数据关联。颜色信息在估计问题中也就不会再用到。诸如视觉条形码，特定频率的信号或者编码信息（如GPS卫星）也是外部数据关联。

如果我们能够通过某种手段，搭建或修改成合作的系统，让外部数据关联就可以很好地工作，就可以大幅简化的状态估计问题。与此同时，前沿的计算机视觉技术也可以使用未经过预处理的模型进行外部数据关联，虽然可能很容易出现错误的数据关联。

内部数据关联

在内部数据关联中，只能用测量和模型的数据来计算数据关联。因此内部数据关联有时被称为未知的数据关联。通常，对于给定的模型，内部数据关联与给定观测的似然有关。在最简单的版本中，只有最有可能的数据关联被接受，而其它可能性的关联将会被忽略。更复杂的算法中，也允许存在多种数据关联的假设，用于估计问题中。

对于一些特定的模型，比如三维空间中的路标或者星表、路标构成的星座，有时可以用于数据关联。

如下图所示。数据对齐的刚体约束穷举搜索（data-aligned rigidity-constrained exhaustive search）算法是一个典型的基于群集的数据关联算法。其主要思想为，同同一群集中点对间的距离可以作为数据关联中独一无二的标识。

不管采用什么类型的数据关联，只要问题估计失败了，那么极有可能是错误的数据关联导致的。因此，我们应该认识到，在实际应用中数据的误关联是很有可能发生的，所以在技术设计中就应该考虑到如何处理误关联，从而提高系统的鲁棒性。下面，我们将在外点的检测和剔除部分，讨论一些处理此类问题的方法。

处理外点

数据误关联有时会使得估计器完全发散。然而，驱使估计器发散的原因不仅仅是数据关联。比如，我们的测量会受到许多因素的干扰而变得不可靠。一个典型型的例子是，GPS信号在建筑物附近存在多路径效应。通过反射信号，我们可以得到测量距离。然而当视距路径被阻挡，并且没有任何附加信息时，接收机是无法知道较长的路径测得的距离是不正确的。

我们通常将非常不可能的测量值（根据观测模型）称为外点。这里，可能性的评判基准有待选择，但一种常见的方法（在一维度数据中）是将超出平均值三个标准差的测量值作为外点。

如果我们认为系统中存在一部分（也可能是很大一部分）外点，那么我们将需要设计一种检测，减少或移除外点对估计问题影响的方案。我们将讨会论处理外点的的两种最常用的方案：

1. 随机采样一致性（Random sample consensus）
2. M-估计（M-Estimation）

两种方案可以单独使用或串联使用。

随机采样一致性

随机采样一致性（RANSAC）是一种根据含有外点的数据拟合参数模型的选代方法。外点是不符合模型的测量，内点是符合模型的测量。RANSAC是一种概率算法，只有花费足够时间去搜索，才能确保（提高）找到合理答案的概率。

RANSAC以迭代的方式进行。在基础版本中，每个选代包括以下五个步骤：

1. 在原始数据中随机选取一个子集作为假设的内点；

2. 根据假设的内点拟合一个模型；

3. 判断剩余的原始数据是否符合拟合的模型，并将其分为内点和外点。如果内点太少，则该次迭代被标记为无效并中止；

4. 根据假设的内点和上一步中划分出的内点重新拟合模型；

5. 计算所有内点的残差，根据残差和评估模型。迭代上述步骤，把具有最小残差和的模型作为最佳模型。

这里一个重要的问题是，需要多少次选代，比方说$k$次，才可以确保有概率$p$选到仅由内点组成的子集？一般而言，该问题很难求解。然而，如果我们假设每个测量被选取是相互独立的，并且每个测量为内点的概率均为$w$，那么有以下关系：

$$1-p=(1-w^n{)}^k$$

其中，$n$为拟合模型所需的最少数据个数。迭代次数$k$可解得：

$$k=\frac{ln(1-p)}{ln(1-w^n)}$$

实际上，这也可以认为是$k$的上限。

M-估计

早期的很多估计技术都是以最小化误差平方和为代价函数。该代价函数的问题是，它非常容易受到外点的干扰。一个偏离较大的外点可以对估计结果产生巨大的影响，因为它主导了代价函数。

M-估计修改代价函数的形状，这样在求解过程中外点就不会占主导地位。

总体非线性MAP目标函数（对于批量估计）可以写为二次型的形式：

$$J(x)=\frac{1}{2}\sum _{i=1}^N{e}_i(x{)}^T{W}_i^{-1}{e}_i(x)$$

目标函数的梯度为：

$$\frac{\partial J(x)}{\partial x}=\sum _{i=1}^N{e}_i(x{)}^T{W}_i^{-1}\frac{\partial e_i(x)}{\partial x}$$

在目标函数取极小值时梯度为0。

现一般化这个目标函数，并将其写成：

$$J_{general}(x)=\sum _{i=1}^N{\alpha }_i\rho (u_i(x))$$

其中，${\alpha }_i>0$是一个标量权重值，而

$$u_i(x)=\sqrt{e_i(x{)}^T{W}_i^{-1}{e}_i(x)}$$

同时，$\rho ()$为非线性代价函数。满足：有界，在$u=0$处有唯一的零点，在$u>0$时单调递增。

有很多这样的函数，如：

Quadratic函数：

$$\rho (u)=\frac{1}{2}{u}^2$$

Cauchy函数：

$$\rho (u)=\frac{1}{2}ln(1+u^2)$$

Geman-MeClure函数：

$$\rho (u)=\frac{u^2}{2(1+u^2)}$$

这些比二次函数增长较慢的函数称为鲁棒代价函数。由于梯度的减小，这意味着大的误差对应的权重不会太大，对结果的影响也就减弱了。下图展示了，不同的鲁棒函数曲线。

利用链式法则，新的代价函数的梯度为：

$$\frac{\partial J_{general}(x)}{\partial x}=\sum _{i=1}^N{\alpha }_i\frac{\partial \rho }{\partial u_i}\frac{\partial u_i}{\partial e_i}\frac{\partial e_i}{\partial x}$$

如果寻找最小值，则应该使梯度为零。代入：

$$\frac{\partial u_i}{\partial e_i}=\frac{1}{u_i(x)}{e}_i(x{)}^T{W}_i^{-1}$$

则梯度可以写成：

$$\frac{\partial J_{general}(x)}{\partial x}=\sum _{i=1}^N{e}_i(x{)}^T{Y}_i(x)\frac{\partial e_i(x)}{\partial x}$$

其中，

$$Y_i(x)=\frac{{\alpha }_i}{u_i(x)}\frac{\partial \rho }{\partial u_i}{|}_{u_i(x)}{W}_i^{-1}$$

$Y_i(x)$是一个依赖于$x$的新的协方差矩阵。

对比：

$$\frac{\partial J(x)}{\partial x}=\sum _{i=1}^N{e}_i(x{)}^T{W}_i^{-1}\frac{\partial e_i(x)}{\partial x}$$

可以看出，鲁棒函数某种意义上可以理解为对协方差进行修改，由固定的$W$修改成可变的$Y$。

在最小二乘的迭代过程中，可以使用当前的工作点计算$Y$：

$$Y_i(x_{op}{)}^{-1}=\frac{\alpha }{u_i(x_{op})}\frac{\partial \rho }{\partial u_i}|u_i(x_{op})W_i^{-1}$$

这种做法也称为迭代重加权最小二乘（Iteratively Reweighted Least Squares, IRLS）。此类方法实际上把目标函数变为了：

$$J(x{)}^{{}^{\prime \prime }}=\frac{1}{2}\sum _{i=1}^N{e}_i(x{)}^T{Y}_i^{-1}(x_{op})e_i(x)$$

总结

1. 总是有非理想的因素（例如偏差和外点），使得实际的估计问题与本书中纯数学的推导不同。这些非理想因素是实际应用中误差的主要来源。
2. 在某些情况下，我们可以将估计的偏差叠加到估计框架中，而在有些情况不能这么做。这是由问题的能观性来决定的。
3. 在大多数实际的估计问题中，外点是真实存在的，因此有必要使用某种形式的预处理（如RANSAC）以及鲁棒代价函数。