CAPO-LeggedRobotOdometry 🦾

CAPO-LeggedRobotOdometry is a ROS 2 (Humble) proprioceptive odometry / state-estimation repository for biped / quadruped / wheel-legged robots on Ubuntu 22.04.

It provides high-accuracy odometry using only IMU + joint encoders + foot contact/force signals, without requiring cameras or LiDAR.

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.

At the repository level, fusion_estimator_node.cpp is mainly the ROS2 wrapper (topics, parameters, message conversion, publishing), while the actual fusion-estimation algorithm lives in FusionEstimator/ as a portable, pure C++ implementation. This makes it straightforward to port the estimator to ROS1, non-ROS2 applications, or embedded platforms.

In addition, the Matlab/ folder provides a MATLAB + MEX example that compiles and calls the same C++ estimator core for offline validation, algorithm analysis, and cross-platform reuse.

📄 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 two Go2-EDU trial datasets, including real-world videos and the corresponding ROS bag topics/messages required by this node, enabling fast reproduction and sanity checks.

• Download (Google Drive): https://drive.google.com/drive/folders/1FfVO69rfmUu6B9crPhZCfKf9wFnV4L7n?usp=sharing

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 node wrapper: params / topics / odom publishing
├── FusionEstimator/                 # pure C++ fusion-estimation core (portable)
│   ├── Estimators/
│   ├── fusion_estimator.h
│   ├── LowlevelState.h
│   ├── SensorBase.cpp
│   ├── SensorBase.h
│   ├── Sensor_IMU.cpp
│   ├── Sensor_IMU.h
│   ├── Sensor_Legs.cpp
│   ├── Sensor_Legs.h
│   └── Readme.md
├── Matlab/                          # MATLAB + MEX example for calling the C++ core
│   ├── build_mex.m
│   ├── fusion_estimator.m
│   ├── fusion_estimator_mex.cpp
│   ├── MPXY150Z10.csv
│   └── MWXY150Z10.csv
├── Plotjuggler.xml                  # PlotJuggler layout / visualization helper
└── Readme.md

🧩 Architecture Notes

This repository is intentionally split into two layers:

File truncated at 100 lines see the full file