Multi-Sensor Fusion (C++)

Fuse IMU heading with wheel odometry position for more accurate localization than either sensor alone. Uses a complementary filter approach to blend the strengths of both sensors.

Problem

Wheel odometry gives position (x, y) and heading (theta), but heading drifts because of wheel slip, uneven terrain, and tire wear. The IMU gyroscope gives accurate short-term heading changes but drifts long-term due to bias. You need to combine both sensors so the robot's position estimate is better than either one alone.

How It Works

This fusion node runs a complementary filter on heading while taking position directly from odometry:

Position (x, y): comes from wheel odometry. The IMU accelerometer is too noisy for double-integration into position — it drifts by meters within seconds. Wheel odometry is the only practical source for position without GPS or SLAM.
Heading (theta): blended between odometry heading and IMU gyro integration. The gyro is trusted for short-term changes (no wheel slip), while odometry heading provides a long-term reference (no gyro drift).

The fusion formula for heading:

theta = alpha * (theta + gyro_z * dt) + (1 - alpha) * odom_theta

Where alpha is the IMU trust factor (0.95 = 95% gyro, 5% odometry correction). High alpha gives smoother heading during turns (gyro-dominated). Low alpha corrects gyro drift faster (odometry-dominated).

Why not a full Kalman filter? A Kalman filter is the theoretically optimal approach, but it requires a noise model (process and measurement covariances), matrix operations, and careful tuning. The complementary filter gives 90% of the benefit with 10% of the complexity. Start here, upgrade to Kalman only if you need covariance estimates or more than two sensors.

When To Use

Any ground robot with wheel encoders and an IMU
When odometry heading drifts due to wheel slip (carpet, gravel, wet floors)
When you need better localization than odometry alone but cannot run SLAM

Prerequisites

HORUS installed (Installation Guide)
Basic understanding of nodes and topics (C++ Quick Start)
An odometry source (e.g., Differential Drive recipe)
An IMU source (e.g., IMU Reader recipe)

Complete Code

#include <horus/horus.hpp>
#include <cmath>
#include <cstdio>
using namespace horus::literals;

class SensorFusion : public horus::Node {
public:
    SensorFusion(double alpha = 0.95)
        : Node("sensor_fusion"), alpha_(alpha)
    {
        // Subscribe to both sensor sources
        imu_sub_   = subscribe<horus::msg::Imu>("imu.data");
        odom_sub_  = subscribe<horus::msg::Odometry>("odom");
        // Publish the fused estimate
        fused_pub_ = advertise<horus::msg::Odometry>("fused.pose");
    }

    void tick() override {
        // ── Always drain both subscribers to avoid stale data ────────
        auto imu  = imu_sub_->recv();
        auto odom = odom_sub_->recv();

        double dt = 1.0 / 100.0;  // matches 100 Hz tick rate

        // ── Predict from odometry (position + heading reference) ────
        if (odom) {
            // Position comes directly from odometry — no IMU correction
            // IMU accelerometer is too noisy for position estimation
            x_ = odom->get()->pose.x;
            y_ = odom->get()->pose.y;

            // Odometry heading provides long-term reference (no gyro drift)
            odom_theta_ = odom->get()->pose.theta;
            has_odom_ = true;
        }

        // ── Correct heading from IMU gyroscope ──────────────────────
        if (imu) {
            double gz = imu->get()->angular_velocity[2];  // yaw rate (rad/s)

            if (has_odom_) {
                // Complementary filter: gyro for short-term, odom for long-term
                // alpha_ high (0.95) = trust gyro more = smooth during wheel slip
                // alpha_ low (0.80) = trust odom more = corrects gyro drift faster
                theta_ = alpha_ * (theta_ + gz * dt) + (1.0 - alpha_) * odom_theta_;
            } else {
                // No odometry yet — integrate gyro only (will drift)
                theta_ += gz * dt;
            }
            has_imu_ = true;
        } else if (has_odom_ && !has_imu_) {
            // No IMU data — fall back to odometry heading
            theta_ = odom_theta_;
        }

        // Normalize theta to [-pi, pi]
        if (theta_ > M_PI)  theta_ -= 2.0 * M_PI;
        if (theta_ < -M_PI) theta_ += 2.0 * M_PI;

        // ── Publish fused estimate ──────────────────────────────────
        horus::msg::Odometry fused{};
        fused.pose.x     = x_;
        fused.pose.y     = y_;
        fused.pose.theta = theta_;
        fused_pub_->send(fused);

        // Log at 2 Hz (every 50th tick at 100 Hz)
        if (++tick_count_ % 50 == 0) {
            double heading_diff = theta_ - odom_theta_;
            char buf[128];
            std::snprintf(buf, sizeof(buf),
                "fused=(%.2f, %.2f, %.1f deg)  odom_drift=%.1f deg",
                x_, y_, theta_ * 180.0 / M_PI,
                heading_diff * 180.0 / M_PI);
            horus::log::info("fusion", buf);

            // Log large heading disagreement as a warning
            if (std::abs(heading_diff) > 0.17) {  // ~10 degrees
                horus::log::warn("fusion",
                    "Large IMU/odom heading disagreement — possible wheel slip");
            }
        }
    }

