7. 从矢量观测到姿态观测

在使用 IMU 时，除了通过陀螺仪积分得到姿态，我们还需要其它非积分的方法测量姿态，从而更新积分得到的预测值。而这些“其它”的方法一般都是测量一些在惯性坐标系内已知的矢量，这些矢量在 IMU 坐标系中的测量值包含了 IMU 自身的姿态信息。最常用的例子包括：用IMU 中自带的加速度计测量重力方向，用磁强计测量磁场方向等等。在上一篇介绍 MEKF 的文章中，我们假设可以直接测量 IMU 的姿态；因此在这篇文章中，我们讨论如何将这些矢量测量转化为姿态测量，从而可以将它们用在 MEKF 中。

将方向测量转化为姿态测量，这一问题在姿态估计的文献中被称为 Wahba 问题（Wahba's Problem）[1]。它的表述如下：如果在惯性坐标系内有 n 个单位矢量，分别记为 \bm{v}_i ， i=1,\ldots,n ；在 IMU 坐标系下测量这些单位矢量得到的值为 \hat{\bm{v}}_i ， i=1,\ldots,n ，那么如何找到一个旋转矩阵 \mathrm{R} ，使得下面的误差函数最小

L(\mathbf{R})=\frac{1}{2}\sum_{i=1}^nw_i||\bm{v}_i-\mathrm{R}\hat{\bm{v}}_i||^2， （1）

其中 w_i 是一组权重。首先要说明的是，旋转矩阵具有三个自由度，每个矢量测量具有两个自由度。因此，当只有一个矢量测量时，这是一个欠约束问题，也就是有无数个旋转矩阵 \mathbf{R} 可以使 L(\mathbf{R})=0 ；当有两个或以上的矢量测量时，这是一个过约束问题，除非这些矢量测量完全没有误差，否则 L(\mathbf{R})>0 。其次，在 Wahba 问题中，这些矢量 \bm{v}_i 已经被归一化，也就是我们只考虑 \bm{v}_i 的方向，不考虑它的长度。下面我们对公式（1）做一些简化：

\begin{align} L(\mathbf{R})&=\frac{1}{2}\sum_{i=1}^nw_i(\bm{v}_i-\mathbf{R}\hat{\bm{v}}_i)^T(\bm{v}_i-\mathbf{R}\hat{\bm{v}}_i) \\ &=\frac{1}{2}\sum_{i=1}^nw_i(2-2\bm{v}_i^T\mathbf{R}\hat{\bm{v}}_i) \\ &=\sum_{i=1}^nw_i-\sum_{i=1}^nw_i\bm{v}_i^T\mathbf{R}\hat{\bm{v}}_i \end{align}

在数理统计中有一个很著名的技巧叫“trace trick”，也就是把一个二次型转化为矩阵的迹，公式是 \bm{x}^T\mathbf{A}\bm{y}=\mathrm{tr}(\bm{xy}^T\mathbf{A}^T) ，用在上面的公式中可以得到：

L(\mathbf{R})=w-\mathrm{tr}(\mathbf{F}\mathbf{R}^T)， （2）

其中 w=\sum_{i=1}^nw_i ， \mathbf{F}=\sum_{i=1}^nw_i\bm{v}_i\hat{\bm{v}}_i^T 。这样，我们把求 \mathbf{R} 使得 L(\mathbf{R}) 最小的问题转化为了使 \mathrm{tr}(\mathbf{F}\mathbf{R}^T) 最大的问题。下面我们介绍如何解决这一问题。

QUEST（QUaternion ESTimator） 算法是 Shuster 在1979年在 [2] 中提出的，这篇论文大概是整个姿态估计领域最著名的论文。具体做法是把（2）式写成单位四元数的形式：

\mathrm{tr(\mathbf{FR}^T)}=\bm{q}^T\begin{bmatrix} \mathrm{tr}(\mathbf{F}) & \left(\sum_{i=1}^nw_i\hat{\bm{v}}_i\times\bm{v}_i\right)^T \\ \sum_{i=1}^nw_i\hat{\bm{v}}_i\times\bm{v}_i & \mathbf{F}+\mathbf{F}^T-\mathrm{tr}(\mathbf{F})\mathbf{I}_{3\times 3} \end{bmatrix}\bm{q}\triangleq\bm{q}^T\mathbf{K}\bm{q}, （3）

