Skip to content

Support for IMU orientation noise #1650

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

Open
faseelmo wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Support for IMU orientation noise #1650

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
dcb3c50
adding support for loading orientation noise from sdf
faseelmo Apr 17, 2026
80a1f34
adding setters and getters for imu orientation
faseelmo Apr 17, 2026
ce73023
adding orientation noise in tests
faseelmo Apr 17, 2026
1b9f89c
passing all the tests now, had to update the schema
faseelmo Apr 17, 2026
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
30 changes: 30 additions & 0 deletions include/sdf/Imu.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,36 @@ namespace sdf
/// \param[in] _noise Noise values for the Z-axis angular velocity.
public: void SetAngularVelocityZNoise(const Noise &_noise);

/// \brief Get the noise values related to the body-frame orientation
/// on the X-axis (Roll).
/// \return Noise values for the X-axis orientation.
public: const Noise &OrientationXNoise() const;

/// \brief Set the noise values related to the body-frame orientation
/// on the X-axis (Roll).
/// \param[in] _noise Noise values for the X-axis orientation.
public: void SetOrientationXNoise(const Noise &_noise) ;

/// \brief Get the noise values related to the body-frame orientation
/// on the Y-axis (Pitch).
/// \return Noise values for the Y-axis orientation.
public: const Noise &OrientationYNoise() const;

/// \brief Set the noise values related to the body-frame orientation
/// on the Y-axis (Pitch).
/// \param[in] _noise Noise values for the Y-axis orientation.
public: void SetOrientationYNoise(const Noise &_noise) ;

/// \brief Get the noise values related to the body-frame orientation
/// on the Z-axis (Yaw).
/// \return Noise values for the Z-axis orientation.
public: const Noise &OrientationZNoise() const;

/// \brief Set the noise values related to the body-frame orientation
/// on the Z-axis (Yaw).
/// \param[in] _noise Noise values for the Z-axis orientation.
public: void SetOrientationZNoise(const Noise &_noise) ;

/// \brief Used when localization is set to GRAV_UP or GRAV_DOWN, a
/// projection of this vector into a plane that is orthogonal to the
/// gravity vector defines the direction of the IMU reference frame's
Expand Down
18 changes: 18 additions & 0 deletions sdf/1.12/imu.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,4 +117,22 @@
<element name="enable_orientation" type="bool" default="true" required="0">
<description>Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation.</description>
</element>

<element name="orientation" required="0">
<description>These elements are specific to body-frame orientation,
which is expressed in radians</description>
<element name="x" required="0">
<description>Orientation about X-axis (Roll)</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="y" required="0">
<description>Orientation about Y-axis (Pitch)</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="z" required="0">
<description>Orientation about Z-axis (Yaw)</description>
<include filename="noise.sdf" required="0"/>
</element>
</element>

</element>
103 changes: 103 additions & 0 deletions src/Imu.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,18 @@ class sdf::Imu::Implementation
/// Z-axis.
public: Noise angularVelZNoise;

/// \brief Noise values related to the body-frame orientation on the
/// X-axis (Roll).
public: Noise orientationXNoise;

/// \brief Noise values related to the body-frame orientation on the
/// Y-axis (Pitch).
public: Noise orientationYNoise;

/// \brief Noise values related to the body-frame orientation on the
/// Z-axis (Yaw).
public: Noise orientationZNoise;

/// \brief The gravity dir
public: gz::math::Vector3d gravityDirX{gz::math::Vector3d::UnitX};

Expand Down Expand Up @@ -195,6 +207,38 @@ Errors Imu::Load(ElementPtr _sdf)
errors, "enable_orientation", this->dataPtr->orientationEnabled).first;
}

if (_sdf->HasElement("orientation"))
{
sdf::ElementPtr elem = _sdf->GetElement("orientation", errors);
if (elem->HasElement("x"))
{
if (elem->GetElement("x", errors)->HasElement("noise"))
{
sdf::Errors loadErrors = this->dataPtr->orientationXNoise.Load(
elem->GetElement("x", errors)->GetElement("noise", errors));
errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
}
}
if (elem->HasElement("y"))
{
if (elem->GetElement("y", errors)->HasElement("noise"))
{
sdf::Errors loadErrors = this->dataPtr->orientationYNoise.Load(
elem->GetElement("y", errors)->GetElement("noise", errors));
errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
}
}
if (elem->HasElement("z"))
{
if (elem->GetElement("z", errors)->HasElement("noise"))
{
sdf::Errors loadErrors = this->dataPtr->orientationZNoise.Load(
elem->GetElement("z", errors)->GetElement("noise", errors));
errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
}
}
}

return errors;
}

Expand All @@ -219,6 +263,9 @@ bool Imu::operator==(const Imu &_imu) const
this->dataPtr->angularVelXNoise == _imu.dataPtr->angularVelXNoise &&
this->dataPtr->angularVelYNoise == _imu.dataPtr->angularVelYNoise &&
this->dataPtr->angularVelZNoise == _imu.dataPtr->angularVelZNoise &&
this->dataPtr->orientationXNoise == _imu.dataPtr->orientationXNoise &&
this->dataPtr->orientationYNoise == _imu.dataPtr->orientationYNoise &&
this->dataPtr->orientationZNoise == _imu.dataPtr->orientationZNoise &&
this->dataPtr->localization == _imu.dataPtr->localization &&

this->dataPtr->gravityDirX == _imu.dataPtr->gravityDirX &&
Expand Down Expand Up @@ -302,6 +349,42 @@ void Imu::SetAngularVelocityZNoise(const Noise &_noise)
this->dataPtr->angularVelZNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &Imu::OrientationXNoise() const
{
return this->dataPtr->orientationXNoise;
}

