← Emre Girgin

Odometry on a Quadruped

Getting OpenVINS, VINS-Fusion and a leg-inertial invariant EKF running on quadruped recordings with ROS2 Humble. Examples use a Unitree Go2 with a RealSense camera, but nothing here is specific to that setup.

Before anything: three things that will bite you

  1. The IMU may not be a standard topic. Unitree robots publish the body IMU inside /lowstate. VIO packages expect sensor_msgs/Imu, so you need a small bridge node.
  2. Do not borrow IMU noise parameters. The values shipped with a camera config describe that camera's IMU, not your robot's. Run an Allan variance test, or start from generic MEMS values.
  3. Calibrate the extrinsics yourself. If you mounted the camera, the camera–IMU transform is unique to your robot. See Camera–IMU Calibration with Kalibr.

1. Bridge the IMU to sensor_msgs/Imu

A small ROS2 node that subscribes to the robot's state message (/lowstate on Unitree) and republishes on /imu:

When replaying a bag, run the bridge with use_sim_time:=true and play the bag with --clock.

2. Calibration

If you have to start before the Allan variance run, these generic MEMS values are a rough placeholder:

accelerometer_noise_density: 2.0e-3
accelerometer_random_walk: 3.0e-3
gyroscope_noise_density: 1.7e-4
gyroscope_random_walk: 2.0e-5

3. OpenVINS

Build

mkdir -p ~/vins_ws/src && cd ~/vins_ws/src
git clone https://github.com/rpng/open_vins/
cd ~/vins_ws
colcon build --event-handlers console_cohesion+ \
    --packages-select ov_core ov_init ov_msckf ov_eval

Sanity check on EuRoC

source ~/vins_ws/install/setup.bash
ros2 launch ov_msckf subscribe.launch.py config:=euroc_mav rviz_enable:=true
ros2 bag play ~/vins_ws/data/V1_01_easy

Configure for your robot

Copy the closest bundled config to a new folder (for a RealSense, rs_d455/ rather than euroc_mav/).

estimator_config.yaml:

use_stereo: false                 # monocular
max_cameras: 1
track_frequency: 30.0             # your actual camera rate
relative_config_imu: "my_robot_imu.yaml"
relative_config_imucam: "my_robot_extrinsics.yaml"

my_robot_imu.yaml (from kalibr_imu_chain.yaml): set rostopic: /imu, the measured update_rate, and the noise values from the Allan variance run.

my_robot_extrinsics.yaml (from kalibr_imucam_chain.yaml): replace T_cam_imu with the Kalibr result and set intrinsics, resolution and the image rostopic.

Run on a recording

# terminal 1
source ~/vins_ws/install/setup.bash
ros2 launch ov_msckf subscribe.launch.py config:=<your_config> rviz_enable:=true

# terminal 2: IMU bridge
ros2 run <your_package> <imu_bridge_node> --ros-args -p use_sim_time:=true

# terminal 3
ros2 bag play <bag> --clock

To open RViz separately:

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix ov_msckf)/share/ov_msckf/launch/display_ros2.rviz \
    --ros-args --log-level warn

Useful docs: installing, tutorial, datasets, calibration.

4. VINS-Fusion (ROS2 port)

Build

cd ~/vins_ws/src
git clone https://github.com/zinuok/VINS-Fusion-ROS2
cd ~/vins_ws
colcon build --packages-up-to vins --symlink-install
source ./install/setup.bash

On a machine without a CUDA build of OpenCV and Ceres, the port needs four changes:

  1. vins/src/featureTracker/feature_tracker.h: comment out #define GPU_MODE 1.
  2. vins/src/utility/visualization.cpp: rclcpp::Duration(0) → rclcpp::Duration(0, 0) for Humble.
  3. vins/src/estimator/estimator.cpp: drop the CUDA dense linear algebra option and use options.linear_solver_type = ceres::DENSE_SCHUR;
  4. vins/src/estimator/parameters.cpp: initialise USE_GPU, USE_GPU_ACC_FLOW and USE_GPU_CERES to 0.

Run

ros2 run vins vins_node ~/vins_ws/src/VINS-Fusion-ROS2/config/<your_config>/mono_imu_config.yaml
rviz2 -d ~/vins_ws/src/VINS-Fusion-ROS2/config/vins_rviz_config.rviz
# plus the IMU bridge and `ros2 bag play <bag> --clock` as above

