Skip to content

Latest commit

 

History

History
49 lines (36 loc) · 5.77 KB

README.md

File metadata and controls

49 lines (36 loc) · 5.77 KB

Software

This repository includes sample MATLAB implementation of different filters for humanoid, floating base estimation based on an Extended Kalman Filter (EKF) based framework using proprioceptive measurements.

Method Implementation Please cite Remark
Observability Constrained quaternion based EKF (OCEKF) Estimation.RotellaEstimator.Filter N. Rotella, M. Bloesch, L. Righetti and S. Schaal, "State estimation for a humanoid robot," 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, pp. 952-958, doi: 10.1109/IROS.2014.6942674.
Right Invariant EKF (InvEKF) Estimation.InvEKF.Filter Hartley, R., Ghaffari, M., Eustice, R. M., & Grizzle, J. W. (2020). Contact-aided invariant extended Kalman filtering for robot state estimation. The International Journal of Robotics Research, 39(4), 402–430. https://doi.org/10.1177/0278364919894385 Re-implements the filter from https://github.com/UMich-BipedLab/Contact-Aided-Invariant-EKF within the scope of our notations and formats.
Simple Weighted Averaging (SWA) Estimation.SimpleBipedEstimator.Filter See citing-this-work.
DILIGENT-KIO Estimation.DLGEKF.Filter See citing-this-work.

Code Dependencies

The software has been tested with MATLAB R2020B. The required dependencies are,

How to run the filter

Calling main in a MATLAB console runs all the filters over a dataset collected from a walking experiment on iCub humanoid robot and plots the base pose and linear velocity estimates, along with the metric error computations.

A few high-level parameters include,

Parameter Description Type Options
experiment_name name of the experiment's mat file string walking or com-sinusoid
enable_ocekf enable OCEKF computations boolean true or false
enable_invekf enable InvEKF computations boolean true or false
enable_swa enable SWA computations boolean true or false
enable_diligent enable DILIGENT-KIO computations boolean true or false

The filter configuration and noise parameters can be modified from within the +Config namespace.

Configuration file Description
+Config/configMatlabPriors Set prior standard deviations for the EKF based estimator's internal state (OCEKF, InvEKF, DILIGENT-KIO).
+Config/configMatlabSensorDev Set measurement noise and prediction model noise standard deviations (OCEKF, InvEKF, DILIGENT-KIO).
+Config/configSchmittParams Set Schmitt Trigger parameters for contact detection based on contact normal forces
+Config/configQEKFParams Set SWA-specific attitude estimator parameters (quaternion based EKF for attitude estimation)

General architecture of the Estimators

All the filters are implemented in a similar architecture (mainly inspired from UMich-BipedLab/Contact-Aided-Invariant-EKF and BlockFactory based thinking of a discrete system block). The architecture is described below,

drawing

  • We call the one-time setup function for the filter by passing the prior standard deviations for the system states, sensor standard deviations for the available measurements and estimator options. All these parameters are available as structs in the Estimation.Proprioception namespace.
  • We also additionally pass ModelComputations object to the filter during the setup phase which separates the model based computations (kinematics and dynamics) from the filter implementation.
  • The filter is initialized with the initial state and the advance function is called in a loop by passing the contact states, IMU and encoder measurements.
  • Internally, the advance step runs the prediction and update steps for the filter.