我一直在研究方向估计，我需要在直线行走时估计正确的航向。遇到一些障碍后，我又从基础开始。
我已经从here 实现了一个互补滤波器，它使用从 Android 获得的重力矢量（不是原始加速度）、原始陀螺仪数据和原始磁力计数据。我还在陀螺仪和磁力计数据上应用低通滤波器并将其用作输入。
互补滤波器的输出是欧拉角，我还记录了 TYPE_ROTATION_VECTOR，它以 4D 四元数形式输出设备方向。
所以我想将四元数转换为欧拉，并将它们与从互补滤波器获得的欧拉进行比较。当手机静止在桌子上时，欧拉角的输出如下所示。
可以看出，Yaw 的值相差很大。
当手机静止时，这个简单的案例我做错了什么
然后我走进客厅，得到以下输出。
Complementary filter 的造型看起来很不错，和安卓的很接近。但值相差很大。
请告诉我我做错了什么？
【问题讨论】：
标签： orientation accelerometer android-orientation gyroscope sensor-fusion
我认为没有必要对陀螺仪应用低通滤波器。由于您正在集成陀螺仪以进行旋转，因此可能会搞砸一切。
请注意，TYPE_GRAVITY 是一种复合传感器读数，由 Android 自己的传感器融合算法中的陀螺仪和加速度合成。也就是说，这已经通过了卡尔曼滤波器。如果你还是要使用 Android 的内置传感器融合，为什么不直接使用 TYPE_ROTATION_VECTOR？
从外观上看，您的角度是弧度，第一组的误差与 90 度相差不远。也许您在磁力计输入中交换了 X 和 Y？
这是我将采取的方法：首先编写一个测试，该测试采用加速度和陀螺仪并从中合成欧拉角。暂时忽略陀螺仪。在房子周围走动，确认它做对了，但很紧张。
接下来，在你的算法上添加一个激进的低通滤波器，例如
yaw0 = yaw;
yaw = computeFromAccelMag(); // yaw in radians
factor = 0.2; // between 0 and 1; experiment
yaw = yaw * factor + yaw0 * (1-factor);
确认这仍然有效。它应该不会那么紧张，但也应该很迟钝。
最后，添加陀螺仪并用它制作一个互补滤波器。
dt = time_since_last_gyro_update;
yaw += gyroData[2] * dt; // test: might need to subtract instead of add
yaw0 = yaw;
yaw = computeFromAccelMag(); // yaw in radians
factor = 0.2; // between 0 and 1; experiment
yaw = yaw * factor + yaw0 * (1-factor);
他们的关键是在开发算法时测试每一步，这样当错误发生时，你就会知道是什么原因造成的。
【讨论】：