diff --git a/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs b/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
index bd05a96a..dc0f29f7 100644
--- a/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
+++ b/Packages/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs
@@ -52,8 +52,20 @@ protected override void Init()
public override IEnumerator UpdateSensorOnce()
{
- //FIXME: IMU sensor should be updated at a fixed frequency
- float dt = Time.deltaTime;
+ // dt must be the REAL time since the previous sensor update: the
+ // position/rotation deltas below span the whole sensor period
+ // (several frames at low frame rates, and one or two frames
+ // depending on scheduling when the requested frequency is at or
+ // above the frame rate), so dividing them by one frame's
+ // Time.deltaTime overestimates every derivative by
+ // (actual sample spacing / frame time) -- measured ~2x angular
+ // velocity at 30 FPS with a 20 Hz IMU, and it is what makes the
+ // velocity and angular velocity read too large and unstable at
+ // high Frequency settings (Field-Robotics-Japan/UnitySensors#156).
+ float now = Time.time;
+ float dt = (_time_last > 0.0f) ? (now - _time_last) : Time.deltaTime;
+ if (dt <= 0.0f) dt = Time.deltaTime;
+ _time_last = now;
_position_tmp = _transform.position;
_velocity_tmp = (_position_tmp - _position_last) / dt;
diff --git a/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs b/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs
new file mode 100644
index 00000000..abedce04
--- /dev/null
+++ b/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs
@@ -0,0 +1,113 @@
+using System.Collections;
+using System.Collections.Generic;
+using NUnit.Framework;
+using UnityEngine;
+using UnityEngine.TestTools;
+using UnitySensors.Sensor.IMU;
+
+namespace UnitySensors.Tests.Runtime
+{
+ ///
+ /// Regression tests for Field-Robotics-Japan/UnitySensors#156: the IMU's
+ /// velocity and angular velocity must match the true motion even when the
+ /// sensor's samples are spaced differently from the frame time. Before
+ /// the fix the derivatives were divided by one frame's Time.deltaTime
+ /// while the pose deltas spanned the whole sensor period, so with the
+ /// 4 Hz sensor below running at test-runner frame rates the readings
+ /// came out several times too large.
+ ///
+ [TestFixture]
+ public class ImuDerivativeTimingTests
+ {
+ private const float kSensorHz = 4.0f;
+ private const float kRunSeconds = 2.5f;
+
+ private GameObject _go;
+
+ [TearDown]
+ public void TearDown()
+ {
+ if (_go != null)
+ Object.DestroyImmediate(_go);
+ }
+
+ private IMUSensor CreateSensor()
+ {
+ _go = new GameObject("imu_under_test");
+ var sensor = _go.AddComponent();
+ // Applied before Start() runs (next frame), so the scheduler
+ // paces the sensor well below the frame rate and every sample
+ // spans several frames -- the condition of issue #156.
+ sensor.frequency = kSensorHz;
+ return sensor;
+ }
+
+ private static float Median(List values)
+ {
+ values.Sort();
+ return values[values.Count / 2];
+ }
+
+ [UnityTest]
+ public IEnumerator AngularVelocity_MatchesConstantRotationRate()
+ {
+ const float rateDeg = 30.0f;
+ IMUSensor sensor = CreateSensor();
+ yield return null; // let Start()/Init() run
+
+ var samples = new List();
+ int updates = 0;
+ float lastUpdateTime = sensor.time;
+ float t0 = Time.time;
+ while (Time.time - t0 < kRunSeconds)
+ {
+ _go.transform.Rotate(Vector3.up, rateDeg * Time.deltaTime, Space.World);
+ yield return null;
+ if (sensor.time != lastUpdateTime)
+ {
+ lastUpdateTime = sensor.time;
+ updates++;
+ if (updates > 2) // the first samples still contain start-up state
+ samples.Add(sensor.angularVelocity.magnitude);
+ }
+ }
+
+ Assert.GreaterOrEqual(samples.Count, 3, "the sensor produced too few updates");
+ float expected = rateDeg * Mathf.Deg2Rad;
+ // Before the fix the reading scales with (sample spacing / frame
+ // time), which is far outside this band at any realistic frame
+ // rate; after it the median sits on the true rate.
+ Assert.That(Median(samples), Is.InRange(expected * 0.7f, expected * 1.3f),
+ "angular velocity must match the true constant rotation rate");
+ }
+
+ [UnityTest]
+ public IEnumerator Velocity_MatchesConstantSpeed()
+ {
+ const float speed = 0.8f; // [m/s]
+ IMUSensor sensor = CreateSensor();
+ yield return null; // let Start()/Init() run
+
+ var samples = new List();
+ int updates = 0;
+ float lastUpdateTime = sensor.time;
+ float t0 = Time.time;
+ while (Time.time - t0 < kRunSeconds)
+ {
+ _go.transform.position += Vector3.right * (speed * Time.deltaTime);
+ yield return null;
+ if (sensor.time != lastUpdateTime)
+ {
+ lastUpdateTime = sensor.time;
+ updates++;
+ if (updates > 2)
+ samples.Add(sensor.velocity.magnitude);
+ }
+ }
+
+ Assert.GreaterOrEqual(samples.Count, 3, "the sensor produced too few updates");
+ Assert.That(Median(samples), Is.InRange(speed * 0.7f, speed * 1.3f),
+ "velocity must match the true constant speed");
+ }
+ }
+}
diff --git a/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs.meta b/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs.meta
new file mode 100644
index 00000000..acfcab67
--- /dev/null
+++ b/Packages/UnitySensors/Tests/Runtime/ImuDerivativeTimingTests.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 165b559df2ed4d42b68b595bcbc7b20d
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant: