漂移消除

虽然陀螺表现相当好，在指令下面每秒的误差很小，但最终对陀螺仪的漂移我们不得不做一些事情。所要做的就是利用其它方向的参考来探测陀螺偏移，提供一个经典的PID负反馈检测回路给陀螺补偿误差。如图1所示。步骤如下：

1. 使用方向参考向量探测定向误差，通过计算一个旋转矢量，将测量值和计算值的参考矢量调整。
2. 通过一个比例积分（PI）反馈控制器来产生的陀螺旋转校准速度，将旋转矢量误差反馈。（PI调节器是常用的PID反馈调节器的一个特例，D代表微分。在我们的这个例子中，我们不需要用到微分项。）
3. 加上（或者减去，这依赖于你对旋转误差的符号约定）比例积分控制器的输出到实际的陀螺仪信号。

对方向参考向量的主要要求是：它不漂移。其瞬态性能并不那么重要是因为陀螺对方向估计有瞬态保真性。

GPS和加速度计为我们提供了两个参考向量提供。磁力计也是非常有用的，特别对偏航的控制，但是对飞行器飞行的指向，仅一个全球定位系统就做得很好。如果你使用磁力计，你应该使用一个三轴磁力计提供一个矢量参考。 低成本三轴磁力计在市场上可以很方便的买到。

我们使用加速度计为飞机的Z轴提供的参考矢量飞。将在一个单独的部分中给出细节。我们使用GPS作为飞机X-轴（滚转轴）的水平投影参考。我们的2个参考向量恰好是彼此垂直的。这很方便，但不是绝对必需的。

对于两个参考向量中的任一个，通过测得的向量的交叉乘积检测方向误差，这个测得的矢量用方向余弦矩阵估计。这个叉积特别合适有两个理由。它的大小与两个矢量夹角是正弦成正比，同时它的方向垂直这两个矢量。所以它代表一个旋转轴和旋转量，需要旋转这个测量的矢量使它变得与这个估计的矢量平行。换句话说，它等于方向旋转误差的负值。通过比例积分控制器把它反馈到陀螺仪，定位估计逐渐被迫跟踪参考向量，这样陀螺的漂移就被消除了。

测量从方向余弦矩阵计算出相应的参考矢量的叉积是一个错误的指示。它大约等于这个旋转，将不得不用这个参考矢量和这个计算矢量校准。我们感兴趣的是旋转校正量，这个量我们需要用在方向余弦矩阵中，它等于旋转误差的负值。通过交换叉积的秩序可以很方便的计算修正。矫正旋转等于通过参考矢量的方向余弦的矢量估计的叉积。

我们用比例积分控制器来矫正陀螺旋转，因为它稳定，同时这个积分项完全消除了陀螺仪的漂移，包括热漂移，零残余方向误差。

参考矢量误差是通过方向余弦矩阵的方式映射到陀螺的。所以这样的映射取决于惯性测量单元。例如，GPS参考矢量可能矫正X、Y、Z或者X、Y和Z轴陀螺信号，取决于地球坐标系轴的方向。

我们现在将获得更多关于我们所用的GPS和加速度计这两个参考的细节。