GNSS-IMU Fusion

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

Description

20-state Unscented Kalman Filter (UKF) fusing GNSS position and heading with IMU accelerometer/gyroscope data for outdoor vehicle navigation. State vector: 3D position (ENU), 3D velocity, gravity, 3D orientation (MRP), 3D gyro bias, 3D accel bias, 3D GPS antenna offset, GNSS-IMU time delay. Supports RTK fix monitoring, RTS smoothing, stopped-state detection, and online estimation of GPS antenna lever arm and time delay. Outputs FusedPose (local), FusedVehiclePoseV2, GlobalFusedPose (WGS84), FusionStateInt, and FusionDiagnostics.

Algorithm notes

How it works

The filter combines an RTK GNSS receiver with an IMU. The IMU carries the motion between GNSS fixes and sets the output rate: every IMU sample produces a fused pose. Each GNSS fix pulls the solution back to the true position, and GNSS heading, pitch and velocity keep the orientation correct. On the side the filter continuously calibrates the gyroscope and accelerometer biases and, if enabled, the GPS antenna offset and the GNSS-IMU time delay. Output is the pose in a local east/north/up frame plus global WGS84 coordinates.

What you must configure

IMU-to-Vehicle Rotation - how the IMU is mounted in the vehicle. A wrong value shows up as a constant heading or tilt error that no amount of tuning removes. Auto IMU-to-Car Rotation can determine pitch and roll automatically while the vehicle is parked on level ground.
GPS Antenna Offset - the antenna position relative to the IMU in the vehicle frame (x forward, y left, z up). Measure it, or leave Estimate GPS Antenna Offset on and drive a few turns so the filter can find it.
Global Origin (optional) - fixes the origin of the local frame. If unset, the first GNSS fix is used.

How it starts

The filter waits for an RTK Fixed position (unless Require RTK Fix is off) and a heading. With a dual-antenna receiver the heading is available immediately; with a single antenna you must drive straight, faster than Velocity Threshold (3 m/s by default), until the heading can be taken from the direction of travel. The filter then applies Initialization Measurement Count fixes before it reports itself converged.

Tuning

The defaults work for most vehicles. If you adjust anything:

GNSS Measurement Error against Acceleration Error and Angular Velocity Error sets the balance between GNSS and IMU. Lower a value to trust that sensor more.
Smoothing removes the small position step visible at each GNSS fix, at the cost of a slight delay. Prefer it over lowering GNSS Measurement Error.
Leave the outlier and gate settings at their defaults - they reject GNSS glitches automatically.

Troubleshooting

No output at all - the filter has not initialized. Check that RTK Fixed is reached, and with a single antenna that the vehicle actually drove above the initialization speed.
Constant heading or tilt offset - wrong IMU-to-Vehicle Rotation.
Position error that changes with driving direction - wrong or missing GPS Antenna Offset.
Output keeps stopping and restarting - RTK is dropping in and out. Improve reception, or turn Require RTK Fix off if reduced accuracy is acceptable.
Wrong position after moving the vehicle with Persist Filter State on - press Reset Filter State.

Inputs / Outputs

Inputs: Gnss, Imu, VehicleSpeed
Outputs: FusedPose, FusedVehiclePoseV2, GlobalFusedPose, FusionStateInt, FusionDiagnostics

Config aliases

GnssImuFilter, gnssImuFilter, gnssImuFusion

Required feature

vehicular_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.

Core Tuning

Property	Type	Default	Description
Acceleration Error `accelError`	number	`0.2`	Accelerometer measurement noise std-dev in m/s². Controls how much the filter trusts IMU acceleration readings. Lower values trust the accelerometer more; higher values rely more on GNSS position updates.
Angular Velocity Error `omegaError`	number	`0.04`	Gyroscope measurement noise std-dev in rad/s. Controls how much the filter trusts IMU gyroscope readings. Lower values trust the gyroscope more for orientation estimation; higher values allow GNSS orientation to dominate.
GNSS Measurement Error `measurementError`	number	`0.07`	GNSS position measurement noise std-dev in meters. Controls how much the filter trusts GNSS position fixes. Lower values trust GNSS more; higher values let IMU dead-reckoning carry more weight.
Orientation from GNSS Error `orientationFromGnssError`	number	`0.006`	GNSS-derived heading measurement noise std-dev. Controls how strongly GNSS heading corrections influence the fused orientation. Lower values give GNSS heading more authority.
Smoothing `smoothFit`	boolean	`true`	Smooth the output by interpolating between the lagged and current filter states across measurement epochs. Removes the visible position step at each GNSS update at the cost of a small effective latency.
Measurement Step `measurementStep`	number	`1`	GNSS measurement decimation: after the initialization phase only every N-th GNSS sample creates measurement updates. A value of 1 uses every sample; higher values reduce computational load.

