From ae3300318168fe783c6c7ab4bc8faf507692947b Mon Sep 17 00:00:00 2001 From: Masaaki Hijikata Date: Wed, 19 Aug 2026 17:38:23 +0900 Subject: [PATCH] Fold the IMU rotation delta onto one hemisphere before ToAngleAxis Fixes the transient angular velocity spike reported in Field-Robotics-Japan/UnitySensors#155: after the accumulated rotation passes a multiple of 2*pi, the angular velocity goes wild for one sample. Quaternions double-cover rotations -- q and -q describe the same attitude -- and consecutive transform.rotation samples can land on opposite hemispheres of that cover. The delta quaternion then has w < 0, and ToAngleAxis reports the LONG way around: ~(360 deg - delta) about the inverted axis instead of the true small increment. Divided by dt this shows up as a one-sample angular velocity spike of roughly 2*pi/dt (thousands of deg/s), exactly at full-turn boundaries. Negate the delta quaternion when w < 0 (a no-op as a rotation) so ToAngleAxis always measures the short arc. The delta-to-rate math moves into a public static AngularVelocityBetween() so it is unit testable; ImuAngularVelocityTests covers the plain small step, the issue's full-turn boundary crossing (AngleAxis(359) -> AngleAxis(1), which spiked before the fix), and the negated-representation equivalence. Co-Authored-By: Claude Fable 5 --- .../Runtime/Scripts/Sensors/IMU/IMUSensor.cs | 31 +++++++-- .../Tests/Editor/ImuAngularVelocityTests.cs | 69 +++++++++++++++++++ .../Editor/ImuAngularVelocityTests.cs.meta | 11 +++ 3 files changed, 107 insertions(+), 4 deletions(-) create mode 100644 Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs create mode 100644 Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs.meta diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs b/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs index bd05a96a..6148c0cb 100644 --- a/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs +++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs @@ -50,6 +50,32 @@ protected override void Init() _gravityMagnitude = Physics.gravity.magnitude; } + /// + /// Mean angular velocity [rad/s] of the rotation taking + /// to over + /// seconds, always measured along the short arc. + /// + /// + /// 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. + /// + 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; diff --git a/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs b/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs new file mode 100644 index 00000000..c328afc8 --- /dev/null +++ b/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs @@ -0,0 +1,69 @@ +using NUnit.Framework; +using UnityEngine; +using UnitySensors.Sensor.IMU; + +namespace UnitySensors.Tests.Editor +{ + /// + /// 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). + /// + [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"); + } + } +} diff --git a/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs.meta b/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs.meta new file mode 100644 index 00000000..5bd8bd92 --- /dev/null +++ b/Packages/UnitySensors/Tests/Editor/ImuAngularVelocityTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5c8b3df10f794f9d8408fe1e0fab89b5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: