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

How it works

This node blends a high-rate IMU with a lower-rate optical tracker into a single smooth 6-DOF pose. The IMU carries the pose between optical frames and fills in fast motion; the optical tracker supplies the absolute position and orientation that keeps the result from drifting. Because optical frames always arrive a little after the moment they describe, the node remembers what its state looked like at the instant each frame was captured and corrects that instant, then carries the correction forward. Along the way it learns the gyroscope and accelerometer biases and the local gravity direction, so accuracy improves over the first seconds of use.

What you must configure

IMU to Optical Frame must describe how the IMU is mounted relative to the tracked target. If it is wrong, the two sensors disagree about which way the rig is turning and the pose will shake or run away during motion.

Target to IMU Offset is the distance from the tracked optical target to the IMU, in the body frame. Without it, every rotation of the rig injects position error. Measure it from CAD or by hand; centimetre accuracy is enough. If you cannot measure it, enable Estimate Offset Automatically and rotate the rig for a few seconds - rotation is what makes the offset observable. Tighten Offset Prior (m) when you know the value well and only want it refined.

Optical Frame Rate (Hz) and Optical Latency (ms) must match the real tracker. They decide when the node takes its snapshots. Reference to Optical Frame and Reference to Optical Offset are optional: use them to report a point other than the tracked target, for example an eye point or a tool tip.

How it starts

Nothing is output until the first optical frame arrives - that frame anchors the pose. From then on, one pose is emitted per IMU sample. Biases stay frozen until gravity looks right and the rig is nearly still, so leave it stationary briefly after start-up. A large orientation jump against the optical data re-anchors the pose automatically.

Tuning

• Optical Position Error (m) / Optical Orientation Error (rad): raise to smooth a jittery tracker, lower to follow it more tightly.
• Gyro Noise (deg/s) / Accelerometer Noise (m/s^2): raise if the pose looks over-confident between optical frames or the biases wander.
• Optical Timing Window (ms): raise if frames are being rejected, lower if the timing is noisy.
• Dead Reckoning Max Time (ms): raise to coast further through occlusions, lower to freeze position sooner.
• Use RK4 Integration: turn on for very fast motion, at extra CPU cost.

Troubleshooting

• Position drifts or lags while orientation is fine - optical frames are not being fused. Check Optical Latency (ms) and Optical Frame Rate (Hz) against the tracker, and that Optical Expiry Age (ms) is longer than the latency plus one frame (leave it empty to derive it).
• Pose swims or overshoots when the rig turns - Target to IMU Offset is wrong or IMU to Optical Frame does not match the mounting.
• Output is jittery - lower the trust in the tracker by raising Optical Position Error (m).
• Pose reacts sluggishly to real motion - Optical Position Error (m) or the noise values are too high.
• Nothing is output - no optical data has arrived yet, or no IMU data is reaching the node.
• Position freezes during occlusions - expected past Dead Reckoning Max Time (ms); raise it if your gaps are longer.

Inputs / Outputs

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

Config aliases

FullFusionFilter, fullFusionFilter, fullFusion

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.

Optical Tracker

Noise Model

Lever Arm

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": {
    "fullFusion": {
      "dataEndpoint": "inproc://fullFusion_data",
      "inputEndpoints": [
        "inproc://imu_data",
        "inproc://optical_data"
      ],
      "inputDataFilter": [
        "Imu",
        "Optical"
      ],
      "settings": {}
    }
  }
}