Fix one-sample angular velocity spike when the IMU rotation passes 2nπ

Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs

@@ -50,6 +50,32 @@ protected override void Init()
             _gravityMagnitude = Physics.gravity.magnitude;
         }
 
+        /// <summary>
+        /// Mean angular velocity [rad/s] of the rotation taking
+        /// <paramref name="previous"/> to <paramref name="current"/> over
+        /// <paramref name="dt"/> seconds, always measured along the short arc.
+        /// </summary>
+        /// <remarks>
+        /// Quaternions double-cover rotations (q and -q are the same
+        /// rotation), and consecutive transform.rotation samples can land on
+        /// opposite hemispheres once the accumulated rotation passes a
+        /// multiple of 2*pi. ToAngleAxis on such a delta reports the LONG way
+        /// around -- ~(360 deg - delta) about the inverted axis -- so the
+        /// angular velocity spikes to ~2*pi/dt for one sample
+        /// (Field-Robotics-Japan/UnitySensors#155). Folding the delta onto
+        /// the w >= 0 hemisphere makes ToAngleAxis measure the short arc.
+        /// </remarks>
+        public static Vector3 AngularVelocityBetween(Quaternion previous, Quaternion current, float dt)
+        {
+            Quaternion delta = Quaternion.Inverse(previous) * current;
+            if (delta.w < 0.0f)
+            {
+                delta = new Quaternion(-delta.x, -delta.y, -delta.z, -delta.w);
+            }
+            delta.ToAngleAxis(out float angle, out Vector3 axis);
+            return axis * (angle * Mathf.Deg2Rad / dt);
+        }
+
         public override IEnumerator UpdateSensorOnce()
         {
             //FIXME: IMU sensor should be updated at a fixed frequency
@@ -61,10 +87,7 @@ public override IEnumerator UpdateSensorOnce()
             _acceleration_tmp -= _transform.InverseTransformDirection(_gravityDirection) * _gravityMagnitude;
 
             _rotation_tmp = _transform.rotation;
-            Quaternion rotation_delta = Quaternion.Inverse(_rotation_last) * _rotation_tmp;
-            rotation_delta.ToAngleAxis(out float angle, out Vector3 axis);
-            float angularSpeed = (angle * Mathf.Deg2Rad) / dt;
-            _angularVelocity_tmp = axis * angularSpeed;
+            _angularVelocity_tmp = AngularVelocityBetween(_rotation_last, _rotation_tmp, dt);
 
             _position_last = _position_tmp;
             _velocity_last = _velocity_tmp;

Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs

@@ -0,0 +1,69 @@
+using NUnit.Framework;
+using UnityEngine;
+using UnitySensors.Sensor.IMU;
+
+namespace UnitySensors.Tests.Editor
+{
+    /// <summary>
+    /// Regression tests for Field-Robotics-Japan/UnitySensors#155: the
+    /// angular velocity must stay on the short arc even when consecutive
+    /// rotation samples land on opposite hemispheres of the quaternion
+    /// double cover (which happens every time the accumulated rotation
+    /// passes a multiple of 2*pi).
+    /// </summary>
+    [TestFixture]
+    public class ImuAngularVelocityTests
+    {
+        private const float kDt = 0.05f;
+
+        private static void AssertRate(Vector3 actual, Vector3 expected, string message)
+        {
+            Assert.That((actual - expected).magnitude, Is.LessThan(1e-3f), message +
+                $" (actual {actual}, expected {expected})");
+        }
+
+        [Test]
+        public void SmallStep_ReportsTheRotationRate()
+        {
+            Quaternion previous = Quaternion.identity;
+            Quaternion current = Quaternion.AngleAxis(2.0f, Vector3.up);
+
+            Vector3 omega = IMUSensor.AngularVelocityBetween(previous, current, kDt);
+
+            AssertRate(omega, Vector3.up * (2.0f * Mathf.Deg2Rad / kDt),
+                "a plain small step must come out as angle / dt");
+        }
+
+        [Test]
+        public void FullTurnBoundary_DoesNotSpike()
+        {
+            // Crossing 360 deg of accumulated rotation: AngleAxis(359) sits on
+            // the w < 0 hemisphere, AngleAxis(1) ( = 361) on w > 0. Before the
+            // fix the delta was read the long way around and the rate spiked
+            // to ~(360 deg - step) / dt -- the exact symptom of issue #155.
+            Quaternion previous = Quaternion.AngleAxis(359.0f, Vector3.up);
+            Quaternion current = Quaternion.AngleAxis(1.0f, Vector3.up);
+
+            Vector3 omega = IMUSensor.AngularVelocityBetween(previous, current, kDt);
+
+            AssertRate(omega, Vector3.up * (2.0f * Mathf.Deg2Rad / kDt),
+                "a 2 deg step across the full-turn boundary must stay a 2 deg step");
+        }
+
+        [Test]
+        public void NegatedRepresentation_IsTheSameRotation()
+        {
+            // q and -q describe the same attitude; feeding the negated
+            // representation must not change the measured rate.
+            Quaternion previous = Quaternion.AngleAxis(10.0f, Vector3.up);
+            Quaternion step = Quaternion.AngleAxis(12.0f, Vector3.up);
+            Quaternion negated = new Quaternion(-step.x, -step.y, -step.z, -step.w);
+
+            Vector3 fromPlain = IMUSensor.AngularVelocityBetween(previous, step, kDt);
+            Vector3 fromNegated = IMUSensor.AngularVelocityBetween(previous, negated, kDt);
+
+            AssertRate(fromNegated, fromPlain,
+                "the negated quaternion is the same rotation and must give the same rate");
+        }
+    }
+}