然后求 \bm{q} 使得 \bm{q}^T\mathbf{K}\bm{q} 最大。注意因为 [2] 中的四元数的顺序与我在第一篇文章中定义的不一样，所以这个公式也有区别。下面直接给出结果： \bm{q} 的最优值是矩阵 \mathbf{K} 的最大的特征值所对应的特征向量，也就是 \mathbf{K}\bm{q}_{\mathrm{opt}}=\lambda_{\mathrm{max}}\bm{q}_{\mathrm{opt}} ，其中 \lambda_{\mathrm{max}} 是 \mathbf{K} 的最大特征值。在 [2] 中，作者给出了如何在不直接求特征向量的前提下估计 \bm{q}_{\mathrm{opt}} ，从而减少计算量，但在现在这样做已经没有太大意义了。

在 MEKF 中，我们还需要使用测量的姿态的不确定性，也就是如果 \mathbf{R}_{\mathrm{opt}}=\mathbf{R}_t\exp(\delta\bm{\theta}) ，我们需要知道 \mathrm{cov}(\delta{\bm{\theta}},\delta{\bm{\theta}}) ，其中 \mathbf{R}_t 是真实的姿态。现在作一些假设：测量得到的矢量 \hat{\bm{v}}_i 的噪声的方差是 \sigma_i^2 ，且是各向同性的； \bm{v}_i 是固定值，没有不确定性。再将权重 w_i 设置为和噪声的方差 \sigma_i^2 成反比，也就是 w_i=\sigma_{\mathrm{tot}}^2/\sigma_i^2 ，其中 (\sigma_\mathrm{tot})^{-1}=\sum_{i=1}^n(\sigma_i^2)^{-1} 。下面直接给出结果：

\mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta})=\sigma_{\mathrm{tot}}^2\left(\mathbf{I}_{3\times3}-\sum_{i=1}^nw_i\hat{\bm{v}}\hat{\bm{v}}^T\right)^{-1}. （4）

知道了 \bm{q}_\mathrm{opt} （或 \mathbf{R}_\mathrm{opt} ）和 \mathrm{cov}(\delta{\bm{\theta}},\delta{\bm{\theta}}) 后，我们就可以将矢量测量用在 MEKF 中了。

第二种算法是 Markley 在1987年在 [3] 中提出的，这种方法与 QUEST 差不多是等价的，思路是直接求 \mathbf{R} 使得 \mathrm{tr}(\mathbf{F}\mathbf{R}^T) 最大。在这里直接给出结果，具体的推导过程请参考 [3]。

假设 \mathbf{U'S'V'}^T=\mathbf{F} 是 \mathbf{F} 的奇异值分解（singular value decomposition，SVD），其中按照惯例， \mathbf{U'} 和 \mathbf{V}' 是正交矩阵（行列式可能等于 -1）， \mathbf{S}'=\mathrm{diag}(s'_1,s'_2,s'_3) 是对角矩阵，且满足 s'_1\geq s'_2\geq s'_3\geq 0 。下面我们做一些调整使 \mathbf{U'} 和 \mathbf{V'} 变成旋转矩阵，也就是行列式为 1 的正交矩阵：

\begin{align} \mathbf{U}&=\mathbf{U'}\mathrm{diag}(1,1,\mathrm{det}(\mathbf{U})) \\ \mathbf{V}&=\mathbf{V'}\mathrm{diag}(1,1,\mathrm{det}(\mathbf{V})) \\ \mathbf{S}&=\mathbf{S'}\mathrm{diag}(1,1,\mathrm{det}(\mathbf{UV})) \end{align}

可以验证 \mathbf{F}=\mathbf{USV}^T ，而且 \mathbf{U},\mathbf{V} 是旋转矩阵， \mathbf{S}=\mathrm{diag}(s_1,s_2,s_3) 且满足 s_1\geq s_2\geq |s_3|\geq 0 。也就是我们现在允许 s_3 取负值，但保证了 \mathbf{U},\mathbf{V} 的行列式是 1。

现在我们给出 \mathbf{R} 的最优值和对应的协方差矩阵：

\mathbf{R}_\mathrm{opt}=\mathbf{UV}^T （5）

\mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta})=\sigma_{\mathrm{tot}}^2\mathbf{U}(\mathbf{I}_{3\times 3}-\mathbf{S})\mathbf{D}^{-2}\mathbf{U}^T （6）

