Skip to content

Feature: Add sensor errors #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
prony5 merged 4 commits into from
Dec 13, 2024
Merged

Feature: Add sensor errors #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
aa7e487
Create gaussian noise utility class
Sh1co Dec 13, 2024
c74a828
Add float override to gaussian noise
Sh1co Dec 13, 2024
45725b8
Add noise to odom publisher
Sh1co Dec 13, 2024
7e017d2
Update Gaussian noise namespace
Sh1co Dec 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions Runtime/Tx/Msgs/Nav/OdometryTx.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,14 @@ public class OdometryTx : ProBridgeTxStamped<Odometry>
public Transform startPos;
public float[] covariancePose = new float[6] { 0.001f, 0.001f, 0.001f, 0.001f, 0.001f, 0.001f };
public float[] covarianceTwist = new float[6] { 0.001f, 0.001f, 0.001f, 0.001f, 0.001f, 0.001f };

[Header("Noise Parameters")]
public bool applyNoise = false;
public float linearVelocityNoiseStdDev = 0.0f;
public float angularVelocityNoiseStdDev = 0.0f;
public float positionNoiseStdDev = 0.0f;
public float rotationNoiseStdDev = 0.0f;

private Vector3 _startPose;
private Quaternion _startRotation;

Expand Down Expand Up @@ -65,6 +73,26 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts)
data.twist.twist.linear = (Quaternion.Inverse(transform.rotation) * Velocity).ToRos();
data.twist.twist.angular = (Quaternion.Inverse(transform.rotation) * AngularVelocity).ToRosAngular();

if (applyNoise)
{
data.pose.pose.position.x += GaussianNoise.Generate(positionNoiseStdDev);
data.pose.pose.position.y += GaussianNoise.Generate(positionNoiseStdDev);
data.pose.pose.position.z += GaussianNoise.Generate(positionNoiseStdDev);

data.pose.pose.orientation.x += GaussianNoise.Generate(rotationNoiseStdDev);
data.pose.pose.orientation.y += GaussianNoise.Generate(rotationNoiseStdDev);
data.pose.pose.orientation.z += GaussianNoise.Generate(rotationNoiseStdDev);
data.pose.pose.orientation.w += GaussianNoise.Generate(rotationNoiseStdDev);

data.twist.twist.linear.x += GaussianNoise.Generate(linearVelocityNoiseStdDev);
data.twist.twist.linear.y += GaussianNoise.Generate(linearVelocityNoiseStdDev);
data.twist.twist.linear.z += GaussianNoise.Generate(linearVelocityNoiseStdDev);

data.twist.twist.angular.x += GaussianNoise.Generate(angularVelocityNoiseStdDev);
data.twist.twist.angular.y += GaussianNoise.Generate(angularVelocityNoiseStdDev);
data.twist.twist.angular.z += GaussianNoise.Generate(angularVelocityNoiseStdDev);
}

data.child_frame_id = childFrameId;
data.pose.covariance[0] = covariancePose[0];
data.pose.covariance[7] = covariancePose[1];
Expand Down
33 changes: 33 additions & 0 deletions Runtime/Utils/GaussianNoise.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
using System;

namespace ProBridge.Utils
{
public static class GaussianNoise
{
/// <summary>
/// Generates Gaussian noise using the Box-Muller transform.
/// </summary>
/// <param name="sigma">The standard deviation of the Gaussian distribution.</param>
/// <returns>A double representing the Gaussian noise with the given standard deviation.</returns>
public static double Generate(double sigma = 1.0d)
{
double u1 = UnityEngine.Random.value;
double u2 = UnityEngine.Random.value;
double randStdNormal = Math.Sqrt(-2.0d * Math.Log(u1)) * Math.Sin(2.0d * Math.PI * u2);
return randStdNormal * sigma;
}

/// <summary>
/// Generates Gaussian noise using the Box-Muller transform.
/// </summary>
/// <param name="sigma">The standard deviation of the Gaussian distribution.</param>
/// <returns>A float representing the Gaussian noise with the given standard deviation.</returns>
public static float Generate(float sigma = 1.0f)
{
float u1 = UnityEngine.Random.value;
float u2 = UnityEngine.Random.value;
float randStdNormal = (float)(Math.Sqrt(-2.0f * Math.Log(u1)) * Math.Sin(2.0f * Math.PI * u2));
return randStdNormal * sigma;
}
}
}
3 changes: 3 additions & 0 deletions Runtime/Utils/GaussianNoise.cs.meta
View file
Open in desktop

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Loading