class
InvariantEKFBaseEstimatorInvariantEKFBaseEstimator 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:/
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. |