iDynTree::IAttitudeEstimator class
#include <iDynTree/AttitudeEstimator.h>

for attitude estimator classes

The aim is to implement different attitude estimators as a block that takes IMU measurements as inputs and gives attitude estimates as outputs. This way the underlying implementation is abstracted and the user only has to set a few parameters and run the estimator.

The general procedure to use the estimators would be,

  • instantiate the filter,
  • set initial internal state
  • in a loop,
    • update the filter with measurements
    • propagate the states

However, additional methods to set and get parameters for the filter might be available with respect to the filters.

The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.

Derived classes

class AttitudeMahonyFilter
explicit passive complementary filter on quaternion groups described in the paper Non-linear complementary filters on SO3 groups
class AttitudeQuaternionEKF
Quaternion based Discrete Extended Kalman Filter fusing IMU measurements, to give estimates of orientation, angular velocity and gyroscope bias.

Constructors, destructors, conversion operators

~IAttitudeEstimator() virtual

Public functions

auto updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas) -> bool pure virtual
Update the filter with accelerometer and gyroscope measurements.
auto updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas) -> bool pure virtual
Update the filter with accelerometer, gyroscope and magnetometer measurements.
auto propagateStates() -> bool pure virtual
Propagate the states and associated uncertainties through properly defined propagation functions The underlying implementation depends on the type of filter being implemented.
auto getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) -> bool pure virtual
Get orientation of the body with respect to inertial frame, in rotation matrix form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us $ {^A}R_B $ as the rotation matrix.
auto getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) -> bool pure virtual
Get orientation of the body with respect to inertial frame, in unit quaternion form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us $ {^A}q_B as the quaternion $ .
auto getOrientationEstimateAsRPY(iDynTree::RPY& rpy) -> bool pure virtual
Get orientation of the body with respect to inertial frame, in Euler's RPY form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us the RPY 3d vector of Euler Angles when composed together gives us $ {^A}R_B $ as the rotation matrix where $ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll)$ .
auto getInternalStateSize() const -> size_t pure virtual
Get dimension of the state vector.
auto getInternalState(const iDynTree::Span<double>& stateBuffer) const -> bool pure virtual
Get internal state of the estimator The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.
auto getDefaultInternalInitialState(const iDynTree::Span<double>& stateBuffer) const -> bool pure virtual
Get initial internal state of the estimator The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.
auto setInternalState(const iDynTree::Span<double>& stateBuffer) -> bool pure virtual
set internal state of the estimator.
auto setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) -> bool pure virtual
set the initial orientation for the internal state of the estimator.

Function documentation

bool iDynTree::IAttitudeEstimator::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas) pure virtual

Update the filter with accelerometer and gyroscope measurements.

Parameters
linAccMeas in proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B
gyroMeas in angular velocity of body frame B with respect to an inertial fram A, expressed in frame B
Returns true/false if successful/not

bool iDynTree::IAttitudeEstimator::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas) pure virtual

Update the filter with accelerometer, gyroscope and magnetometer measurements.

Parameters
linAccMeas in proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B
gyroMeas in angular velocity of body frame B with respect to an inertial fram A, expressed in frame B
magMeas in magnetometer measurements expressed in frame B
Returns true/false if successful/not

bool iDynTree::IAttitudeEstimator::propagateStates() pure virtual

Propagate the states and associated uncertainties through properly defined propagation functions The underlying implementation depends on the type of filter being implemented.

Returns true/false if successful/not

bool iDynTree::IAttitudeEstimator::getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) pure virtual

Get orientation of the body with respect to inertial frame, in rotation matrix form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us $ {^A}R_B $ as the rotation matrix.

Parameters
rot out Rotation matrix
Returns true/false if successful/not

bool iDynTree::IAttitudeEstimator::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) pure virtual

Get orientation of the body with respect to inertial frame, in unit quaternion form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us $ {^A}q_B as the quaternion $ .

Parameters
q out UnitQuaternion
Returns true/false if successful/not

bool iDynTree::IAttitudeEstimator::getOrientationEstimateAsRPY(iDynTree::RPY& rpy) pure virtual

Get orientation of the body with respect to inertial frame, in Euler's RPY form If we denote $ A $ as inertial frame and $ B $ as the frame attached to the body, then this method gives us the RPY 3d vector of Euler Angles when composed together gives us $ {^A}R_B $ as the rotation matrix where $ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll)$ .

Parameters
rpy out 3D vector containing roll pitch yaw angles
Returns true/false if successful/not

For more details about the range of the RPY Euler angles, please refer the documentation of GetRPY()

size_t iDynTree::IAttitudeEstimator::getInternalStateSize() const pure virtual

Get dimension of the state vector.

Returns size_t size of state vector

bool iDynTree::IAttitudeEstimator::getInternalState(const iDynTree::Span<double>& stateBuffer) const pure virtual

Get internal state of the estimator The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.

Parameters
stateBuffer out Span object as reference of the container where state vector should be copied to
Returns true/false if successful/not

The default internal state of the estimator would be $ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T $

bool iDynTree::IAttitudeEstimator::getDefaultInternalInitialState(const iDynTree::Span<double>& stateBuffer) const pure virtual

Get initial internal state of the estimator The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.

Parameters
stateBuffer out Span object as reference of the container where state vector should be copied to
Returns true/false if successful/not

The default internal state of the estimator would be $ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T $

bool iDynTree::IAttitudeEstimator::setInternalState(const iDynTree::Span<double>& stateBuffer) pure virtual

set internal state of the estimator.

Parameters
stateBuffer in Span object as reference of the container from which the internal state vector should be assigned. The size of the buffer should be 10.
Returns true/false if successful/not

The internal state of the estimator is described as $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , $ {^B}\Omega_{A,B} \in \mathbb{R}^3 $ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and $ {^B}b \in \mathbb{R}^3 $ is the gyroscope bias expressed in the body frame.

bool iDynTree::IAttitudeEstimator::setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) pure virtual

set the initial orientation for the internal state of the estimator.

Returns true/false if successful/not

The initial orientation for the internal state of the estimator is described as $ {^A}q_B $ $ {^A}q_B \in \mathbb{R}^4 $ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,