根据IMU的加速度计和陀螺仪估算姿态？

Question

我很难理解下面包含的方程的数学推导。这段代码是来自here的SparkFun IMU库示例的一部分。 有人可以帮我理解使用反正切函数估算侧倾，俯仰和偏航的理论吗？磁力计数据如何用于获得正确的偏航估计？我可以在网上找到的所有资源都无法以我能理解的方式回答这些问题。

 float roll = atan2(ay, az);
 float pitch = atan2(-ax, sqrt(ay * ay + az * az));

 float heading;
 if (my == 0)
   heading = (mx < 0) ? PI : 0;
 else
 heading = atan2(mx, my);

 heading -= EARTH_DECLINATION * PI / 180;

 if (heading > PI) heading -= (2 * PI);
 else if (heading < -PI) heading += (2 * PI);

 // Convert everything from radians to degrees:
 heading *= 180.0 / PI;
 pitch *= 180.0 / PI;
 roll  *= 180.0 / PI;

Answer 1

假设ay和az是磁力计给出的距原点的偏移量，那么atan（ay，az）将为您提供产生该偏移量的角度。

sqrt（ay * ay + az * az）遵循毕达哥拉斯定理，为您提供“偏移三角形”的第三边的长度，以便能够计算音高。 -ax来自于音高的定义方式。

Answer 2

atan2（y，x）是角度theta，如下图所示：

在三个维度中，您具有三个平面，并且atan2（）应用于x，y和z中的一对，具体取决于您要为其计算theta的平面（滚动，俯仰，偏航）。

在稳定速度下（即不改变速度或方向）时，值ax，ay，az分别仅测量加速度的重力分量。该值在加速度下将不准确。此时，您必须结合其他传感器（例如陀螺仪或磁力计）和其他加速器的信息（即当固定ax，ay，az总和为1G时）变得更加聪明，这需要其他任何方法，并且还会产生额外的加速度。加速器可测量加速度，但包括重力引起的加速度。即，在稳定速度下，加速器是相对于重力的倾斜传感器。

磁力计的计算是从x和y分量确定相对于北方的角度，并补偿真实北方和磁性北方（the magnetic declination）之间的差异。磁力计的x和y分量在与磁场对准时最大，因为它们是正交安装的，所以它们的相对值使用atan2（mx，my）解析为一个方向。因为指南针方向顺时针方向增加，而数学角度逆时针方向增加，所以x和y被替换为常规顺序。

在实践中，您需要使用传感器功能来组合来自陀螺仪（角速度），加速器和磁力计的信息，并且可能还应用一些启发式方法来准确跟踪运动。这些传感器中的每一个都有不同的混杂因素，并且某种意义上的某些测量要素存在重叠，因为单个外力会对一个以上的传感器产生影响。这可以用来区分不同的运动和姿势。它很复杂，这可能就是为什么它不尝试对其进行处理的原因。