Outlier Handling

Property	Type	Default	Description
Use GNSS Position Outlier De-weighting `useGnssPositionOutlierDeweighting`	boolean	`true`	When a GNSS position update exceeds the innovation gate, inflate its measurement noise and apply it softly instead of applying it at full weight.
Use GNSS Velocity Outlier De-weighting `useGnssVelocityOutlierDeweighting`	boolean	`true`	When a GNSS Doppler velocity update exceeds the innovation gate, inflate its measurement noise and apply it softly instead of applying it at full weight. Only applies when GNSS velocity measurement is enabled.
Innovation Gate (sigma) `innovationGate`	number	`6.0`	Chi-square threshold for enabled outlier handling: a measurement whose squared Mahalanobis distance exceeds dof + gate x sqrt(2 x dof) is treated as an outlier. 0 disables position/velocity outlier de-weighting.

Orientation & Heading

Property	Type	Default	Description
Initialize from GNSS Orientation `initializeFromGnssOrientation`	boolean	`true`	Use the initial GNSS-derived heading to set the filter’s starting orientation. This avoids the need to estimate heading from velocity during the first seconds of motion.
Use GNSS Orientation Measurement `useGnssOrientationMeasurement`	boolean	`true`	Continuously use GNSS heading as a measurement update to correct the IMU-derived orientation. Helps prevent heading drift during extended operation.
Transform GNSS Orientation `transformGnssOrientation`	boolean	`true`	Apply the IMU-to-car rotation when interpreting GNSS orientation data. Enable when the GNSS heading is in the vehicle frame but the IMU is mounted at a different orientation.
Use GNSS Pitch Measurement `useGnssPitchMeasurement`	boolean	`true`	Apply GNSS-derived pitch as a measurement update for the filter’s tilt estimate. Uses receiver-supplied dual-antenna pitch (Unicore KSXT) when present; otherwise uses receiver Doppler velocity (gated by gnssPitchMinSpeed). Default on - harmless when no pitch source is available.
GNSS Pitch Error `gnssPitchError`	number	`0.003`	GNSS pitch measurement noise std-dev in radians, sized for direct dual-antenna pitch from a Unicore receiver.
GNSS Pitch Min Speed (m/s) `gnssPitchMinSpeed`	number	`0.3`	Minimum vehicle speed in m/s before Doppler-velocity-derived GNSS pitch is used. Receiver Doppler is accurate at very low speed, so the gate only suppresses the degenerate near-zero case. Does not affect direct dual-antenna pitch.
GNSS Use Pitch Outlier Gate `useGnssPitchOutlierGate`	boolean	`true`	Reject GNSS pitch measurements that disagree with the IMU-propagated tilt by more than the gate, suppressing dual-antenna pitch blunders around RTK fix transitions (still flagged quality-4).
GNSS Pitch Outlier Gate (deg) `gnssPitchOutlierGateDeg`	number	`6.0`	Maximum disagreement in degrees between a GNSS pitch measurement and the IMU-propagated tilt before the measurement is rejected. Only used when GNSS Use Pitch Outlier Gate is on.
GNSS Use Heading Outlier Gate `useGnssHeadingOutlierGate`	boolean	`true`	Reject GNSS heading (orientation) measurements that disagree with the gyro-propagated heading by more than the gate, suppressing dual-antenna heading blunders around RTK fix transitions. A sustained outage re-seeds heading via the init path, which bypasses this gate.
GNSS Heading Outlier Gate (deg) `gnssHeadingOutlierGateDeg`	number	`10.0`	Maximum disagreement in degrees between a GNSS heading measurement and the gyro-propagated heading before the measurement is rejected. Only used when GNSS Use Heading Outlier Gate is on.

Gravity & Velocity Updates

