基于角速度积分的IMU姿态解算旋转量约为预期值一半的问题排查

我目前正参考Madgwick滤波器官方文档实现空间中IMU的姿态解算。我已完成基于磁力计的姿态解算方案，但该方案在载体快速运动时性能劣化严重，因此计划切换为基于陀螺仪角速度积分的解算路线。

当前解算得到的旋转趋势符合预期（我使用四元数转换生成的旋转矩阵实时渲染彩色立方体验证效果），但旋转速率远低于实际值：代码输出的旋转量约为真实世界对应旋转量的1/2，存在小幅偏差。

我曾尝试直接将旋转角度乘以2，但该方案存在两个问题：

• 无法完全解决问题，解算结果仍存在明显滞后
• 无法帮助我定位问题的根本原因

我想确认当前实现思路是否存在概念性错误？是否存在我未注意到的核心缺陷？该实现用于我运动科学方向的硕士论文，万分感谢各位的帮助。

排查进展更新 #

我已采纳建议将Quaternion.CreateFromYawPitchRoll()接口替换为Quaternion.CreateFromAxisAngle()接口，但问题并未解决。我已为所用Quaternion类编写单元测试，验证其行为符合预期，因此排除四元数类本身的实现错误。

参考《Efficient quaternion angular velocity》一文的说明，我尝试使用半角方式实现，但结果与预期一致——旋转速率进一步降低。

目前我仍未解决旋转量约为预期值一半的问题，核心实现代码如下：

/// <summary>
/// 对x,y,z轴角速度进行数值积分，得到x,y,z轴方向的姿态
/// 存在问题：实际旋转速率约为真实世界旋转速率的一半（存在小幅偏差）
/// </summary>
/// <param name="dt">距上次调用的时间间隔，单位为毫秒</param>
private void UpdateAngularVelocities(double dt)
{
    // 旋转阈值（单位：°/秒），用于最小化噪声影响
    // 临时设置为0
    double threshold = 0.0; 

    // 基于Madgwick滤波器的迭代四元数乘法逻辑
    // 存储AngularOrientation向量用于效果展示

    // 应用旋转速度阈值
    // 即丢弃低于设定阈值的慢速旋转
    var wtx = (Math.Abs(w[0]) > threshold) ? w[0] : 0.0;
    var wty = (Math.Abs(w[1]) > threshold) ? w[1] : 0.0;
    var wtz = (Math.Abs(w[2]) > threshold) ? w[2] : 0.0;

    // 旋转速度存储为向量格式方便后续调用
    // 用于构造旋转四元数
    Vector3 deltaTRotation = new Vector3((float)ConvertToRadians(wtx), (float)ConvertToRadians(wty), (float)ConvertToRadians(wtz));
    Vector3 deltaAxis = deltaTRotation;

    // 按时间缩放旋转量
    // 输入dt单位为毫秒，需转换为秒，从而将°/s转换为°
    // 依据四元数角速度积分规则使用半角计算
    deltaTRotation.X *= (float)(dt / 1000.0) * 0.5f;
    deltaTRotation.Y *= (float)(dt / 1000.0) * 0.5f;
    deltaTRotation.Z *= (float)(dt / 1000.0) * 0.5f;


    // 基于缩放后的旋转量构造四元数
    // 使用Quaternion.CreateFromAxisAngle()实现多轴同时旋转计算
    // Quaternion qDelta = Quaternion.CreateFromYawPitchRoll(deltaTRotation.Y, deltaTRotation.X, deltaTRotation.Z);
    Quaternion qDelta = Quaternion.CreateFromAxisAngle(Vector3.Normalize(deltaAxis), deltaTRotation.Length());

    // 处理NaN值：无旋转时||qCurrent||=0，归一化操作会产出NaN，需做边界判断
    if (qDelta.Length() > 0.0)
    {
        // 将新增旋转与历史旋转合并
        qRotation = qDelta * qRotation;
        //AdditiveOrientation += 0.5f * Vector4.Normalize(new Vector4(deltaTRotation.X, deltaTRotation.Y, deltaTRotation.Z, deltaTRotation.W)) * qDelta.Length();
    }

    // Madgwick滤波器文档提出可通过旧状态与新状态相加的方式更新
    // 该方式会导致四元数不再是单位四元数（versor），因此弃用加法逻辑，保留简单乘法更新
    //qRotation = qRotation + Quaternion.Multiply(qCurrent * qRotation, (float)(0.5));// * dt / 1000.0)); 
    //qRotation = Quaternion.Normalize(qRotation);


    // 旋转磁场方向用于效果展示
    // 始终从任意初始向量[1,0,0]开始，应用最新四元数做旋转
    // 目前无法正常工作
    // 旋转角度约为预期值的一半
    AngularOrientation = Vector4.Transform(new Vector4(1.0f,0.0f,0.0f,1.0f), qRotation);

    // 存储角速度数据供后续使用
    OmegaXData.Add(deltaTRotation.X);
    OmegaYData.Add(deltaTRotation.Y);
    OmegaZData.Add(deltaTRotation.Z);
}

内容的提问来源于stack exchange，提问作者Lukas Pezenka