Skip to content

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

Open
hijimasa wants to merge 1 commit into
Field-Robotics-Japan:masterfrom
hijimasa:fix/ImuAngularVelocitySpikeAtFullTurn
Open

hijimasa wants to merge 1 commit into
Field-Robotics-Japan:masterfrom
hijimasa:fix/ImuAngularVelocitySpikeAtFullTurn

Conversation

@hijimasa

Copy link
Copy Markdown
Contributor

Fixes #155

Symptom

The moment an object carrying the IMU passes a multiple of 2π of
accumulated rotation (i.e. every full turn), the reported angular
velocity goes wild for exactly one sample — roughly 2π/dt, which is
about 7200 deg/s at a 50 Hz update rate.

Root cause

Quaternions double-cover rotations: q and −q describe the same
attitude. Consecutive transform.rotation samples can land on opposite
hemispheres of that cover, and when they do, the delta
Quaternion.Inverse(last) * current comes out with w < 0.
ToAngleAxis on such a quaternion reports the long way around
~(360° − δ) about the inverted axis instead of the true small increment
δ. Divided by dt, that is the one-sample spike, and it lands exactly at
full-turn boundaries.

Fix

  • When the delta quaternion has w < 0, negate all four components
    before calling ToAngleAxis (a no-op as a rotation), so the short
    arc is always measured.
  • To make the math unit-testable, the delta-to-rate computation is
    extracted into public static IMUSensor.AngularVelocityBetween( previous, current, dt), called from UpdateSensorOnce(). Behaviour
    is otherwise unchanged.

Tests (EditMode: Tests/Editor/ImuAngularVelocityTests.cs)

Test What it checks Before fix After fix
SmallStep_ReportsTheRotationRate a plain small step comes out as angle / dt Pass Pass
FullTurnBoundary_DoesNotSpike a 2° step across the full-turn boundary (AngleAxis(359°) → AngleAxis(1°)) — the exact scenario of this issue Fail (reports ~358°/dt) Pass
NegatedRepresentation_IsTheSameRotation feeding the −q representation yields the same rate Fail Pass

Verified with the Unity Test Runner (6000.3.21f1, batchmode, EditMode):
3/3 pass with the fix; with the fold removed, exactly the two boundary
tests fail while the plain-step test still passes, so the tests isolate
this specific defect.

Impact

All downstream consumers of the angular velocity. Attitude-integrating
consumers (e.g. an AHRS) are barely affected, because rotating by
(2π − δ) about the inverted axis is almost the same attitude as
rotating by δ about the true axis — but anything using the raw rate
(rate logging, rate sanity checks that drop the sample, controllers)
sees a real glitch once per full turn.

Fixes the transient angular velocity spike reported in
Field-Robotics-Japan#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 <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

[IMU]2nπ回転した際、角速度の値がおかしくなる

1 participant