Multi-Sensor Fusion¶

Combine visual SLAM with IMU, depth, and DVL sensors.

Sensor Combinations¶

Visual-Inertial (VI)¶

Camera + IMU

Benefits: - Better tracking in fast motion - Resolves scale ambiguity - Reduced drift

Visual-Inertial-Depth (VID)¶

Camera + IMU + Depth sensor

Benefits: - Absolute depth measurements - No scale drift - Better in featureless areas

Visual-Inertial-DVL (VI-DVL)¶

Camera + IMU + Doppler Velocity Log

Benefits: - Highest accuracy (<0.5% drift) - Long mission capability - Works in poor visibility

IMU Preintegration¶

def preintegrate_imu(imu_data, dt):
    delta_p = np.zeros(3)
    delta_v = np.zeros(3)
    delta_q = Quaternion.identity()

    for imu in imu_data:
        # Integrate rotation
        omega = imu.gyro - bias_gyro
        delta_q = delta_q * Quaternion.from_angular_velocity(omega, dt)

        # Integrate velocity and position
        accel_world = delta_q.rotate(imu.accel - bias_accel)
        delta_v += accel_world * dt
        delta_p += delta_v * dt + 0.5 * accel_world * dt**2

    return delta_p, delta_v, delta_q

Configuration¶

sensor_fusion:
  imu:
    enabled: true
    rate: 200  # Hz
    noise_gyro: 0.01
    noise_accel: 0.1

  depth:
    enabled: true
    rate: 10  # Hz
    noise: 0.01  # meters

  dvl:
    enabled: false  # optional
    rate: 5  # Hz
    noise: 0.01  # m/s