Vehicular Fusion (Odometry-IMU)

Node ID: vehicularFusion · Role: Filter · Realtime config: no

Description

Vehicle dead-reckoning filter fusing wheel odometry (CAN bus speed) with IMU. Uses bicycle or differential drive model. Outputs 2D vehicle pose with heading, velocity, and UTM coordinates.

Algorithm notes

What the filter estimates

This node runs a planar sigma-point (unscented) Kalman filter for a road or field vehicle. The estimated state is two-dimensional position, heading (yaw), and a slowly varying gyroscope yaw-rate bias. Motion is propagated with a constant-velocity, constant-turn-rate kinematic model driven by wheel speed and yaw rate as control inputs, so there is no acceleration or pitch/roll state.

Positions are held in a local metric frame whose axes are northing and easting, anchored on the first usable position fix. The UTM zone is latched at that moment, so the published global latitude and longitude never jump at a zone boundary. Yaw is measured from north toward east. Gyroscope samples are interpreted in the sensor frame and brought into the vehicle frame through the mounting rotation and the turn-rate axis selection.

How each input is used

Wheel speed is the pacing input: one fused pose is published per wheel-speed sample, and no other stream can produce output on its own. Speed enters as the linear control input, optionally scaled by the estimated wheel-speed scale factor.

The IMU supplies yaw rate about the selected axis, and its magnitude is also used for slip detection and standstill bias updates. If IMU samples stop arriving for longer than the configured timeout, the node either switches to the wheel-derived turn rate (only if the wheel fallback is enabled and the wheel rate is flagged valid) or stops publishing entirely, rather than dead-reckoning on a stale rate.

GNSS provides the position update, and optionally a heading update from a dual-antenna solution. Nothing is published until the filter has initialized: it waits for a fix of acceptable quality, then takes the initial heading either from the receiver orientation or, more commonly, from course over ground once several consecutive samples exceed the minimum speed and agree with the wheel speed. Heading quality is checked independently of position quality, so a degraded moving-baseline solution is rejected even at an RTK-fixed position, and a good heading keeps correcting yaw straight through a position outage.

Optical pose can replace GNSS as the position source. Its frame is scaled and, when alignment is enabled, rotated onto the GNSS frame after both sensors have observed a couple of metres of motion; the first optical position is deliberately withheld until that rotation is known.

What the parameters control

Process noise on speed and turn rate sets how far the filter trusts dead reckoning between fixes: raise them for a responsive but noisier trajectory, lower them for a smooth trajectory that lags real accelerations and turns.

Measurement trust is set by the base position sigma plus the quality-dependent options. Quality weighting admits DGPS and float fixes at loose sigmas as a drift leash rather than treating them like an RTK fix, and correction-age de-weighting bleeds trust away from RTK fixes computed on stale corrections. Robustness parameters gate the position innovation; an outlier is softly de-weighted, never discarded.

Heading and calibration options select the yaw-rate source, enable the gyro bias state (learned from long straight segments with verified GNSS heading and from stopped samples), and enable the slow wheel-speed scale estimate learned from RTK-versus-odometry travelled distance.

Stopped detection freezes the state below the velocity threshold, with hysteresis, and smoothing blends the lagged and current states so fixes do not appear as visible steps.

Tuning and failure modes

Start with heading: verify the turn-rate axis and mounting rotation by driving a slow figure-eight and checking that reported yaw rate matches the turn direction. A wrong axis or sign produces heading that diverges immediately while position looks plausible for a few seconds.

Common problems: no output at all usually means initialization never completed (fix quality too low, or the vehicle never drove straight above the initialization speed); reversing during startup corrupts course-over-ground initialization; an overly tight innovation gate combined with very small position sigma makes the filter reluctant after long outages; loose float sigmas set too tight make the filter chase float noise; smoothing adds a small effective latency that matters for control loops; and wheel scale or slip estimates degrade if the wheel angular rate is unreliable. Playback seeks and loops re-anchor the filter instead of fusing across the time jump.

Inputs / Outputs

• Inputs: Gnss, Imu, VehicleSpeed, Optical
• Outputs: FusedVehiclePose, FusedVehiclePoseV2

Config aliases

OdometryImuFilter, odometryImuFilter, vehicularFusion

Required feature

vehicular_fusion

Properties

The following fields are exposed in the node’s Properties panel in the UI. Types map to UI widgets (e.g. string → text input, number → numeric, boolean → toggle, select → dropdown, json → JSON editor).

Example node definition

A full node definition in config.json looks like the block below. Paste it under the sinks key of your config, keyed by the node’s instance name (any identifier; it doesn’t have to match the node ID).

{
  "sinks": {
    "vehicularFusion": {
      "dataEndpoint": "inproc://vehicularFusion_data",
      "inputEndpoints": [
        "inproc://gnss_data",
        "inproc://imu_data",
        "inproc://vehicleSpeed_data",
        "inproc://optical_data"
      ],
      "inputDataFilter": [
        "Gnss",
        "Imu",
        "VehicleSpeed",
        "Optical"
      ],
      "settings": {
        "fuser": {
          "alignOpticalToGnss": true,
          "detectSlip": true,
          "estimateGyroBias": true,
          "estimateWheelScale": true,
          "gnssDgpsError": 4.0,
          "gnssFloatError": 12.0,
          "gnssYawError": 0.05,
          "gyroBiasRandomWalk": 0.0005,
          "gyroBiasZuptError": 0.0035,
          "imuTimeoutLimit": 0.5,
          "imuToCarRotation": {
            "w": 1,
            "x": 0,
            "y": 0,
            "z": 0
          },
          "imuTurnRateAxis": {
            "x": 0,
            "y": 0,
            "z": 1
          },
          "initVelocityThreshold": 3.0,
          "initVelocityTolerance": 0.4,
          "initializeFromGnssOrientation": false,
          "innovationGate": 5.0,
          "measurementError": 0.1,
          "omegaError": 0.5,
          "opticalScalingFactor": 10.0,
          "retainStateWhenStopped": true,
          "rtcmAgeSigmaPerSec": 0.05,
          "smoothFit": true,
          "useGnssQualityWeighting": false,
          "useGnssYawMeasurement": false,
          "useGpsOnRtkFloat": false,
          "useImuTurnRate": true,
          "useOpticalData": false,
          "usePositionOutlierDeweighting": true,
          "useRtcmAgeDeweighting": true,
          "useWheelAngularFallback": false,
          "velError": 0.278,
          "velocityThreshold": 0.01,
          "wheelOmegaScale": 1.0
        }
      }
    }
  }
}

Field reference

Connections can also be declared at the top-level connections array when settings.explicitConnections is true, as an alternative to filling inputEndpoints on each node.