iDynTree::AttitudeMahonyFilter class
#include <iDynTree/AttitudeMahonyFilter.h>

explicit passive complementary filter on quaternion groups described in the paper Non-linear complementary filters on SO3 groups

The filter is used to estimate the states $ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T $ where $ {^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.

The discretized dynamics of the filter is implemented in the propagateStates() method and is described by the following equations, $ q_{k+1} = q_{k} + \Delta t \frac{1}{2}q_{k} \circ \begin{bmatrix} 0 \\ {\Omega_y}_{k+1} - b_k + K_p \omega_{mes_{k+1}}\end{bmatrix}$ $ \Omega_{k+1} = {\Omega_y}_{k+1} - b_k $ $ b_{k+1} = b_k - K_i \Delta t \frac{1}{2} \omega_{mes_{k+1}} $

The updateFilterWithMeasurements() uses the recent IMU measurements to compute the term $ \omega_{mes} $ which gives the vectorial from accelerometer and magnetometer measurements $ \omega_{mes} = -(\Sigma{n}{i=1} \frac{k_i}{2} (v_i \hat{v}_i^T - \hat{v}_i v_i^T) )^{\vee} $ where $ v_i $ is the normalized accelerometer or magnetometer measurement, $ \hat{v_i} $ is the vector obtained from the orientation estimated combined with gravity direction or absolute magnetic field direction, for e.g, $ \hat{v_acc} = {^w}R_b^T e_3 $ and $ k_i $ is the confidence weight on the i-th measurement. In our case, i = 1 or 2.

The usage of the attitude estimator can be as follows,

  • After instantiation, the parameters of the filter can be set using the individual parameter methods or the struct method.
  • The filter state can be initialized by calling the setInternalState() method
  • Once initialized, the following filter methods can be run in a loop to get the orientation estimates,
    • updateFilterWithMeasurements() method to pass the recent measurements to the filter
    • propagateStates() method to propagate the states through the system dynamics and correcting using the updated measurements
    • getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation

Base classes

class IAttitudeEstimator
for attitude estimator classes

Constructors, destructors, conversion operators

AttitudeMahonyFilter()

Public functions

void useMagnetoMeterMeasurements(bool flag)
set flag to use magnetometer measurements
void setConfidenceForMagnetometerMeasurements(double confidence)
set the confidence weights on magenetometer measurements, if used
void setGainkp(double kp)
set the Kp gain
void setGainki(double ki)
set the Ki gain
void setTimeStepInSeconds(double timestepInSeconds)
set discretization time step in seconds
void setGravityDirection(const iDynTree::Direction& gravity_dir)
Set the gravity direction assumed by the filter (for computing orientation vectorial from accelerometer)
auto setParameters(const AttitudeMahonyFilterParameters& params) -> bool
Set filter parameters with the struct members.
void getParameters(AttitudeMahonyFilterParameters& params)
Get filter parameters as a struct.
auto updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas) -> bool override
Update the filter with accelerometer and gyroscope measurements.
auto updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas) -> bool override
Update the filter with accelerometer, gyroscope and magnetometer measurements.
auto propagateStates() -> bool override
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 override
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 override
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 override
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 override
Get dimension of the state vector.
auto getInternalState(const iDynTree::Span<double>& stateBuffer) const -> bool override
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 override
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 override
set internal state of the estimator.
auto setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) -> bool override
set the initial orientation for the internal state of the estimator.

Protected variables

AttitudeMahonyFilterParameters m_params_mahony
struct holding the Mahony filter parameters
AttitudeEstimatorState m_state_mahony
AttitudeEstimatorState m_initial_state_mahony

Function documentation

void iDynTree::AttitudeMahonyFilter::useMagnetoMeterMeasurements(bool flag)

set flag to use magnetometer measurements

Parameters
flag in enable/disable magnetometer measurements

void iDynTree::AttitudeMahonyFilter::setConfidenceForMagnetometerMeasurements(double confidence)

set the confidence weights on magenetometer measurements, if used

Parameters
confidence in can take values between $ [0, 1] $

void iDynTree::AttitudeMahonyFilter::setGainkp(double kp)

set the Kp gain

Parameters
kp in gain

void iDynTree::AttitudeMahonyFilter::setGainki(double ki)

set the Ki gain

Parameters
ki in gain

void iDynTree::AttitudeMahonyFilter::setTimeStepInSeconds(double timestepInSeconds)

set discretization time step in seconds

Parameters
timestepInSeconds in time step

void iDynTree::AttitudeMahonyFilter::setGravityDirection(const iDynTree::Direction& gravity_dir)

Set the gravity direction assumed by the filter (for computing orientation vectorial from accelerometer)

Parameters
gravity_dir in gravity direction

bool iDynTree::AttitudeMahonyFilter::setParameters(const AttitudeMahonyFilterParameters& params)

Set filter parameters with the struct members.

Parameters
params in object of AttitudeMahonyFilterParameters passed as a const reference
Returns true/false if successful/not

This does not reset the internal state.

void iDynTree::AttitudeMahonyFilter::getParameters(AttitudeMahonyFilterParameters& params)

Get filter parameters as a struct.

Parameters
params out object of AttitudeMahonyFilterParameters passed as reference

bool iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas) override

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::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas) override

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::AttitudeMahonyFilter::propagateStates() override

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::AttitudeMahonyFilter::getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override

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::AttitudeMahonyFilter::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override

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::AttitudeMahonyFilter::getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override

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::AttitudeMahonyFilter::getInternalStateSize() const override

Get dimension of the state vector.

Returns size_t size of state vector

bool iDynTree::AttitudeMahonyFilter::getInternalState(const iDynTree::Span<double>& stateBuffer) const override

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::AttitudeMahonyFilter::getDefaultInternalInitialState(const iDynTree::Span<double>& stateBuffer) const override

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::AttitudeMahonyFilter::setInternalState(const iDynTree::Span<double>& stateBuffer) override

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::AttitudeMahonyFilter::setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) override

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 ,