Property	Type	Default	Description
Use Gravity Leveling Measurement `useGravityMeasurement`	boolean	`true`	Centripetal-compensated gravity-direction update from the accelerometer. Subtracting omega x v removes the turn acceleration, so pitch AND roll stay observable even during sustained circular driving. Rejected automatically during longitudinal acceleration transients.
Gravity Measurement Error `gravityMeasurementError`	number	`0.09`	Per-component noise std-dev of the gravity direction measurement (unitless, ~sin of the angle error; e.g. 0.02 corresponds to about 1.1 degrees).
Gravity Measurement Interval (ms) `gravityMeasurementIntervalMs`	number	`250`	Minimum interval between gravity leveling updates in milliseconds.
Gravity Norm Tolerance (m/s²) `gravityNormTolerance`	number	`0.5`	Reject the gravity measurement when the centripetal-compensated specific-force norm deviates from g by more than this. Filters out acceleration/braking transients and bumps.
Use GNSS Velocity Measurement `useGnssVelocityMeasurement`	boolean	`true`	Use the receiver Doppler velocity vector as a direct 3D velocity update. Improves tilt observability and speeds up recovery after dropouts.
GNSS Velocity Error `gnssVelocityError`	number	`0.06`	GNSS Doppler velocity measurement noise std-dev in m/s.

Stopped Detection & ZUPT

Property	Type	Default	Description
Retain State When Stopped `retainStateWhenStopped`	boolean	`true`	Freeze the filter state when the vehicle is detected as stationary. Prevents position drift from IMU noise when not moving.
Is-Stopped Threshold `isStoppedThreshold`	number	`0.005`	Velocity magnitude threshold in m/s below which the vehicle is considered stationary. Used for stopped-state detection and optional state freezing.
Set Height to Zero `setHeightToZero`	boolean	`false`	Constrain the vertical (Up) position component to zero. Useful for ground vehicles operating on a known flat plane to prevent altitude drift.
Zero-Velocity Updates When Stopped `enableZupt`	boolean	`true`	While the vehicle is stopped, apply a zero-velocity pseudo-measurement plus an IMU at-rest consistency update. Converges gyro/accel biases and tilt at every stop.
ZUPT Interval (ms) `zuptIntervalMs`	number	`250`	Minimum interval between zero-velocity updates in milliseconds.

Initialization & Convergence

Property	Type	Default	Description
Initialization Measurement Count `nMeasurementInit`	number	`100`	Number of measurement updates during the initialization phase before switching to normal operation. More samples give a better initial state estimate but delay filter readiness.
Velocity Threshold `velocityThreshold`	number	`3.0`	Minimum vehicle speed in m/s required to initialize the filter heading from GNSS course-over-ground. Below this speed, heading is ambiguous and initialization is deferred.
Velocity Tolerance `velocityTolerance`	number	`0.4`	Maximum allowed difference in m/s between GNSS-derived velocity and vehicle speed (CAN/odometry) during cross-validation at initialization. Larger tolerance accepts noisier speed sources.
Filter Convergence Time (ms) `filterConvergenceTimeMs`	number	`3000`	Grace period in milliseconds after reinitialization during which the filter output is considered not yet converged. The convergence flag will be false during this time.

RTK & GNSS Quality

Property	Type	Default	Description
Require RTK Fix `requireRtkFix`	boolean	`true`	Only accept GNSS measurements with RTK Fixed quality (quality=4). When enabled, float or standalone fixes are rejected to ensure centimeter-level accuracy.
Use Reported Accuracy `useReportedAccuracy`	boolean	`false`	Use the receiver-reported horizontal accuracy as a floor for the GNSS position measurement error, so degraded epochs are automatically de-weighted.
Use RTCM Age De-weighting `useRtcmAgeDeweighting`	boolean	`true`	Inflate the GNSS position measurement noise with the receiver’s RTCM correction age, so an RTK-fixed fix on stale corrections is smoothly de-weighted instead of hard-rejected. When off, RTK-fixed fixes are trusted regardless of correction age.
RTCM Age Sigma Per Second (m/s) `rtcmAgeSigmaPerSec`	number	`0.05`	Per-second growth (in meters) of GNSS position sigma when the receiver reports RTK FIXED with a non-zero RTCM correction age. Effective measurement error becomes `measurementError + diffAge * rtcmAgeSigmaPerSec`. Only used when Use RTCM Age De-weighting is on.
RTCM Age Hard Limit (s) `rtcmAgeMaxS`	number	`30.0`	Hard ceiling on RTCM correction age in seconds. A sample reported as RTK FIXED with older corrections is treated as fix loss and handled by the normal dropout logic. 0 disables the ceiling.
RTK Fix Lost Timeout (ms) `rtkFixLostTimeoutMs`	number	`2000`	Time in milliseconds after losing RTK fix before the filter triggers a reinitialization sequence. Allows brief RTK outages without disrupting the fused solution.
RTK Settle After Restore (ms) `rtkSettleMs`	number	`5000`	After a sustained RTK outage, wait this many milliseconds of uninterrupted RTK-FIXED samples before the filter reinitialises and resumes emitting. Avoids the first noisy epochs right after a reacquire.

