This repository contains an example Matlab/Simulink implementation of the contact-aided invariant extended Kalman filter. The filter was designed for use on a Cassie-series biped robot, however it can be easily modified for other systems. The included measurement dataset was generated through a simulation of Cassie walking.
This filter is developed and explained in: "Contact-aided Invariant Extended Kalman Filtering for Legged Robot State Estimation".
A C++ library and ROS wrapper for the filter is currently under development.
Open Matlab to the "matlab_example" folder.
Run the scipt "run_RIEKF_test.m". This will open and run a simulink model with the measurement data stored in the "/data" folder. After the simulation finishes, a few plots will appear analyzing the results of the state estimator.
The filter parameters can be changed in the "example_code/RIEKF_InitFcn.m" script. This script is automatically executed when the simulink model is run.
The following parameters will affect the actual noisy measurements coming into the filter:
gyro_true_bias_init
- Initial gyroscope bias accel_true_bias_init
- Initial accelerometer bias gyro_true_noise_std
- Standard deviation of noise added to the gyroscope measurement gyro_true_bias_noise_std
- Standard deviation of noise added to the gyroscope bias accel_true_noise_std
- Standard deviation of noise added to the accelerometer measurement accel_true_bias_noise_std
- Standard deviation of noise added to the accelerometer bias landmark_true_noise_std
- Standard deviation of noise added to the landmark measurements landmark_positions
- List of landmark positions with an associated IDlandmark_measurement_frequency
- Frequency of incoming landmark measurementsThe following parameters will affect how the filter is run:
static_bias_initialization
- Flag that enables static bias initialization, where the initial bias estimate is obtained from the first few seconds of data assuming the base pose remains fixed. Keep this flag to false for the included dataset.ekf_update_enabled
- Flag that enables the update phase of the Kalman filter.enable_kinematic_measurements
- Flag that enables kinematic measurements.enable_landmark_measurements
- Flag that enables landmark measurements.enable_static_landmarks
- Flag that enables static landmarks. If false, the landmark positions will be estimated along with the rest of the state variables.The following parameters affect the initial condition and covariances used for the process and measurement models:
gyro_bias_init
- Initial gyroscope bias estimateaccel_bias_init
- Initial accelerometer bias estimategyro_noise_std
- Standard deviation of the gyroscope measurement noisegyro_bias_noise_std
- Standard deviation of the gyroscope bias noiseaccel_noise_std
- Standard deviation of the accelerometer measurement noiseaccel_bias_noise_std
- Standard deviation the accelerometer bias noisecontact_noise_std
- Standard deviation of the contact frame velocity measurement noiseencoder_noise_std
- Standard deviation of the joint encoder measurement noiselandmark_noise_std
- Standard deviation of the landmark measurement noiseThe following parameters set the initial covariance for the state estimate:
prior_base_pose_std
- Initial base orientation and position standard deviationprior_base_velocity_std
- Initial base velocity standard deviationprior_contact_position_std
- Initial contact position standard deviationprior_gyro_bias_std
- Initial gyroscope bias standard deviationprior_accel_bias_std
- Initial accelerometer bias standard deviationprior_forward_kinematics_std
- Additional noise term that is added to increase the forward kinematics measurement covarianceThe contact-aided invariant extended Kalman filter is described in:
@INPROCEEDINGS{Hartley-RSS-18,
AUTHOR = {Ross Hartley AND Maani Ghaffari Jadidi AND Jessy Grizzle AND Ryan M Eustice},
TITLE = {Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation},
BOOKTITLE = {Proceedings of Robotics: Science and Systems},
YEAR = {2018},
ADDRESS = {Pittsburgh, Pennsylvania},
MONTH = {June},
DOI = {10.15607/RSS.2018.XIV.050}
}
The core theory of invariant extended Kalman filtering is presented in:
@article{barrau2017invariant,
title={The invariant extended Kalman filter as a stable observer},
author={Barrau, Axel and Bonnabel, Silv{\`e}re},
journal={IEEE Transactions on Automatic Control},
volume={62},
number={4},
pages={1797--1812},
year={2017},
publisher={IEEE}
}