我之前在移动端姿态估计项目里也碰到过一模一样的问题——运动时算法跟踪挺准，但一静置就开始飘，调固定β和采样率都没彻底解决。后来结合算法原理和传感器特性，试了几个方法，效果都不错：

1. 动态调整滤波器增益β #

Madgwick的固定β值是一把双刃剑：大β能快速跟踪姿态变化，但会放大传感器噪声；小β能抑制噪声，但运动时响应变慢。静置时设备的姿态几乎不变，应该让β自适应减小，让算法更依赖加速度计/磁力计的稳定观测，减少陀螺仪噪声的积分漂移。

你可以通过加速度计的模长判断设备是否静置：

// 计算当前加速度模长与标准重力的偏差，判断运动状态
float accMag = (float) Math.sqrt(accX*accX + accY*accY + accZ*accZ);
// 运动加速度=实际加速度模长-重力加速度（9.81m/s²）
float motionAccel = Math.abs(accMag - 9.81f);

// 动态调整β：静置时用极小值，运动时恢复正常增益
float beta = motionAccel > 0.5f ? 0.1f : 0.001f; // 阈值和β值可根据你的硬件微调

我当时把静置时的β从0.1降到0.001，漂移速度直接降了一个数量级。

2. 彻底校准加速度计和磁力计 #

静置时Madgwick完全依赖加速度计的重力向量和磁力计的北向向量做姿态约束，如果这两个传感器没校准准，观测值本身就有偏差，算法自然会越飘越远。

• 加速度计校准：把手机分别平放在桌面、倒扣、左右侧躺、前后侧躺，每个姿态静置3-5秒，记录每个方向的加速度输出，计算偏移量，之后每次读取传感器数据都先减去偏移量，确保静置时加速度模长接近9.81。
• 磁力计校准：在空旷无电磁干扰的地方，拿着手机画8字（尽量覆盖所有空间方向），完成系统级的磁力计校准，消除硬铁/软铁干扰，保证静置时磁力计输出稳定。

校准后，算法的观测约束会准确很多，静置漂移会大幅减少。

3. 静置时增加姿态锁定逻辑 #

如果设备长时间静置（比如超过1秒），可以暂时降低陀螺仪的权重，甚至直接用加速度计+磁力计计算的姿态修正陀螺仪积分结果，避免噪声累积：

private long staticStartTime = 0;
private boolean isStatic = false;

// 在更新函数里判断静置时长
if (motionAccel < 0.3f) { // 更小的阈值判断严格静置
    if (!isStatic) {
        staticStartTime = System.currentTimeMillis();
        isStatic = true;
    }
    // 静置超过1秒，直接用加速度计+磁力计计算姿态
    if (System.currentTimeMillis() - staticStartTime > 1000) {
        float[] staticAttitude = calculateAttitudeFromAccelMag(accX, accY, accZ, magX, magY, magZ);
        // 用计算出的姿态替换当前四元数，或者做平滑过渡
        quaternion.slerp(staticAttitude, 0.1f); // 0.1是平滑系数，避免突变
    }
} else {
    isStatic = false;
    // 正常运行Madgwick梯度下降更新
    updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ, beta);
}

这里的calculateAttitudeFromAccelMag是用加速度计和磁力计直接计算欧拉角或四元数的函数，网上很容易找到实现。

4. 预处理传感器原始数据 #

传感器的高频噪声是静置漂移的隐形杀手——陀螺仪的微小噪声会被积分放大，导致姿态慢慢偏移。你可以给传感器数据加一阶低通滤波：

// 低通滤波系数alpha，0.9左右比较合适（值越大，平滑效果越强，响应越慢）
private static final float ALPHA = 0.9f;

// 对陀螺仪数据滤波
gyroX = ALPHA * gyroX + (1 - ALPHA) * rawGyroX;
gyroY = ALPHA * gyroY + (1 - ALPHA) * rawGyroY;
gyroZ = ALPHA * gyroZ + (1 - ALPHA) * rawGyroZ;

// 加速度计也可以滤波，但要注意不要过度，避免运动时延迟
accX = ALPHA * accX + (1 - ALPHA) * rawAccX;
accY = ALPHA * accY + (1 - ALPHA) * rawAccY;
accZ = ALPHA * accZ + (1 - ALPHA) * rawAccZ;

另外还要确保传感器采样率和算法更新率严格匹配，比如传感器设置为100Hz采样，算法就要每10ms更新一次，dt（时间步长）计算准确，否则陀螺仪积分会出现误差。

内容的提问来源于stack exchange，提问作者Jopo Podpleský