其中 \mathbf{D}=\mathrm{diag}(s_2+s_3,s_1+s_3,s_1+s_2) 。在这里用 SVD 算出的 \mathbf{R}_\mathrm{opt} 和用 QUEST 方法算出的 \bm{q}_\mathrm{opt} 是相同的；在测量误差不太大，也就是 L(\mathbf{R}_\mathrm{opt})\approx0 时，用（4）式和（6）式计算出的协方差矩阵也很接近。但要注意的是，（4）和（6）在推导的过程中都使用了 L(\mathbf{R}_\mathrm{opt})\approx0 这一条件。

在很多实际应用中，我们可能只用 IMU 的加速度计测量重力方向，特别是在室内磁场干扰很大的情况下。如果只能测量一个向量，那么 \mathbf{K} 的最大特征值对应的特征向量，以及 \mathbf{F} 的奇异值分解都不唯一，因为姿态有一个自由度不能确定。在这种情况下，理论上使用任何一个 \mathbf{R}_{\mathrm{opt}} 都是可以的，但在求 \mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta}) 时要特别注意，要把不能确定的那个自由度的方差设置为特别大，这样 MEKF 会几乎忽略这个自由度随意指定的值对通过积分得到的预测值的修改。具体做法是，在只有一个矢量测量时，（6）式中的 s_1=1 ， s_2=s_3=0 ，因此 \mathbf{D} 无法求逆。这时 \mathbf{I}_{3\times 3}-\mathbf{S}=\mathbf{D} ，所以 \mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta})\approx\sigma_{\mathrm{tot}}^2\mathbf{U}\mathrm{diag}(1/\varepsilon,1,1)\mathbf{U}^T ，其中 \varepsilon 是一个十分小的数，用来代替 0。

因为在 MEKF 测量更新的那一步，我们只需要计算预测的姿态 \mathbf{R}_{n|n-1} 和测量的姿态 \mathbf{R}_m 之间相差的旋转矢量（参考第 6 篇文章的公式 6），一种更合理的做法是直接将 \delta\bm{\theta}_m=\log(\mathbf{R}_{n|n-1}^T\mathbf{R}_m) 的方向设置为 \hat{\bm{v}}\times(\mathbf{R}_{n|n-1}^T\bm{v}) ，大小设置为 \hat{\bm{v}} 和 \mathbf{R}_{n|n-1}^T\bm{v} 相差的角度。这样做的几何解释是： \delta\bm{\theta}_m 是使测量到的 \hat{\bm{v}} 和预测的 \mathbf{R}_{n|n-1}^T\bm{v} 重合的旋转，从而 \delta\bm{\theta} 不会改变无法测量的那个自由度。

至此，我们已经有了两种方法将矢量测量转化为姿态测量，并将它们运用在 MEKF 中。如果你是只关心 MEKF 应用的工程师朋友们，那么这篇文章已经结束了。下面我会讨论一些纯理论的内容，将这两种方法用一种更宏观的理论联系起来，并且从这种宏观的理论出发，讨论是否可以做一些改进。

实际上 [2,3] 中的算法和上世纪六七十年代左右发展的一门叫作 directional statistics [4] 的学科有着密切的关系。Directional statistics 研究的内容是在圆上，球面上和其它一些具有高度对称性的流形上的概率分布和统计检验。下面介绍两种概率分布：定义在 SO(3) 群上的 Matrix-Fisher 分布和定义在单位四元数上的 Bingham 分布。其中 Matrix-Fisher 分布有如下的概率密度函数

f(\mathbf{R}|\mathbf{F})=\frac{1}{c(\mathbf{F})}\exp(\mathrm{tr}(\mathbf{FR}^T)) ，

其中 \mathbf{F} 是参数， c(\mathbf{F}) 是一个归一化的常数，也就是使概率密度在 SO(3) 群上的积分等于 1。Bingham 分布的概率密度函数是：

f(\bm{q}|\mathbf{K})=\frac{1}{c(\mathbf{K})}\exp(\bm{q}^T\mathbf{K}\bm{q}) ,

其中 \mathbf{K} 是参数， c(\mathbf{K}) 是归一化常数。可以看到，我们在 Wahba 问题中要求最大值的函数，实际上就是这两种概率分布的指数部分。可以用 [5] 中的方法证明，当单位矢量 \hat{\bm{v}}_i 的噪声服从 Von Mises-Fisher 分布（一个定义在二维球面上的概率分布）时，这些矢量对应的姿态 \mathbf{R} 服从 Matrix-Fisher 分布，参数 \mathbf{F} 通过如下的公式计算

