CAPO-LeggedRobotOdometry 🦾

CAPO-LeggedRobotOdometry is a pure proprioceptive odometry library for legged robots, implemented with a portable C++ estimator core that depends only on IMU and joint motor data.

The core estimation logic is implemented in FusionEstimator/fusion_estimator.h. The file fusion_estimator_node.cpp provides a ROS 2 wrapper around this estimator, while the Matlab/ folder contains examples for MATLAB + C++ mixed compilation and offline evaluation.

For side-by-side comparison, Matlab/Comparison/invariant-ekf/ provides a MATLAB mixed-compilation workflow for invariant-ekf, making it easier to compare this repository against a representative open-source legged odometry baseline.

In 3D closed-loop trials (a 200 m horizontal and 15 m vertical loop), Astrall point-foot robot A achieves 0.1638 m horizontal error and 0.219 m vertical error; for wheel-legged robot B, the corresponding errors are 0.2264 m and 0.199 m.

📄 Paper

Contact-Anchored Proprioceptive Odometry for Quadruped Robots (arXiv:2602.17393)

• Paper: https://arxiv.org/abs/2602.17393

If you use this repository in research, please consider citing the paper.

📦 Data Sharing (Go2-EDU ROS bags)

To help readers quickly validate the pipeline, we provide Go2-EDU trial datasets, including real-world videos, derived CSV files, and the corresponding ROS bag topics/messages required by this node, enabling fast reproduction and sanity checks.

• Download (GitHub Releases): https://github.com/ShineMinxing/CAPO-LeggedRobotOdometry/releases/tag/DataForTest
• Recommended assets in that release include GO2Flat, GO2Stairs, MPXY150Z10, MWXY150Z10, robot_flat_1_compress.zip, and robot_stairs_1_compress.zip

Note: the IMU on this Go2-EDU platform exhibits noticeable yaw drift, so the odometry accuracy is generally worse than the results reported for Astrall robots A and B in the paper.

✨ Key Features

🏗️ Related Repositories

⚠️ Clone as needed. If you only need state estimation, this repository is sufficient. For mapping, it is natural to pair it with Ros2SLAM and Ros2Go2Base.

📂 Repository Layout

CAPO-LeggedRobotOdometry/
├── CMakeLists.txt
├── package.xml
├── config.yaml
├── fusion_estimator_node.cpp        # ROS2 wrapper around the C++ estimator core
├── FusionEstimator/                 # portable pure C++ estimator core
│   ├── Estimators/
│   ├── fusion_estimator.h           # main estimator entry
│   ├── LowlevelState.h
│   ├── SensorBase.cpp
│   ├── SensorBase.h
│   ├── Sensor_IMU.cpp
│   ├── Sensor_IMU.h
│   ├── Sensor_Legs.cpp
│   ├── Sensor_Legs.h
│   └── Readme.md
├── Matlab/                          # MATLAB + MEX examples for the same C++ core
│   ├── build_mex.m
│   ├── fusion_estimator.m
│   ├── fusion_estimator_mex.cpp
│   ├── Comparison/
│   │   └── invariant-ekf/           # MATLAB mixed-compilation workflow for invariant-ekf
│   └── ...                          # optional test datasets are published via GitHub Releases
├── Plotjuggler.xml
└── Readme.md

🧩 Architecture Notes

This repository is intentionally split into three layers:

File truncated at 100 lines see the full file