From b580c351acf9b03ad7c5bc9fcd6720a3273d2785 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Fri, 13 Dec 2024 15:52:02 +0300 Subject: [PATCH 1/3] Remove GaussianNoise class from UnitySensors --- .../Scripts/Utils/Noise/GaussianNoise.cs | 34 ------------------- .../Scripts/Utils/Noise/GaussianNoise.cs.meta | 11 ------ 2 files changed, 45 deletions(-) delete mode 100644 Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs delete mode 100644 Runtime/Utils/UnitySensors/Scripts/Utils/Noise/GaussianNoise.cs.meta 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: From f729518c282c915074ba893a98a05c81a94c04fd Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Fri, 13 Dec 2024 16:07:21 +0300 Subject: [PATCH 2/3] Fix error application on imu rot --- Runtime/Tx/ImuTx.cs | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/Runtime/Tx/ImuTx.cs b/Runtime/Tx/ImuTx.cs index 55a6b8c..d85de3a 100644 --- a/Runtime/Tx/ImuTx.cs +++ b/Runtime/Tx/ImuTx.cs @@ -70,10 +70,14 @@ 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 euler = data.orientation.FromRos().eulerAngles; + + euler.x += GaussianNoise.Generate(orientationNoiseStdDev); + euler.y += GaussianNoise.Generate(orientationNoiseStdDev); + euler.z += GaussianNoise.Generate(orientationNoiseStdDev); + + data.orientation = Quaternion.Euler(euler).ToRos(); } From 77f1a55bc118d475c0bd87543548eed7ccf668a3 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Mon, 13 Jan 2025 12:06:06 +0300 Subject: [PATCH 3/3] Update error application on IMU rot --- Runtime/Tx/ImuTx.cs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/Runtime/Tx/ImuTx.cs b/Runtime/Tx/ImuTx.cs index d85de3a..81409b4 100644 --- a/Runtime/Tx/ImuTx.cs +++ b/Runtime/Tx/ImuTx.cs @@ -70,16 +70,16 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) data.linear_acceleration.y += GaussianNoise.Generate(linearAccelerationNoiseStdDev); data.linear_acceleration.z += GaussianNoise.Generate(linearAccelerationNoiseStdDev); - - var euler = data.orientation.FromRos().eulerAngles; - - euler.x += GaussianNoise.Generate(orientationNoiseStdDev); - euler.y += GaussianNoise.Generate(orientationNoiseStdDev); - euler.z += GaussianNoise.Generate(orientationNoiseStdDev); + var qNoise = Quaternion.Euler( + GaussianNoise.Generate(orientationNoiseStdDev), + GaussianNoise.Generate(orientationNoiseStdDev), + GaussianNoise.Generate(orientationNoiseStdDev) + ); - data.orientation = Quaternion.Euler(euler).ToRos(); - } - + var orientationWithNoise = data.orientation.FromRos() * qNoise; + + data.orientation = orientationWithNoise.ToRos(); + } data.angular_velocity = _angularVelocity.ToRos(); data.linear_acceleration = acceleration.ToRos();