BipedalLocomotion::Estimators::InvariantEKFBaseEstimator class
#include <BipedalLocomotion/FloatingBaseEstimators/InvariantEKFBaseEstimator.h>

InvariantEKFBaseEstimator class implements the Contact Aided Invariant EKF algorithm, developed by Ross Hartley, Maani Ghaffari, Ryan M.

Eustice, Jessy W. Grizzle in the research article, "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation" arXiv:1904.09251 [cs.RO] Please cite this paper, if using this implementation as well. Link to the paper (https://arxiv.org/abs/1904.09251). For the original version of the filter implementation, please see https://github.com/RossHartley/invariant-ekf.

This implementation is a reduced version of the algorithm used to estimate only the bipedal feet poses along with the base pose and IMU biases.

Usage:

  • setup the estimator using the configuration paremeters related to
    • Options group
    • SensorsStdDev group
    • InitialStates group
    • PriorsStdDev group (For details on these parameter configuration groups, plese see the documentation of FloatingBaseEstimator class)
  • set the measurments
    • IMU measurements (accelerometer and gyroscope)
    • Kinematic measurements (joint positions and velocities)
    • Feet contact states (left foot contact state and right foot contact state)
  • advance the filter to run the computation step
  • get estimator outputs

Base classes

class FloatingBaseEstimator
FloatingBaseEstimator class contains the bare-bones implementation for a LeggedOdometry based or Extended Kalman Filter based floating base estimation algorithms for bipedal robots.

Constructors, destructors, conversion operators

InvariantEKFBaseEstimator()
Constructor.
~InvariantEKFBaseEstimator()
Destructor (necessary for PIMPL idiom)

Public functions

auto resetEstimator(const FloatingBaseEstimators::InternalState& newState, const FloatingBaseEstimators::StateStdDev& newPriorDev) -> bool
Reset the base pose estimate and consequently the internal state of the estimator.
auto resetEstimator(const FloatingBaseEstimators::InternalState& newState, const FloatingBaseEstimators::StateStdDev& newPriorDev, const FloatingBaseEstimators::SensorsStdDev& newSensorsDev) -> bool
Reset the base pose estimate and consequently the internal state of the estimator.
void toggleBiasEstimation(const bool& flag)
Toggle bias estimation.
void toggleEKFUpdate(const bool& flag)
Toggle EKF update.
auto resetEstimator(const FloatingBaseEstimators::InternalState& newState) -> bool
To prevent function hiding due to overloading of virtual methods.
auto resetEstimator(const Eigen::Quaterniond& newBaseOrientation, const Eigen::Vector3d& newBasePosition) -> bool
To prevent function hiding due to overloading of virtual methods.

Protected functions

auto customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool override
These custom parameter specifications should be specified by the derived class.
auto predictState(const FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool override
Propagate the states through the prediction model, if there exists any (eg.
auto updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool override
Update the predicted state estimates using kinematics and contact measurements.

Function documentation

bool BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::resetEstimator(const FloatingBaseEstimators::InternalState& newState, const FloatingBaseEstimators::StateStdDev& newPriorDev)

Reset the base pose estimate and consequently the internal state of the estimator.

Parameters
newState in internal state of the estimator
newPriorDev in internal state priors
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::resetEstimator(const FloatingBaseEstimators::InternalState& newState, const FloatingBaseEstimators::StateStdDev& newPriorDev, const FloatingBaseEstimators::SensorsStdDev& newSensorsDev)

Reset the base pose estimate and consequently the internal state of the estimator.

Parameters
newState in internal state of the estimator
newPriorDev in internal state priors
newSensorsDev in sensor measurement noise
Returns True in case of success, false otherwise.

void BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::toggleBiasEstimation(const bool& flag)

Toggle bias estimation.

Parameters
flag in flag for bias estimation

void BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::toggleEKFUpdate(const bool& flag)

Toggle EKF update.

Parameters
flag in flag for ekf update

bool BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) override protected

These custom parameter specifications should be specified by the derived class.

Parameters
handler in configure the custom parameters for the estimator
Returns bool

bool BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::predictState(const FloatingBaseEstimators::Measurements& meas, const double& dt) override protected

Propagate the states through the prediction model, if there exists any (eg.

Parameters
meas in measurements passed as exogeneous inputs to the prediction model
dt in sampling period in seconds
Returns True in case of success, false otherwise.

a strap-down IMU model)

bool BipedalLocomotion::Estimators::InvariantEKFBaseEstimator::updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) override protected

Update the predicted state estimates using kinematics and contact measurements.

Parameters
meas in measurements to update the predicted states
dt in sampling period in seconds
Returns True in case of success, false otherwise.