Divide IMU derivatives by the real sample interval, not one frame's deltaTime - #215
Open
hijimasa wants to merge 1 commit into
Open
Conversation
Fixes the unstable, too-large velocity and angular velocity readings reported in Field-Robotics-Japan#156 (Frequency set to 100 Hz or more). UpdateSensorOnce differentiates position and rotation across the whole interval since the previous sensor update, but divided by a single frame's Time.deltaTime. The two only agree when the sensor happens to update exactly once per frame; whenever the actual sample spacing is longer -- several frames at low frame rates, or one-to-two frames jittering with the scheduler when the requested frequency is at or above the frame rate -- every derivative comes out scaled by (actual sample spacing / frame time). That is exactly the reported symptom: values larger than reality and fluctuating, getting worse as the Frequency setting rises past the frame rate. Measured on a robot simulator: a 20 Hz IMU rendered at 30 FPS reported ~2x the true angular velocity. Track the time of the previous update (the until-now unused _time_last) and divide by that real elapsed time. The first update falls back to Time.deltaTime, as does a degenerate non-positive interval. Note the effective sample rate is still capped by the frame rate; this change makes the reported values correct at whatever spacing the scheduler actually achieves. ImuDerivativeTimingTests (play mode) drives a 4 Hz sensor -- so every sample spans several frames -- on an object rotating and translating at known constant rates, and asserts the reported magnitudes match the truth; before the fix they read several times too large. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Fixes #156
Symptom
With the Frequency parameter set to 100 Hz or more, the reported
velocity and angular velocity become larger than the true values and
unstable.
Root cause
UpdateSensorOnce()differentiates position and rotation across thewhole interval since the previous sensor update, but divides by a
single frame's
Time.deltaTime(the//FIXMEin the code marks thespot). The two only agree when the sensor happens to update exactly
once per frame:
scheduler spaces samples one-to-two frames apart, so the readings
jitter between 1× and 2× the true value.
and all derivatives come out scaled by (sample spacing / frame time).
Measured on a robot simulator, a 20 Hz IMU rendered at 30 FPS
reported ~2× the true angular velocity.
Fix
Record the time of the previous update in the until-now unused
_time_lastfield and divide by that real elapsed time. The firstupdate, and any degenerate non-positive interval, falls back to
Time.deltaTime.Note the effective sample rate is still capped by the frame rate; this
change guarantees the values are correct at whatever spacing the
scheduler actually achieves.
Tests (PlayMode:
Tests/Runtime/ImuDerivativeTimingTests.cs)An IMU set to 4 Hz — so every sample necessarily spans several frames —
is attached to a GameObject rotating / translating at a known constant
rate, and the median of the reported magnitudes must land within ±30 %
of the truth (updates are detected via the public
sensor.time).AngularVelocity_MatchesConstantRotationRate(constant 30 deg/s)Velocity_MatchesConstantSpeed(constant 0.8 m/s)Verified with the Unity Test Runner (6000.3.21f1, batchmode, PlayMode):
2/2 pass with the fix, 2/2 fail with the old
dt = Time.deltaTime.Note
Independent of the #155 fix (full-turn angular velocity spike): both
touch the same method but on disjoint lines, and the branches merge
cleanly in either order.