//////////////////////////////////////////////////
void Imu::SetOrientationXNoise(const Noise &_noise)
{
this->dataPtr->orientationXNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &Imu::OrientationYNoise() const
{
return this->dataPtr->orientationYNoise;
}

//////////////////////////////////////////////////
void Imu::SetOrientationYNoise(const Noise &_noise)
{
this->dataPtr->orientationYNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &Imu::OrientationZNoise() const
{
return this->dataPtr->orientationZNoise;
}

//////////////////////////////////////////////////
void Imu::SetOrientationZNoise(const Noise &_noise)
{
this->dataPtr->orientationZNoise = _noise;
}

//////////////////////////////////////////////////
const gz::math::Vector3d &Imu::GravityDirX() const
{
Expand Down Expand Up @@ -447,6 +530,26 @@ sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const
linearAccZNoiseElem->Copy(this->dataPtr->linearAccelZNoise.ToElement(
_errors), _errors);

sdf::ElementPtr orientationElem = elem->GetElement(
"orientation", _errors);
sdf::ElementPtr orientationXElem = orientationElem->GetElement("x", _errors);
sdf::ElementPtr orientationXNoiseElem = orientationXElem->GetElement(
"noise", _errors);
orientationXNoiseElem->Copy(this->dataPtr->orientationXNoise.ToElement(
_errors), _errors);

sdf::ElementPtr orientationYElem = orientationElem->GetElement("y", _errors);
sdf::ElementPtr orientationYNoiseElem = orientationYElem->GetElement(
"noise", _errors);
orientationYNoiseElem->Copy(this->dataPtr->orientationYNoise.ToElement(
_errors), _errors);

sdf::ElementPtr orientationZElem = orientationElem->GetElement("z", _errors);
sdf::ElementPtr orientationZNoiseElem = orientationZElem->GetElement(
"noise", _errors);
orientationZNoiseElem->Copy(this->dataPtr->orientationZNoise.ToElement(
_errors), _errors);

elem->GetElement("enable_orientation", _errors)->Set<bool>(
_errors, this->OrientationEnabled());

Expand Down
24 changes: 24 additions & 0 deletions src/Imu_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,18 @@ TEST(DOMImu, Construction)
imu.SetAngularVelocityZNoise(noise);
EXPECT_EQ(noise, imu.AngularVelocityZNoise());

EXPECT_EQ(defaultNoise, imu.OrientationXNoise());
imu.SetOrientationXNoise(noise);
EXPECT_EQ(noise, imu.OrientationXNoise());

EXPECT_EQ(defaultNoise, imu.OrientationYNoise());
imu.SetOrientationYNoise(noise);
EXPECT_EQ(noise, imu.OrientationYNoise());

EXPECT_EQ(defaultNoise, imu.OrientationZNoise());
imu.SetOrientationZNoise(noise);
EXPECT_EQ(noise, imu.OrientationZNoise());

EXPECT_EQ(gz::math::Vector3d::UnitX, imu.GravityDirX());
imu.SetGravityDirX(gz::math::Vector3d::Zero);
EXPECT_EQ(gz::math::Vector3d::Zero, imu.GravityDirX());
Expand Down Expand Up @@ -146,6 +158,9 @@ TEST(DOMImu, ToElement)
imu.SetAngularVelocityXNoise(noise);
imu.SetAngularVelocityYNoise(noise);
imu.SetAngularVelocityZNoise(noise);
imu.SetOrientationXNoise(noise);
imu.SetOrientationYNoise(noise);
imu.SetOrientationZNoise(noise);
imu.SetGravityDirX(gz::math::Vector3d::Zero);
imu.SetGravityDirXParentFrame("my_frame");
imu.SetCustomRpy(gz::math::Vector3d::UnitZ);
Expand All @@ -167,6 +182,9 @@ TEST(DOMImu, ToElement)
EXPECT_EQ(noise, imu2.AngularVelocityXNoise());
EXPECT_EQ(noise, imu2.AngularVelocityYNoise());
EXPECT_EQ(noise, imu2.AngularVelocityZNoise());
EXPECT_EQ(noise, imu2.OrientationXNoise());
EXPECT_EQ(noise, imu2.OrientationYNoise());
EXPECT_EQ(noise, imu2.OrientationZNoise());
EXPECT_EQ(gz::math::Vector3d::Zero, imu2.GravityDirX());
EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame());
EXPECT_EQ(gz::math::Vector3d::UnitZ, imu2.CustomRpy());
Expand Down Expand Up @@ -215,6 +233,9 @@ TEST(DOMImu, ToElementErrorOutput)
imu.SetAngularVelocityXNoise(noise);
imu.SetAngularVelocityYNoise(noise);
imu.SetAngularVelocityZNoise(noise);
imu.SetOrientationXNoise(noise);
imu.SetOrientationYNoise(noise);
imu.SetOrientationZNoise(noise);
imu.SetGravityDirX(gz::math::Vector3d::Zero);
imu.SetGravityDirXParentFrame("my_frame");
imu.SetCustomRpy(gz::math::Vector3d::UnitZ);
Expand All @@ -238,6 +259,9 @@ TEST(DOMImu, ToElementErrorOutput)
EXPECT_EQ(noise, imu2.AngularVelocityXNoise());
EXPECT_EQ(noise, imu2.AngularVelocityYNoise());
EXPECT_EQ(noise, imu2.AngularVelocityZNoise());
EXPECT_EQ(noise, imu2.OrientationXNoise());
EXPECT_EQ(noise, imu2.OrientationYNoise());
EXPECT_EQ(noise, imu2.OrientationZNoise());
EXPECT_EQ(gz::math::Vector3d::Zero, imu2.GravityDirX());
EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame());
EXPECT_EQ(gz::math::Vector3d::UnitZ, imu2.CustomRpy());
Expand Down
Loading