\mathbf{F}=\sum_{i=1}^nw_i\bm{v}_i\hat{\bm{v}}_i^T （7）

其中 w_i 是 Von Mises-Fisher 分布的参数。一个重要的事实是当误差的分布比较集中时，Von Mises-Fisher 分布可以近似地理解为在二维球面的切平面上的正态分布，而且 w_i\approx 1/\sigma_i^2 。这样实际上我们把 Wahba 问题转化为了求 Matrix-Fisher 分布的最大似然估计的问题，而且结果与（5）式完全一致 [5]。更有趣的是，Matrix-Fisher 分布已经被证明和 Bingham 分布是等价的 [6]，因此求 Bingham 分布的最大似然估计也会得到相同的结果，这个结果和 QUEST 的计算完全一致 [7]。这样我们通过一个更宏观的理论将这篇文章介绍的两种算法联系了起来。

Matrix-Fisher 分布在概率密度比较集中的情况下近似于三维正态分布 [5]，我们可以通过这个性质来估计 \delta\bm{\theta} 的协方差矩阵，这里直接给出结果：

\mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta})=\sigma_{\mathrm{tot}}^2\mathbf{UD}^{-1}\mathbf{U}^T （8）

公式 (8) 和 (4)、(6) 在 L(\mathbf{R}_\mathrm{opt})\approx0 时是相近的，在 [3] 中作者指出在 L(\mathbf{R}_\mathrm{opt}) 较大时 (6) 要优于 (4)，但 (6) 和 (8) 的比较还不清楚。

文献 [2,3] 中的方法还有一个问题，就是它们都假设归一化后的单位向量 \hat{\bm{v}}_i 服从正态分布；但实际测量得到的向量一般长度都不是 1，例如重力和磁场。如果假设测量的向量在没有归一化之前的噪声服从正态分布，那么归一化后的噪声分布是定义在单位球面上的“投影正态分布”（projected normal distribution）。如果利用 [8] 中的方法将投影正态分布近似地转化为 Von Mises-Fisher 分布，根据公式（7），我们可以得到测量的旋转矩阵 \mathbf{R} 的概率分布的解析表达式。这样求出来的 \mathbf{R}_{\mathrm{opt}} 和 \mathrm{cov}(\delta\bm{\theta},\delta\bm{\theta}) ，我认为要比直接对噪声的方差作放缩使其归一化更准确。但这些推论还需要仿真和实验的验证。

这篇文章介绍了两种将矢量测量转换为姿态测量的方法，从而使经常用到的矢量测量可以和 MEKF 结合起来。两种方法分别是使用单位四元数的 QUEST 算法，和使用旋转矩阵的奇异值分解算法，这两种方法求出的最优姿态是一致的，姿态误差的协方差矩阵有一些区别，但在误差不大的情况下几乎一致。这两种算法可以通过定义在 SO(3) 群上的两种概率分布以一种统一的途径解释，并且在新的理论框架下还有一些提升精度的可能。

参考文献

[1] Wahba, Grace. "A least squares estimate of satellite attitude."SIAM review7.3 (1965): 409-409.

[2] Shuster, Malcolm David, and S. D_ Oh. "Three-axis attitude determination from vector observations."Journal of guidance and Control4.1 (1981): 70-77.

[3] Markley, F. Landis. "Attitude determination using vector observations and the singular value decomposition."Journal of the Astronautical Sciences36.3 (1988): 245-258.

[4] Mardia, Kanti V., and Peter E. Jupp.Directional statistics. Vol. 494. John Wiley & Sons, 2009.

[5] Lee, Taeyoung. "Bayesian attitude estimation with the matrix Fisher distribution on SO (3)."IEEE Transactions on Automatic Control63.10 (2018): 3377-3392.

[6] Prentice, Michael J. "Orientation statistics without parametric assumptions."Journal of the Royal Statistical Society: Series B (Methodological)48.2 (1986): 214-222.

[7] Glover, Jared, and Leslie Pack Kaelbling. "Tracking 3-D rotations with the quaternion Bingham filter." (2013).

[8] Presnell, Brett, and Pavlina Rumcheva. "The mean resultant length of the spherically projected normal distribution."Statistics & Probability Letters78.5 (2008): 557-563.