eidos_transform

Dual-EKF multi-source odometry fusion and TF broadcasting for ROS 2.

eidos_transform runs as a standalone lifecycle node, separate from eidos SLAM. It maintains two independent EKF instances – a local EKF operating in the odom frame and a global EKF operating in the map frame – fuses configurable measurement sources into each, broadcasts the resulting TF tree, and publishes a fused odometry topic.

Architecture

Dual-EKF design

The node follows a robot_localization-style dual-EKF architecture:

• Local EKF (odom frame): Fuses odom_sources only. Produces a smooth, continuous odom -> base_link transform and the published /odom topic. This EKF never receives map-frame corrections, so it remains jitter-free.
• Global EKF (map frame): Fuses odom_sources (same data as the local EKF) plus map_sources (e.g. SLAM pose, GPS corrections). Used solely to derive the map -> odom transform.

Both EKFs are the same pluginlib-loaded model (e.g. HolonomicEKF), instantiated twice with separate parameter namespaces (holonomic_ekf for local, holonomic_ekf_global for global).

map -> odom computation

The map -> odom transform is computed directly from the two EKF poses without any TF lookup:

map_to_odom = global_ekf_pose * inverse(local_ekf_pose)

Both EKFs are updated in the same tick, so the local EKF pose used here is exactly the odom -> base_link that was just broadcast. This eliminates timestamp-matching issues entirely.

Rewind-replay for delayed measurements

Map-frame sources (e.g. SLAM) often arrive with latency. The global EKF handles this with a rewind-replay mechanism:

1. After each predict step, a StateSnapshot of the global EKF is saved to a bounded history (max 500 entries).
2. When a map source measurement arrives with a timestamp older than the current tick, the algorithm finds the latest snapshot before that timestamp.
3. The global EKF is restored to that snapshot.
4. All measurements from that point forward (including the delayed one) are sorted by time and replayed with predict steps between them.
5. A final predict brings the EKF to the current time.

This only applies to the global EKF. The local EKF fuses measurements immediately without delay handling.

Measurement sources

Sources are configured as a unified MeasurementSource struct supporting two types via the type field:

• type: "odom" – subscribes to a nav_msgs/Odometry topic. Provides 6-DOF pose and 6-DOF twist, each independently masked and with per-DOF noise.
• type: "imu" – subscribes to a sensor_msgs/Imu topic. Provides angular velocity, orientation, and optionally linear acceleration with gravity compensation and accelerometer bias estimation.

Sources are assigned to EKFs by which list they appear in:

IMU source processing

When an IMU source is fused:

1. Frame rotation: The imu_frame -> base_link static TF is looked up once and cached. All IMU measurements are rotated into the base link frame before fusion.
2. Angular velocity: Fused as a twist update (rotation DOFs only) using updateTwist.
3. Orientation: The IMU quaternion is converted to a rotation matrix, combined with the IMU-to-base rotation, and fused as a pose update (rotation DOFs only) using updatePose. The current EKF translation is preserved so Logmap only sees rotation error.
4. Linear acceleration (optional): Gravity is removed using the EKF’s current orientation estimate (unless gravity_compensated is true). The gravity-free acceleration is passed to updateAcceleration, which handles bias subtraction and velocity integration internally.

Data flow

odom_sources (wheel odom, LiDAR odom, IMU) --> [Local EKF]  --> odom->base_link TF + /odom topic
odom_sources + map_sources (SLAM, GPS)      --> [Global EKF] --> map->odom TF
UTM transform                               --> [broadcast]  --> utm->map static TF

Quick start

Standalone launch:

ros2 launch eidos_transform eidos_transform.launch.yaml

With a custom config:

ros2 launch eidos_transform eidos_transform.launch.yaml config_file:=/path/to/params.yaml

Launch arguments:

The node runs under the /world_modeling namespace and is managed by wato_lifecycle_manager.

Configuration

All parameters live under /**/eidos_transform_node/ros__parameters.

Core parameters

File truncated at 100 lines see the full file