    void enter_safe_state() override {
        // Publish last known good pose with a flag indicating degraded mode.
        // Downstream nodes (e.g., navigation) should check for stale data
        // and switch to a conservative behavior.
        horus::msg::Odometry last{};
        last.pose.x     = x_;
        last.pose.y     = y_;
        last.pose.theta = theta_;
        fused_pub_->send(last);
        horus::blackbox::record("fusion",
            "Entered safe state — publishing last known pose");
        horus::log::warn("fusion",
            "Safe state active — fused estimate may be stale");
    }

private:
    horus::Subscriber<horus::msg::Imu>*      imu_sub_;
    horus::Subscriber<horus::msg::Odometry>* odom_sub_;
    horus::Publisher<horus::msg::Odometry>*  fused_pub_;

    // Filter parameter
    double alpha_;  // IMU trust factor (0.80-0.99 typical)

    // Fused state (double for running-sum precision)
    double x_ = 0, y_ = 0, theta_ = 0;
    double odom_theta_ = 0;

    // Data availability flags
    bool has_odom_ = false;
    bool has_imu_  = false;

    int tick_count_ = 0;
};

int main() {
    horus::Scheduler sched;
    sched.tick_rate(100_hz).name("fusion_demo");

    // alpha=0.95: 95% gyro trust for heading, 5% odometry correction
    SensorFusion fusion(0.95);
    sched.add(fusion)
        .order(20)           // after odometry (0-10) and IMU (5) nodes
        .budget(2_ms)        // lightweight math
        .on_miss(horus::Miss::Warn)  // warn but don't stop — downstream can tolerate a stale estimate
        .build();

    sched.spin();
}

Common Errors

Symptom	Cause	Fix
Heading oscillates during turns	`alpha` too low — odometry heading disagrees with gyro	Increase alpha (0.95-0.99) to trust gyro more during turns
Heading drifts when stationary	`alpha` too high — gyro drift not corrected	Decrease alpha toward 0.90 or calibrate gyro bias
Position is completely wrong	Odometry node not running or publishing on wrong topic	Verify odom node publishes to "odom" topic
Sudden heading jumps	Odometry theta wraps at pi/-pi differently than fused theta	Normalize both angles before blending
No output on startup	Neither sensor has published yet	Check has_odom/has_imu flags; fusion waits for first data
Heading matches odometry exactly	IMU not publishing or alpha = 0	Verify IMU node runs and alpha is set correctly

Design Decisions

Choice	Rationale
Position from odometry only	IMU accelerometer double-integration drifts meters in seconds — unusable for position
Complementary filter over Kalman	90% of Kalman's benefit with 10% of the complexity; no covariance matrices
`alpha = 0.95` default	Trusts gyro for smooth turns while slowly correcting drift from odometry
Graceful degradation	Works with odometry-only, IMU-only, or both — does not crash on missing data
`order(20)`	Runs after sensor nodes (IMU=5, odom=10) so both inputs are fresh
`Miss::Warn` policy	A stale fused estimate is imperfect but not dangerous — downstream nodes tolerate it
Heading disagreement warning	Large drift between IMU and odometry signals wheel slip or sensor failure
`enter_safe_state()` publishes last pose	Downstream navigation nodes get a final estimate rather than silence during a safety event

Variations

Extended Kalman Filter (EKF) — for optimal fusion with uncertainty estimates:

// State: [x, y, theta], Measurement: [odom_x, odom_y, odom_theta, gyro_z]
// Requires process noise Q and measurement noise R matrices
void predict(double v, double w, double dt) {
    x_ += v * std::cos(theta_) * dt;
    y_ += v * std::sin(theta_) * dt;
    theta_ += w * dt;
    // P = F * P * F^T + Q  (covariance propagation)
}
void correct_odom(double ox, double oy, double otheta) {
    // K = P * H^T * (H * P * H^T + R)^-1  (Kalman gain)
    // x = x + K * (z - H * x)               (state update)
}

GPS fusion — add absolute position correction when available:

if (auto gps = gps_sub_->recv()) {
    // GPS provides absolute x/y but at low rate (1-10 Hz) and with noise (~2m)
    double gps_alpha = 0.05;  // low trust — GPS is noisy
    x_ = (1.0 - gps_alpha) * x_ + gps_alpha * gps->get()->x;
    y_ = (1.0 - gps_alpha) * y_ + gps_alpha * gps->get()->y;
}

Multi-IMU fusion — average multiple IMUs for redundancy and noise reduction:

auto imu1 = imu1_sub_->recv();
auto imu2 = imu2_sub_->recv();
double gz = 0;
int count = 0;
if (imu1) { gz += imu1->get()->angular_velocity[2]; count++; }
if (imu2) { gz += imu2->get()->angular_velocity[2]; count++; }
if (count > 0) gz /= count;  // averaged yaw rate

Key Takeaways

Never integrate IMU accelerometer for position — it drifts meters in seconds
The complementary filter is the right starting point: simple, tunable, and effective
Drain both subscribers every tick, even if only one has new data, to avoid stale buffers
Log the heading disagreement between sensors — it reveals wheel slip and sensor failures
order(20) ensures sensor nodes run first and fusion gets fresh data every tick
Use enter_safe_state() to publish a final known-good pose before the system shuts down