矢量归一化_7. 从矢量观测到姿态观测

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

1. Wahba 问题

将方向测量转化为姿态测量，这一问题在姿态估计的文献中被称为 Wahba 问题（Wahba's Problem）[1]。它的表述如下：如果在惯性坐标系内有

个单位矢量，分别记为

，

；在 IMU 坐标系下测量这些单位矢量得到的值为

，

，那么如何找到一个旋转矩阵

，使得下面的误差函数最小

（1）

其中

是一组权重。首先要说明的是，旋转矩阵具有三个自由度，每个矢量测量具有两个自由度。因此，当只有一个矢量测量时，这是一个欠约束问题，也就是有无数个旋转矩阵

可以使

；当有两个或以上的矢量测量时，这是一个过约束问题，除非这些矢量测量完全没有误差，否则

。其次，在 Wahba 问题中，这些矢量

已经被归一化，也就是我们只考虑

的方向，不考虑它的长度。下面我们对公式（1）做一些简化：

在数理统计中有一个很著名的技巧叫“trace trick”，也就是把一个二次型转化为矩阵的迹，公式是

，用在上面的公式中可以得到：

（2）

其中

，

。这样，我们把求

使得

最小的问题转化为了使

最大的问题。下面我们介绍如何解决这一问题。

2. QUEST 算法

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

（3ÿ