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

How it works

This node blends a fast IMU with a slower optical tracking system into one smooth 6-DOF pose. Orientation comes mainly from the gyroscope, which is smooth and low-latency, and is pulled gently toward the optical orientation so it does not drift away over time. Position comes from the optical system. Because the two sensors sit in different frames, the node needs to know how the IMU is mounted relative to the tracked optical target before the blend makes sense.

What you must configure

Alignment is the rotation from the IMU body into the optical target’s frame. Get this wrong and the fused orientation will fight the optical reference, drift, or rotate about the wrong axes. Optical Alignment rotates the incoming optical orientation into the frame you want to output. Optical Vector is the distance in meters from the IMU to the optical target, expressed in the IMU frame; set it so the reported point stays put when the body rotates in place.

Turn on Passthrough Optical (debug) to check the optical target’s own alignment in the tracking system, with the IMU removed from the picture. Turn it off again for normal operation.

How it starts

The node needs both streams. On the first IMU sample it uses gravity to level the orientation, then integrates from there. Until any optical data arrives, the reported position is a fixed point about 1.9 m above the tracking origin - that is expected, not a fault. If only optical data arrives, the node passes the optical pose through after a few frames.

Tuning

• Orientation Weight is the main knob. Small values (the 0.005 default) give a very smooth output that corrects slowly; raise it if the orientation lags or drifts, lower it if optical jitter shows in the output.
• Prediction Interval (ms) compensates for display or render latency. Raise it until motion feels in sync, and no further - too much causes overshoot on fast turns.
• Tilt Correction on stops slow pitch and roll drift using gravity. Use it for static or slow-moving targets. With it on, the optical reference only corrects heading.
• SGG Points Each Side and SGG Polynomial Order control angular-velocity smoothing. More points means smoother but later; a higher order keeps sharp motion but rejects less noise. Keep the order below twice the point count plus one. The defaults are fine for most setups.
• Weight by Optical Quality and Optical Quality Threshold only help with sources that report a meaningful, continuous quality value. Leave them off for trackers that report only 0 or 1.

For optical dropouts, enable Inside-Out Fallback and wire a second pose source into the node. Optical Timeout (s) sets how long the optical stream may be silent before the fallback takes over; shorten it for faster handover, lengthen it if brief gaps trigger it needlessly.

Troubleshooting

• Position sits at a fixed height and never moves - no optical data is reaching the node; check the optical source and its wiring.
• Orientation slowly drifts away from reality - raise Orientation Weight, or enable Tilt Correction for pitch and roll.
• Output jitters in step with the optical tracker - lower Orientation Weight.
• Reported point swings when the body rotates in place - Optical Vector is wrong or unset.
• Fused orientation disagrees with the tracking system - verify with Passthrough Optical (debug); if the optical pose alone is correct, Alignment is wrong.
• Fallback never engages - the second pose source is not connected, is stale, or has not yet seen enough motion to align its frame.
• Output feels ahead of the real motion - reduce Prediction Interval (ms).

Inputs / Outputs

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

Config aliases

ImuOpticalFilter, imuOpticalFilter, ImuOpticalFusion, fusion

Required feature

imu_optical_fusion

Properties

Each row shows the setting’s label in the node’s Properties panel and, in code, its key in the node’s settings object in the config file. A setting omitted from the config file uses the default shown here.

Sensor Fusion

Example node definition

A minimal entry in config.json looks like the block below. Paste it under the sinks key, keyed by an instance name of your choice. All settings are optional: anything not listed under settings uses the default from the Properties table above. See The configuration file for how the sections and endpoint wiring work.

{
  "sinks": {
    "fusion": {
      "dataEndpoint": "inproc://fusion_data",
      "inputEndpoints": [
        "inproc://imu_data",
        "inproc://optical_data",
        "inproc://fusedPose_data"
      ],
      "inputDataFilter": [
        "Imu",
        "Optical",
        "FusedPose"
      ],
      "settings": {}
    }
  }
}