Sanity-check first with config/euroc/euroc_stereo_imu_config.yaml on a EuRoC sequence.

5. Invariant EKF (leg + IMU)

legged_state_estimator fuses IMU and leg kinematics with contact estimation. It ships with Unitree A1 settings, which work for other quadrupeds given the robot's URDF.

Install

conda create -n legged_est python=3.8 -y
conda activate legged_est
conda install -c conda-forge pinocchio eigen pybind11 cmake pkg-config compilers python=3.8 -y
pip install pybullet scipy matplotlib numpy pandas

git clone --recursive https://github.com/mayataka/legged_state_estimator.git
cd legged_state_estimator

# CMakeLists.txt is missing the Python3 requirement
sed -i '/find_package(pinocchio REQUIRED)/a find_package(Python3 COMPONENTS Interpreter Development REQUIRED)' CMakeLists.txt
grep "find_package(Python3" CMakeLists.txt   # should print the added line

mkdir build && cd build
cmake .. -DPython3_EXECUTABLE=$(which python) -DCMAKE_BUILD_TYPE=Release
make -j4

Test with the A1 example

cd ../examples_python
export PYTHONPATH=$PYTHONPATH:$(pwd)/../build/bindings/python
cp ../bindings/python/legged_state_estimator/__init__.py ../build/bindings/python/legged_state_estimator/
python a1.py

How contact uncertainty is set

The contact covariance grows with how fast the normal ground reaction force changes:

const double df = normal_contact_force_estimate_[i] - normal_contact_force_estimate_prev_[i];
contact_force_covariance_[i] = settings_.contact_force_covariance_alpha * df * df;

That is, covariance = α · (Δfnormal)², with α typically 10 or 100. Large jumps in force are treated as unreliable contact.

Evaluating against GPS

The script below runs the estimator over a folder of recorded sequences and reports the absolute trajectory error (ATE) of each against ground truth (GPS/RTK, motion capture, anything with a pose). Export your recordings to the two CSV files described at the top of the script, set the paths, and run it from examples_python/.

Show script 
"""Run legged_state_estimator (InEKF) on recorded sequences and report ATE against ground truth.

Expected layout: one folder per sequence under DATA_ROOT, each containing

  proprio.csv       time [s], gyro_x/y/z [rad/s], accel_x/y/z [m/s^2],
                    and for each motor i in 0..11: motor_{i}_q, motor_{i}_dq, motor_{i}_tau
                    (Unitree motor order: FR, FL, RR, RL; hip, thigh, calf)
  groundtruth.csv   time [s], x, y, z [m], yaw [rad]   (any world frame, e.g. GPS/RTK or mocap)

Both trajectories are moved to the origin and rotated so the initial heading is +X
before computing the error, so only drift is measured.
"""
import os
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import legged_state_estimator
from scipy.interpolate import interp1d

# --- Configuration ---
URDF_PATH = "path/to/robot.urdf"
DATA_ROOT = "path/to/sequences"
OUTPUT_ROOT = "./odometry_results"

TIME_STEP = 0.002                    # IMU / joint rate, here 500 Hz
IMU_FRAME = "imu"
CONTACT_FRAMES = ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]

LPF_CUTOFF = 250                     # IMU low-pass [Hz]: higher = more jitter, less lag
JOINT_CUTOFF = 10                    # joint low-pass [Hz], A1 default

# CSV leg order (FR, FL, RR, RL) -> URDF leg order (FL, FR, RL, RR): first motor index per URDF leg
LEG_MAP = [3, 0, 9, 6]


def align_to_start(t, xyz, yaw, t_start, t_end):
    """Crop to [t_start, t_end], move the first pose to the origin and rotate it to face +X."""
    mask = (t >= t_start) & (t <= t_end)
    if np.sum(mask) < 2:
        return None, None
    t, xyz, yaw = t[mask], xyz[mask], yaw[mask]
    c, s = np.cos(-yaw[0]), np.sin(-yaw[0])
    R = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
    return t, (R @ (xyz - xyz[0]).T).T


