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

Quaternion based Discrete Extended Kalman Filter fusing IMU measurements, to give estimates of orientation, angular velocity and gyroscope bias.

It follows the implementation detailed in Quaternion Based Extended Kalman Filter, slides by Michael Stohmeier 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.

Discretized dynamics during the prediction step,

\[ \hat{{x}}_{k+1} = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \]

Measurement model for accelerometer is given as,

\[ h_{acc}(\hat{x}_{k+1}) = \begin{bmatrix} 2(q_1q_3 - q_0q_2) \\ 2(q_2q_3 - q_0q_1) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix} \]

obtained from $ {^w}R_b^T e_3 $ of the assumed gravity direction.

Measurement model for magnetometer measurement is given as,

\[ h_{mag}(\hat{x}_{k+1}) = atan2( 2(q_0q_3 + q_1q_2),1 - 2(q_2^2 + q_3^2) ) \]

The linearized system propogation and measurement model is obtained by computing Jacobins F and H with respect to the state.

The zero mean, additive Gaussian noise can be set using the covariance matrices which will be used during predict and update steps.

The propagateStates() method is called to set the input vector for the EKF, then ekfPredict() is called to propagate the state through the propagation function f() and propate the state covariance using the Jacobian F.

The updateFilterWithMeasurements() is called to set the measurement vector for the EKF, and then ekfUpdate is used to correct the state estimate and its covariance using the measurement model function h() and the measurement Jacobian H.

The usage of the QEKF should follow the decribed procedure below,

  • instantiate the filter
  • set parameters
  • call initializeFilter() (this is necessary for resizing the buffers, the user should call this method after setting parameters)
  • use setInternalState() to set initial state (The filter will throw an error, if this is not called atleast once, this enforces the user to set intial state)
  • Once initialized, the following filter methods can be run in a loop to get the orientation estimates,

Base classes

class IAttitudeEstimator
for attitude estimator classes
class DiscreteExtendedKalmanFilterHelper
class implementation of discrete EKF with additive Gaussian noise

Constructors, destructors, conversion operators

AttitudeQuaternionEKF()

Public functions

void getParameters(AttitudeQuaternionEKFParameters& params)
Get filter parameters as a struct.
void setParameters(const AttitudeQuaternionEKFParameters& params)
Set filter parameters with the struct members.
void setGravityDirection(const iDynTree::Direction& gravity_dir)
Set the gravity direction assumed by the filter This affects the measurement model function h() and Jacobian H.
void setTimeStepInSeconds(double time_step_in_seconds)
set discretization time step in seconds
void setBiasCorrelationTimeFactor(double bias_correlation_time_factor)
set bias correlation time factor
auto useMagnetometerMeasurements(bool use_magnetometer_measurements) -> bool
set flag to use magnetometer measurements
auto setMeasurementNoiseVariance(double acc, double mag) -> bool
prepares the measurement noise covariance matrix and calls ekfSetMeasurementNoiseMeanAndCovariance() measurement noise depends only on accelerometer xyz (and magnetometer z)
auto setSystemNoiseVariance(double gyro, double gyro_bias) -> bool
prepares the system noise covariance matrix and calls ekfSetSystemNoiseMeanAndCovariance() process noise depends on gyro measurement and gyro bias estimate - since gyro measurement is passed as input
auto setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var) -> bool
prepares the state covariance matrix and calls ekfSetStateCovariance()
auto initializeFilter() -> bool
intializes the filter by resizing buffers and setting parameters
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

AttitudeEstimatorState m_state_qekf
AttitudeEstimatorState m_initial_state_qekf
AttitudeQuaternionEKFParameters m_params_qekf
struct holding the QEKF parameters

Private functions

