IMU-Optical Fusion

Node ID: fusion · Role: Filter · Realtime config: yes

Description

Extended Kalman filter fusing IMU (accelerometer+gyroscope) with optical tracking (camera/mocap) to produce a 6-DOF fused pose. Supports real-time intercalibration to align sensor frames. Settings: alignment quaternion, intercalibration toggle.

Algorithm notes

What the node estimates

This node runs a complementary orientation filter with direct optical position pass-through, producing a 6-DOF pose stream. The estimated state is an orientation quaternion, the position of the tracked point, plus derived angular velocity, linear velocity, acceleration and latency fields. Orientation is propagated by integrating the gyroscope and is continuously pulled toward the optical orientation; position is taken from the optical stream and is never dead-reckoned from the IMU.

Three frames are involved: the IMU body frame, the optical target frame, and the optical/world reference frame, whose vertical axis is the +Y axis. Inertial measurements are rotated into the optical frame by the mounting alignment rotation before use. After fusion, the pose is composed with the optical alignment rotation and the lever-arm offset, so the published point is the offset point carried rigidly with the body. By default the output is expressed in the optical/world frame.

How the input streams are used

Every accepted sample produces an output pose, so the output rate is the sum of the inertial and optical rates.

Inertial samples drive prediction. The first valid sample initializes orientation from the measured gravity direction, which fixes pitch and roll but leaves heading arbitrary until optical data arrives. Subsequent samples integrate angular rate over the measured sample interval, optionally apply gravity-based tilt correction, and update the smoothed angular velocity used for reporting and prediction. Samples with a vanishing accelerometer magnitude are discarded.

Optical samples supply position and the orientation reference. The first optical sample received before any inertial data initializes the pose outright. In normal operation each optical sample replaces the position and blends the current orientation a fraction of the way toward the optical orientation, giving exponential convergence: with a blend weight w, the residual error decays by a factor (1 - w) per optical frame, so the effective time constant is roughly 1/w frames. If several consecutive optical frames arrive with no inertial data in between, the filter concludes the IMU is absent and falls back to publishing optical orientation directly, resuming blended operation as soon as inertial samples return. If optical data never arrives, position is held at a fixed default height above the tracking origin.

Parameters by concern

Frame and geometry: the mounting alignment rotation, the optical alignment rotation, and the lever-arm offset describe the physical build. They are not tuning knobs; get them right first, because every other parameter behaves badly when they are wrong.

Optical trust: the orientation weight sets the trade-off between smooth inertial output and fast agreement with the optical reference. Quality weighting and the quality threshold gate how strongly, or whether at all, individual optical frames are allowed to contribute.

Smoothing and latency: the Savitzky-Golay half-window and polynomial order shape the angular-velocity estimate; the prediction interval extrapolates orientation forward to hide downstream render latency.

Drift control: tilt correction uses low-pass filtered gravity to hold pitch and roll. When it is enabled, optical correction is restricted to heading only.

Diagnostics: optical pass-through and IMU-frame output bypass or re-express the result and should be off in production.

Tuning and failure modes

Start with the default weight of 0.005 for smooth output and raise it if the fused orientation visibly lags or never catches up after startup; values above roughly 0.05 begin to inject optical jitter. During bring-up, raise it temporarily so initial heading converges in seconds, then lower it. Keep the polynomial order below the total window size, and keep the prediction interval close to the real display latency.

Common failures: a wrong mounting alignment causes the output to twitch back toward optical after each frame while drifting between them; tilt correction expects gravity-normalized accelerometer units and silently stops correcting if the magnitude gate never passes or if the target is accelerating; a quality threshold set too high, or a source reporting binary quality, freezes heading correction during dropouts and lets gyro drift accumulate; long optical outages freeze position, since position comes only from optical; and leaving pass-through enabled makes the node emit raw optical data with no inertial smoothing at all.

Inputs / Outputs

• Inputs: Imu, Optical
• Outputs: FusedPose

Config aliases

ImuOpticalFilter, imuOpticalFilter, ImuOpticalFusion, fusion

Required feature

imu_optical_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": {
    "fusion": {
      "dataEndpoint": "inproc://fusion_data",
      "inputEndpoints": [
        "inproc://imu_data",
        "inproc://optical_data"
      ],
      "inputDataFilter": [
        "Imu",
        "Optical"
      ],
      "settings": {
        "SensorFusion": {
          "alignment": {
            "w": 1.0,
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
          },
          "opticalAlignment": {
            "w": 1.0,
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
          },
          "opticalQualityThreshold": 0.0,
          "opticalVector": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
          },
          "orientationWeight": 0.005,
          "predictionIntervalMs": 0,
          "sggPointsEachSide": 5,
          "sggPolynomialOrder": 5,
          "tiltCorrection": "off",
          "useOpticalQuality": false
        },
        "outputInImuFrame": false,
        "passthroughOptical": false
      }
    }
  }
}

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.