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

Question

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

 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

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

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

当处于稳定速度（即，不改变速度或方向）时，值ax，ay，az各自仅测量加速度的重力分量。加速时该值将不准确。通过组合来自其他传感器（如陀螺仪或磁力计）和其他加速器的信息 - 当固定轴，ay，az总和达到1G - 其他任何东西并且还有一个额外的加速度时，你必须更聪明一点。加速器测量加速度，但包括重力加速度。也就是说，当处于稳定速度时，加速器是相对于重力的倾斜传感器。

磁力计计算是从其x和y分量确定相对于北方的角度，并补偿真北和磁北（the magnetic declination）之间的差异。当磁力计与磁场对齐时，磁力计的x和y分量最大，因为它们是正交性的，它们的相对值使用atan2（mx，my）分解为单一方向。 x和y从传统顺序交换，因为罗盘方向顺时针增加，而数学角度逆时针增加。

在实践中，您需要使用传感器功能来组合来自陀螺仪（角速度），加速器和磁力计的信息，并且可能还应用一些启发式方法来准确跟踪运动。这些传感器中的每一个都具有不同的混杂因素，并且在一个外力可能对多于一个传感器产生影响的意义上，一些测量元素重叠。这可以用于区分不同类型的运动和态度。这很复杂，这可能就是为什么这并不试图解决它。

Answer 2

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

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