diff --git a/Runtime/Tx/ImuTx.cs b/Runtime/Tx/ImuTx.cs index 55a6b8c..81409b4 100644 --- a/Runtime/Tx/ImuTx.cs +++ b/Runtime/Tx/ImuTx.cs @@ -70,12 +70,16 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) data.linear_acceleration.y += GaussianNoise.Generate(linearAccelerationNoiseStdDev); data.linear_acceleration.z += GaussianNoise.Generate(linearAccelerationNoiseStdDev); - data.orientation.x += GaussianNoise.Generate(orientationNoiseStdDev); - data.orientation.y += GaussianNoise.Generate(orientationNoiseStdDev); - data.orientation.z += GaussianNoise.Generate(orientationNoiseStdDev); - data.orientation.w += GaussianNoise.Generate(orientationNoiseStdDev); - } - + var qNoise = Quaternion.Euler( + GaussianNoise.Generate(orientationNoiseStdDev), + GaussianNoise.Generate(orientationNoiseStdDev), + GaussianNoise.Generate(orientationNoiseStdDev) + ); + + var orientationWithNoise = data.orientation.FromRos() * qNoise; + + data.orientation = orientationWithNoise.ToRos(); + } data.angular_velocity = _angularVelocity.ToRos(); data.linear_acceleration = acceleration.ToRos(); diff --git a/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs b/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs deleted file mode 100644 index 2d40aca..0000000 --- a/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright [2020-2024] Ryodo Tanaka (groadpg@gmail.com) and Akiro Harada -// SPDX-License-Identifier: Apache-2.0 - -using System; - -namespace UnitySensors.Utils.Noise -{ - public class GaussianNoise - { - private Random _random; - - public GaussianNoise() - { - _random = new Random(Environment.TickCount); - } - - public GaussianNoise(int seed) - { - _random = new Random(seed); - } - - public void Init(int seed) - { - _random = new Random(seed); - } - - public double GetNoise(double sigma = 1.0d) - { - double rand = _random.NextDouble(); - double normrand = Math.Sqrt(-2.0d * Math.Log(0.0d)) * Math.Cos(2.0d * Math.PI * rand); - return normrand; - } - } -} \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs.meta b/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs.meta deleted file mode 100644 index 25282bf..0000000 --- a/Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: a6c48d7675ab83f4e82ce5241d4ccc74 -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: