IMU的态度估计来自角速度的态度旋转大约是预期值的一半。为什么？

Question

我正在努力按照 https://ahrs.readthedocs.io/en/latest/filters/madgwick.html 。我已经可以基于磁力计确定方向（但是，这遭到了非常快速的动作，因此我想走陀螺仪路线）。

旋转基本上可以执行它应该做的事情（我使用四元组的旋转矩阵实时渲染一个彩色盒子），但速率要低得多。从本质上讲，由我的代码产生的旋转大约（但不是很快）的1/2速度与现实世界中的旋转速度一样快。

简单地将角度乘以2

• 并不能完全解决问题（仍然存在某个滞后）
• 并没有帮助我理解问题

是我在概念上正在做的事情吗？我只是看不到一个主要问题吗？由于我需要这些东西来撰写主人的论文（在运动科学中），因此对任何帮助都非常感谢。

更新：我已经实施了@jamestursa的建议，并切换到quaternion.createfromaxisangle（）而不是quaternion.createfromyawpitchroll（）。不幸的是，这并不能解决问题。我还为四个类别类实施了单位测试，以检查其表现是否如预期的。确实如此，所以我怀疑这是错误的。

阅读有效的quaternion角度速度 ，但这完全可以做我想的，只是进一步减慢了旋转。

因此，我仍然被旋转所困扰，而旋转基本上大约是（但不是）应该做的一半。同样，任何帮助都会受到调查。

/// <summary>
        /// Numerically integrate angular velocities in x,y,z to get orientation in x,y,z
        /// Does not work. Rotation happens at around (but not quite) half the rate as in the real world.
        /// </summary>
        /// <param name="dt">time in ms since last invocation</param>
        private void UpdateAngularVelocities(double dt)
        {
            // rotation threshold (°/sec) to minimize noise
            // set to 0 for the time being
            double threshold = 0.0; 

            // iterative quaternion multiplication based on madgwick filter
            // https://ahrs.readthedocs.io/en/latest/filters/madgwick.html
            // keep an AngularOrientation Vector for illustration purposes

            // apply rotation velocity threshold
            // ie, discard any rotation that is slower than the defined threshold
            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;

            // rotation velocities, stored as a Vector for easier use
            // theses will be used to onstruct the Quaternion
            Vector3 deltaTRotation = new Vector3((float)ConvertToRadians(wtx), (float)ConvertToRadians(wty), (float)ConvertToRadians(wtz));
            Vector3 deltaAxis = deltaTRotation;

            // scale rotation with time
            // dt is supplied in ms, we need s to get from °/s to °
            // use half angle because of
            // https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity/24201879#24201879
            deltaTRotation.X *= (float)(dt / 1000.0) * 0.5f;
            deltaTRotation.Y *= (float)(dt / 1000.0) * 0.5f;
            deltaTRotation.Z *= (float)(dt / 1000.0) * 0.5f;


            // build quaternion from scaled rotations
            // use Quaternion.CreateFromAxisAngle() to get simultaneous angles
            // https://stackoverflow.com/questions/72578478/imu-orientation-from-gyro
            // Quaternion qDelta = Quaternion.CreateFromYawPitchRoll(deltaTRotation.Y, deltaTRotation.X, deltaTRotation.Z);
            Quaternion qDelta = Quaternion.CreateFromAxisAngle(Vector3.Normalize(deltaAxis), deltaTRotation.Length());

            // deal with NaN, in case no Rotation takes place and hence, ||qCurrent|| = 0 which makes Normalize throw NaN
            if (qDelta.Length() > 0.0)
            {
                // combine new rotation with previous rotation
                qRotation = qDelta * qRotation;
                //AdditiveOrientation += 0.5f * Vector4.Normalize(new Vector4(deltaTRotation.X, deltaTRotation.Y, deltaTRotation.Z, deltaTRotation.W)) * qDelta.Length();
            }

            // https://ahrs.readthedocs.io/en/latest/filters/madgwick.html suggests an addition of the old state and the new state.
            // this results in the quaternion not being a unit quaternion / versor anymore. Ditch the addition and stick with simple multiplication
            //qRotation = qRotation + Quaternion.Multiply(qCurrent * qRotation, (float)(0.5));// * dt / 1000.0)); 
            //qRotation = Quaternion.Normalize(qRotation);


            // rotate magnetic orientation for illustration purposes
            // always start rotation from (arbitrary) [1,0,0] and apply most recent quaternion
            // does not work yet
            // rotates roughly half the angle it should
            AngularOrientation = Vector4.Transform(new Vector4(1.0f,0.0f,0.0f,1.0f), qRotation);

            // save angular velocities for later use
            OmegaXData.Add(deltaTRotation.X);
            OmegaYData.Add(deltaTRotation.Y);
            OmegaZData.Add(deltaTRotation.Z);
        }