auto ekf_f(const iDynTree::VectorDynSize& x_k, const iDynTree::VectorDynSize& u_k, iDynTree::VectorDynSize& xhat_k_plus_one) -> bool override
discrete system propagation $ f(X, u) = f(X, y_gyro) $ where $ X = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & b_x & b_y & b_z \end{bmatrix}^T $ $ u = \begin{bmatrix} {y_{gyro}}_x & {y_{gyro}}_y & {y_{gyro}}_z \end{bmatrix}^T $ $ f(X, u) = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}$
auto ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, iDynTree::VectorDynSize& zhat_k_plus_one) -> bool override
discrete measurement prediction where $ h(X) = \begin{bmatrix} h_{acc}(X) & h_{mag}(X) \end{bmatrix}^T $ $ h_{acc}(X) = R^T \begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix} $ $ h_{mag}(X) = atan2(tan(yaw))$
auto ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) -> bool override
Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state.
auto ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) -> bool override
Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state and the system propagation with respect to the input.
auto ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) -> bool override
Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance The analytical Jacobian describing the partial derivative of the measurement model with respect to the state.

Function documentation

void iDynTree::AttitudeQuaternionEKF::getParameters(AttitudeQuaternionEKFParameters& params)

Get filter parameters as a struct.

Parameters
params out object of AttitudeQuaternionEKFParameters passed as reference

void iDynTree::AttitudeQuaternionEKF::setParameters(const AttitudeQuaternionEKFParameters& params)

Set filter parameters with the struct members.

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

This resets filter since it also calls useMagnetometerMeasurements(flag) (if the use_magnetometer_measurements flag has been toggled).

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

Set the gravity direction assumed by the filter This affects the measurement model function h() and Jacobian H.

Parameters
gravity_dir in gravity direction

void iDynTree::AttitudeQuaternionEKF::setTimeStepInSeconds(double time_step_in_seconds)

set discretization time step in seconds

Parameters
time_step_in_seconds in time step

void iDynTree::AttitudeQuaternionEKF::setBiasCorrelationTimeFactor(double bias_correlation_time_factor)

set bias correlation time factor

Parameters
bias_correlation_time_factor in time factor for bias evolution

bool iDynTree::AttitudeQuaternionEKF::useMagnetometerMeasurements(bool use_magnetometer_measurements)

set flag to use magnetometer measurements

Parameters
use_magnetometer_measurements in enable/disable magnetometer measurements

bool iDynTree::AttitudeQuaternionEKF::setMeasurementNoiseVariance(double acc, double mag)

prepares the measurement noise covariance matrix and calls ekfSetMeasurementNoiseMeanAndCovariance() measurement noise depends only on accelerometer xyz (and magnetometer z)

Parameters
acc in variance for accelerometer measurements
mag in variance for magnetometer measurements
Returns true/false if successful/not

bool iDynTree::AttitudeQuaternionEKF::setSystemNoiseVariance(double gyro, double gyro_bias)

prepares the system noise covariance matrix and calls ekfSetSystemNoiseMeanAndCovariance() process noise depends on gyro measurement and gyro bias estimate - since gyro measurement is passed as input

Parameters
gyro in variance for gyroscope measurements
gyro_bias in variance for gyroscope bias estimates
Returns true/false if successful/not

bool iDynTree::AttitudeQuaternionEKF::setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var)

prepares the state covariance matrix and calls ekfSetStateCovariance()

Parameters
orientation_var in variance for intial orientation state estimate
ang_vel_var in variance for initial angular velocity state estimate
gyro_bias_var in variance for intial gyro bias state estimate
Returns true/false if successful/not

bool iDynTree::AttitudeQuaternionEKF::initializeFilter()

intializes the filter by resizing buffers and setting parameters

Returns true/false if successful/not
  • sets state, output and input dimensions for the ekf
  • resizes internal buffers
  • calls ekfInit()
  • sets system noise, measurement noise and initial state covariance
  • if successful sets initialized flag to true

bool iDynTree::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::getInternalStateSize() const override

Get dimension of the state vector.

Returns size_t size of state vector

bool iDynTree::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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::AttitudeQuaternionEKF::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 ,

bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override private

Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state.

Parameters
x in system state
F out system Jacobian
Returns bool true/false if successful or not

bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) override private

Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state and the system propagation with respect to the input.

Parameters
x in system state
u in system input
F out system Jacobian
Returns bool true/false if successful or not

bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) override private

Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance The analytical Jacobian describing the partial derivative of the measurement model with respect to the state.

Parameters
x in system state
H out measurement Jacobian
Returns bool true/false if successful or not