Full 6-DOF Fusion

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

Description

Unscented Kalman filter fusing IMU with optical tracking into a full 6-DOF pose: position, velocity, orientation, gyro and accelerometer bias, and gravity. Optical measurements are latency-compensated through a delayed-measurement scheme, so it tracks position as well as orientation.

Algorithm notes

What the filter estimates

This node runs an unscented Kalman filter that fuses a single inertial stream with a single optical 6-DOF stream into one pose. The estimated state has 18 dimensions: position, linear velocity, orientation (held internally as a three-parameter rotation vector rather than a quaternion), gyroscope bias, accelerometer bias, and the local gravity vector. Enabling online offset estimation adds three more dimensions for the body-frame vector from the tracked optical target to the IMU.

All estimation happens in the frame the optical tracker reports in. Incoming inertial samples are rotated into that frame by the configured mounting rotation. Internally the state follows the IMU, because that is where specific force is measured; on output the configured or estimated target-to-IMU offset is removed so the reported pose describes the optical target, with velocity corrected for the rotational contribution of that offset. A final reference transform (rotation plus offset) is applied to the published pose so it can be expressed at a point of interest instead of at the target.

Optical frames are fused through a delayed-measurement scheme. At each instant where a frame is expected to have been captured, the filter records a snapshot of position and orientation inside the augmented covariance, keeping the correlations between that snapshot and the evolving present state. When the frame actually arrives, it is matched to the nearest pending snapshot and fused there; the correction propagates forward to the current estimate through those correlations. Nothing is rewound or replayed.

How the streams are used

The inertial stream drives every prediction step and every published pose, so output rate equals IMU rate. Sample period is taken from the stream and, if missing, derived from timestamps with implausible values clamped. The first sample seeds the accelerometer bias from the sensed gravity direction. Reported angular velocity is bias-corrected gyroscope data smoothed by a polynomial fit over eleven samples, which adds a small, symmetric lag.

The first optical frame anchors position, orientation and the gravity direction; before it arrives nothing is published as tracked. Bias states start effectively frozen and are released only after an optical measurement has been fused, gravity magnitude is within about ten percent of standard gravity, and speed is low. Once released, biases converge over seconds; implausible values force a re-freeze and re-zero. If an optical orientation disagrees with the prediction by more than roughly 45 degrees, the pose is re-anchored to it, but any learned target-to-IMU offset is carried across so its convergence is not restarted. A stream discontinuity or an explicit reset command re-anchors and clears the timebase.

What the parameters control

Geometry parameters (mounting rotation, output reference transform, target-to-IMU offset, its prior width and whether it is estimated) fix how sensor and output frames relate. Online offset estimation is only observable while the rig rotates; without rotation it simply stays near the entered value.

Optical timing parameters (frame rate, expected latency, timing window, snapshot expiry) determine when snapshots are scheduled and which arriving frame is matched to which snapshot. The per-frame latency reported by the source is what matching actually uses; the configured values only shape scheduling. The matching window is additionally capped at half a reported frame interval.

Noise parameters set the trust balance: optical position and orientation sigmas against gyroscope and accelerometer sigmas. Effective optical noise is inflated automatically according to how far the body moves within the latency uncertainty, so fast motion is trusted less. Integration options (fourth-order versus first-order stepping, antipodal rotation threshold) affect accuracy during fast motion and near half-turn rotations. The dead-reckoning limit bounds how long position is integrated inertially after optical data stops; beyond it, position is held and only orientation tracks.

Tuning and failure modes

Keep defaults and change one group at a time. Raise the optical position sigma or lower accelerometer noise to smooth tracker jitter; do the reverse to follow the tracker tightly. Setting gyroscope noise too low makes the filter overconfident and pushes residual error into the bias estimate.

Watch the accepted-versus-rejected optical counters. Persistent rejection means the timing configuration disagrees with what the source reports. If snapshot expiry is shorter than latency plus one frame interval, measurements arrive after their snapshot is gone and the node silently degrades to inertial-only with drifting position; a warning is logged at startup for this case. A window wide relative to the frame interval can lock matching onto a shifted snapshot during start-up, producing an orientation error that grows with angular rate.

Other common issues: a wrong mounting rotation causes repeated re-anchoring and acceleration entering the wrong axes; a rig that never rests keeps biases frozen; long occlusions produce drift up to the dead-reckoning limit and a visible jump on reacquisition.

Inputs / Outputs

• Inputs: Imu, Optical
• Outputs: FusedPose, FusionStateInt

Config aliases

FullFusionFilter, fullFusionFilter, fullFusion

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": {
    "fullFusion": {
      "dataEndpoint": "inproc://fullFusion_data",
      "inputEndpoints": [
        "inproc://imu_data",
        "inproc://optical_data"
      ],
      "inputDataFilter": [
        "Imu",
        "Optical"
      ],
      "settings": {
        "accErrorMS2": 0.3,
        "antipodalThreshold": 0.9,
        "deadReckonMaxTimeMS": 500,
        "estimateLeverArm": false,
        "gyroErrorDegS": 0.1,
        "imuToOpticalFrameQuat": {
          "w": 1.0,
          "x": 0.0,
          "y": 0.0,
          "z": 0.0
        },
        "leverArmM": {
          "x": 0.0,
          "y": 0.0,
          "z": 0.0
        },
        "leverArmSigmaM": 0.05,
        "opticalExpiryAgeMS": null,
        "opticalFPS": 90.0,
        "opticalLatencyMS": 25,
        "opticalTimingWindowMS": 11,
        "posErrorM": 0.001,
        "referenceToOpticalFrameQuat": {
          "w": 1.0,
          "x": 0.0,
          "y": 0.0,
          "z": 0.0
        },
        "referenceToOpticalFrameVec": {
          "x": 0.0,
          "y": 0.0,
          "z": 0.0
        },
        "rotErrorRad": 0.01,
        "useRK4": 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.