Architecture¶

Link to module repo: odom_gnss_fusion

Observer-Based Fusion Algorithm¶

The node fuses two complementary sensor sources:

• Odometry velocity (high-frequency, drifts over time)
• GNSS pose (low-frequency, globally accurate)

It maintains two parallel pose estimates:

• Odometry pose — dead-reckoned by integrating velocity in the local odom frame.
• Observed pose — fused estimate that is continuously corrected toward GNSS.

Both are used to broadcast the REP 105 TF hierarchy (map -> odom -> base_link).

Mathematical Formulation¶

The fusion uses a continuous observer where the observed state \(\mathbf{\xi}_{O}\) tracks the true state through:

Where:

State Vector¶

The rover operates on a 3-DOF planar state:

where \(x, y\) are positions in the map plane and \(\varphi\) is the yaw angle. Roll, pitch, and Z position are assumed zero.

Discrete Implementation¶

The continuous observer is discretized using Euler forward integration:

where \(\Delta t = \frac{1}{f_{control}}\) is the control loop period.

Observer Time Constant¶

The time constant \(\tau\) determines how quickly the observer corrects toward GNSS:

• Smaller \(\tau\) → faster convergence, noisier output
• Larger \(\tau\) → slower convergence, smoother output
• Convergence time \(\approx 5\tau\) to reach 99% of steady state

See the Tuning Guide for how to choose \(\tau\) in practice.

Data Flow¶

Subscribers and Publishers¶

flowchart TD
    A@{ shape: stadium, label: "/odom_velocity<br/>(geometry_msgs/Twist)<br/>e.g., 1000 Hz"}
    B@{ shape: stadium, label: "/gnss_pose<br/>(geometry_msgs/PoseStamped)<br/>e.g., 10 Hz"}
    C@{ shape: rect, label: "Observer Node" }
    D@{ shape: stadium, label: "/observed_pose<br/>(geometry_msgs/PoseStamped)<br/>e.g., 1000 Hz"}
    E@{ shape: stadium, label: "/observed_twist<br/>(geometry_msgs/TwistStamped)<br/>e.g., 1000 Hz"}

    A -->|Velocity Input| C
    B -->|GNSS Position| C

    C -->|Fused Pose| D
    C -->|Fused Velocity| E

Transform Broadcasters (REP 105)¶

flowchart TD
    A@{ shape: rect, label: "Observer Node" }
    B@{ shape: stadium, label: "odom -> base_link<br/>(TF2 Transform)<br/>e.g., 1000 Hz"}
    C@{ shape: stadium, label: "map -> odom<br/>(TF2 Transform)<br/>e.g., 1000 Hz"}

    A -->|Dead-reckoned Pose| B
    A -->|Correction Factor| C

Processing Steps¶

1. Velocity input arrives on /odom_velocity (body frame, i.e. base_link).
2. GNSS pose arrives on /gnss_pose (pose of gnss_link in map frame).
3. GNSS transform: the node looks up the TF from gnss_frame_id → base_link_frame_id to obtain the base pose in map.
4. Observer update (runs at control_loop_frequency_in_hz):
   • Velocity is rotated into the odom frame and integrated → updates odom_pose.
   • Velocity is rotated into the map frame, the GNSS error term is added, and the result is integrated → updates observed_pose.
5. TF broadcast:
   • odom_frame_id -> base_link_frame_id from the dead-reckoned odom_pose.
   • map_frame_id -> odom_frame_id computed as \({}^{map}T_{base\_link} \cdot {}^{odom}T_{base\_link}^{-1}\).

Timing and Synchronization¶

The node does not perform explicit time synchronization. It uses the latest available measurement from each sensor at each control cycle. The observer naturally handles the rate mismatch between high-frequency odometry and low-frequency GNSS.

If no GNSS message has been received for more than 1 second, the correction term is paused and the node degrades to pure dead-reckoning until a fresh GNSS message arrives.

Error Handling¶