def run_estimator(proprio_path):
    """Return a DataFrame with time, x, y, z. The estimator starts at the origin facing +X."""
    df = pd.read_csv(proprio_path)

    settings = legged_state_estimator.LeggedStateEstimatorSettings.UnitreeA1(URDF_PATH, TIME_STEP)
    settings.lpf_gyro_accel_cutoff_frequency = LPF_CUTOFF
    settings.lpf_lin_accel_cutoff_frequency = LPF_CUTOFF
    settings.lpf_dqJ_cutoff_frequency = JOINT_CUTOFF
    settings.lpf_tauJ_cutoff_frequency = JOINT_CUTOFF
    settings.imu_frame = IMU_FRAME
    settings.contact_frames = CONTACT_FRAMES

    estimator = legged_state_estimator.LeggedStateEstimator(settings)
    estimator.init(
        base_pos=np.array([0.0, 0.0, 0.28]),        # nominal standing height
        base_quat=np.array([0.0, 0.0, 0.0, 1.0]),
        base_lin_vel_world=np.zeros(3),
        imu_gyro_bias=np.zeros(3),
        imu_lin_accel_bias=np.zeros(3),
    )

    out = []
    for _, row in df.iterrows():
        gyro = row[['gyro_x', 'gyro_y', 'gyro_z']].to_numpy(dtype=float)
        accel = row[['accel_x', 'accel_y', 'accel_z']].to_numpy(dtype=float)

        qJ, dqJ, tauJ = np.zeros(12), np.zeros(12), np.zeros(12)
        for leg, first in enumerate(LEG_MAP):
            for j in range(3):
                m = first + j
                qJ[3*leg + j] = row[f'motor_{m}_q']
                dqJ[3*leg + j] = row[f'motor_{m}_dq']
                tauJ[3*leg + j] = row[f'motor_{m}_tau']

        estimator.update(gyro, accel, qJ, dqJ, tauJ)
        out.append((row['time'], *estimator.base_position_estimate))

    return pd.DataFrame(out, columns=['time', 'x', 'y', 'z'])


def main():
    os.makedirs(OUTPUT_ROOT, exist_ok=True)
    results = []

    for seq_dir in sorted(f.path for f in os.scandir(DATA_ROOT) if f.is_dir()):
        seq = os.path.basename(seq_dir)
        proprio = os.path.join(seq_dir, "proprio.csv")
        gt_file = os.path.join(seq_dir, "groundtruth.csv")
        if not (os.path.exists(proprio) and os.path.exists(gt_file)):
            print(f"[skip] {seq}: missing files")
            continue

        est = run_estimator(proprio)
        gt = pd.read_csv(gt_file)

        t_start = max(est['time'].min(), gt['time'].min())
        t_end = min(est['time'].max(), gt['time'].max())
        if t_end - t_start < 1.0:
            print(f"[skip] {seq}: less than 1 s overlap")
            continue

        gt_t, gt_xyz = align_to_start(gt['time'].to_numpy(), gt[['x', 'y', 'z']].to_numpy(),
                                      gt['yaw'].to_numpy(), t_start, t_end)

        # Sample the estimate at ground-truth timestamps and zero it at the window start
        est_xyz = interp1d(est['time'], est[['x', 'y', 'z']], axis=0,
                           fill_value="extrapolate")(gt_t)
        est_xyz -= est_xyz[0]

        ate = np.sqrt(np.mean(np.sum((est_xyz - gt_xyz) ** 2, axis=1)))
        results.append({"sequence": seq, "ate_rmse": ate, "samples": len(gt_t)})
        print(f"{seq}: ATE {ate:.3f} m")

        plt.figure(figsize=(8, 7))
        plt.plot(gt_xyz[:, 0], gt_xyz[:, 1], 'k--', label='Ground truth')
        plt.plot(est_xyz[:, 0], est_xyz[:, 1], 'r-', label='InEKF')
        plt.title(f"{seq}  ATE {ate:.3f} m")
        plt.xlabel("x [m]")
        plt.ylabel("y [m]")
        plt.axis('equal')
        plt.legend()
        plt.grid(alpha=0.3)
        plt.savefig(os.path.join(OUTPUT_ROOT, f"{seq}.png"))
        plt.close()

    if results:
        df = pd.DataFrame(results)
        df.to_csv(os.path.join(OUTPUT_ROOT, "ate.csv"), index=False)
        print(f"\nMean ATE over {len(df)} sequences: {df['ate_rmse'].mean():.3f} m")


if __name__ == "__main__":
    main()