IMU Mounting & Frames

Property	Type	Default	Description
Auto IMU-to-Car Rotation `autoImuToCarRotation`	boolean	`false`	Automatically compute the IMU-to-car rotation from the accelerometer during initialization. Assumes the car is parked on a level surface so that any accelerometer tilt is the IMU mounting offset. Only determines pitch/roll; yaw is handled by GNSS heading.
IMU-to-Vehicle Rotation `imuToCarRotation`	quaternion	`{"w":1,"x":0,"y":0,"z":0}`	Quaternion (w,x,y,z) describing the rotation from IMU sensor frame to vehicle body frame. Accounts for the physical mounting orientation of the IMU relative to the vehicle.
Estimate GPS Antenna Offset `estimateGpsAntennaOffset`	boolean	`true`	Estimate the GPS antenna lever arm (position offset from IMU to antenna) as part of the Kalman filter state. Adds 3 states (x/y/z offset in meters) to the state model. The emitted position stays at the antenna regardless of the estimate.
GPS Antenna Offset `gpsAntennaOffset`	vector3	`{"x":0,"y":0,"z":0}`	Vector from the reference IMU to the GNSS antenna in the vehicle frame (x=forward, y=left, z=up), in meters. Improves the internal lever-arm modeling only; the emitted position is always the antenna point. Starting value when estimateGpsAntennaOffset is enabled, fixed offset when disabled.
Global Origin `globalOrigin`	latlon	—	Reference point for converting GNSS WGS84 coordinates to local East-North-Up (ENU) frame. Accepts latitude, longitude, and optional altitude (meters). If not set, the first GNSS fix is used as origin.

Timing & Advanced Process Noise

Property	Type	Default	Description
IMU Queue Delay (ms) `imuQueueDelayMs`	number	`0`	Delay in milliseconds applied to incoming IMU data for time synchronization with GNSS, measured on data timestamps. Compensates for different arrival times of IMU and GNSS messages.
Estimate GNSS-IMU Time Delay `estimateTimeDelay`	boolean	`false`	Let the filter estimate the time offset between GNSS and IMU clocks as an additional state variable. Adds online time-delay calibration to the Kalman filter state.
Gyro Bias Random Walk `omegaBiasRandomWalk`	number	`0.0005`	Random-walk process noise density for the gyro bias states in rad/s per sqrt(s). Lets the filter track slow (thermal) gyro bias drift instead of becoming overconfident. 0 disables.
Accel Bias Random Walk `accelBiasRandomWalk`	number	`0.003`	Random-walk process noise density for the accelerometer bias states in m/s² per sqrt(s). 0 disables.
Control Noise Reference dt (s) `controlNoiseRefDt`	number	`0.01`	Reference IMU sample interval for process-noise scaling. When set, the per-step control sigma is scaled by sqrt(refDt/dt) so tuning is independent of the IMU rate. 0 keeps the legacy fixed per-sample sigma.

Output & Diagnostics

Property	Type	Default	Description
Output When Filter Not Ready `outputWhenFilterNotReady`	boolean	`false`	Emit fused pose output even before the filter has fully initialized and converged. Useful for diagnostics but the output quality will be poor.
Output Raw GNSS Data `outputRawGnssData`	boolean	`false`	Include raw GNSS position data alongside fused output for comparison and debugging.
Debug Filter `debugFilter`	boolean	`false`	Enable verbose debug logging for the fusion filter. Logs prediction/measurement steps, innovations, and state updates.
Persist Filter State `persistFilterState`	boolean	`false`	Save the Kalman filter state (state vector, covariance, ENU origin, init flags) to disk and restore it on the next start. Lets the filter resume warm instead of re-converging from scratch. State is keyed by node name and stored alongside the gyroscope autocalibration file.
Reset Filter State `resetFilterState`	button	`resetFilterState`	Wipe the persisted filter state and reset the filter to its initial values. Use after moving the vehicle to a new region or when the saved state is stale.

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": {
    "gnssImuFusion": {
      "dataEndpoint": "inproc://gnssImuFusion_data",
      "inputEndpoints": [
        "inproc://gnss_data",
        "inproc://imu_data",
        "inproc://vehicleSpeed_data"
      ],
      "inputDataFilter": [
        "Gnss",
        "Imu",
        "VehicleSpeed"
      ],
      "settings": {
        "imuToCarRotation": {
          "w": 1,
          "x": 0,
          "y": 0,
          "z": 0
        },
        "gpsAntennaOffset": {
          "x": 0,
          "y": 0,
          "z": 0
        }
      }
    }
  }
}

Loading documentation…