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

What the filter estimates

The node runs an unscented Kalman filter over a 20-element state: position, velocity, local gravity magnitude, IMU-to-world attitude (stored as modified Rodrigues parameters, which avoids the singularities of Euler angles), gyroscope bias, accelerometer bias, the GNSS antenna lever arm relative to the IMU, and the residual GNSS-IMU time offset. Position and velocity live in a local East-North-Up frame anchored at a configurable global origin; if none is set, the first fix becomes the origin. Attitude is estimated for the IMU and rotated into vehicle-body terms with the mounting rotation before it is published, so outputs are always vehicle-referenced. Outputs are produced at IMU rate: local pose, planar vehicle pose, WGS84 global pose, the raw internal state, and per-state uncertainties.

How each input is used

IMU samples drive the prediction step and set the output rate. They are pushed through a delay queue keyed on data timestamps so they can be aligned with the slower GNSS stream. Large gaps are bridged by sub-stepping; gaps over a second are skipped rather than dead-reckoned.

GNSS supplies up to four independent corrections: position, Doppler velocity, heading, and pitch. Heading is a yaw-only observation; pitch comes either from receiver-reported dual-antenna pitch (valid even at standstill) or, failing that, from the Doppler velocity vector above a minimum-speed gate. Doppler velocity also sharpens tilt observability and shortens recovery after outages.

Vehicle speed, from a dedicated speed input or decoded from CAN, is used to cross-validate GNSS-derived velocity during startup and to detect standstill.

Initialization needs an origin fix plus an attitude seed. Attitude is seeded from GNSS heading when available; otherwise the filter waits for sustained agreement between GNSS-derived and vehicle speed above the velocity threshold. Gyro and accel biases are seeded from averages collected while stationary, so leaving the vehicle parked for a few seconds before starting improves the seed. A configurable number of early fixes are consumed at full weight before decimation and the pitch outlier gate engage, and a grace period after every reinitialization marks the output as not-yet-converged.

Parameter groups

Noise balance. Accelerometer, gyroscope, GNSS position, heading, pitch, and Doppler sigmas set the relative authority of dead reckoning versus satellite corrections. They are ratios, not absolutes - lowering GNSS position error and raising accelerometer error both mean “trust GNSS more”. Bias random-walk densities let slow thermal drift be tracked instead of the filter freezing an early bias estimate. The reference sample interval makes this tuning independent of IMU rate.

Robustness. A chi-square innovation gate de-weights, rather than discards, suspect position and Doppler updates. Separate angular gates guard heading and pitch against receiver blunders around fix transitions; both release automatically after a few seconds of sustained disagreement, on the assumption that a persistent conflict means the filter, not the receiver, is wrong.

GNSS quality handling. Requiring RTK-fixed quality, inflating sigma with correction age, honoring receiver-reported accuracy, and the hard correction-age ceiling all decide which epochs are trusted. Dropout and settle timers control how long an outage is tolerated before reinitialization, and how long a restored fix must hold before output resumes.

Standstill and leveling. Stopped detection freezes position drift; zero-velocity updates plus an at-rest IMU consistency update converge biases and tilt at every stop. The centripetal-compensated gravity update keeps pitch and roll observable during sustained turns and is gated on the specific-force magnitude.

Geometry. Mounting rotation, antenna lever arm, and the optional online estimation of both must match physical reality, or errors appear as heading-dependent position offsets.

Tuning and failure modes

Start from defaults and change one sigma at a time; watch innovation magnitude and per-state uncertainty rather than the trajectory alone. A jittery track usually means GNSS is over-trusted; a track that lags corners or drifts through tunnels means the IMU is. Enable smoothing to remove the visible step at each fix, at the cost of a small latency.

Known trouble spots: heading is unobservable at standstill without a dual-antenna receiver, so a cold start requires straight-line motion above the velocity threshold. Pure circular driving without a pitch or gravity observation accumulates a small tilt bias. Multipath in urban canyons produces position outliers that the gate softens but cannot eliminate. A wrong mounting rotation or lever arm produces systematic errors that no noise tuning will fix. Persisted state resumed after moving to a different region should be reset before the first run.

Inputs / Outputs

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

Config aliases

GnssImuFilter, gnssImuFilter, gnssImuFusion

Required feature

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

Key	Label	Type	Default	Description / Notes
`_h_coreTuning`	Core Tuning	`heading`	—	—
`accelError`	Acceleration Error	`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.
`omegaError`	Angular Velocity Error	`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.
`measurementError`	GNSS Measurement Error	`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.
`orientationFromGnssError`	Orientation from GNSS Error	`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.
`smoothFit`	Smoothing	`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.
`measurementStep`	Measurement Step	`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.
`_h_outlierHandling`	Outlier Handling	`heading`	—	—
`useGnssPositionOutlierDeweighting`	Use GNSS Position Outlier De-weighting	`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.
`useGnssVelocityOutlierDeweighting`	Use GNSS Velocity Outlier De-weighting	`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.
`innovationGate`	Innovation Gate (sigma)	`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.
`_h_orientation`	Orientation & Heading	`heading`	—	—
`initializeFromGnssOrientation`	Initialize from GNSS Orientation	`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.
`useGnssOrientationMeasurement`	Use GNSS Orientation Measurement	`boolean`	`true`	Continuously use GNSS heading as a measurement update to correct the IMU-derived orientation. Helps prevent heading drift during extended operation.
`transformGnssOrientation`	Transform GNSS Orientation	`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.
`useGnssPitchMeasurement`	Use GNSS Pitch Measurement	`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.
`gnssPitchError`	GNSS Pitch Error	`number`	`0.003`	GNSS pitch measurement noise std-dev in radians, sized for direct dual-antenna pitch from a Unicore receiver.
`gnssPitchMinSpeed`	GNSS Pitch Min Speed (m/s)	`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.
`useGnssPitchOutlierGate`	GNSS Use Pitch Outlier Gate	`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).
`gnssPitchOutlierGateDeg`	GNSS Pitch Outlier Gate (deg)	`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.
`useGnssHeadingOutlierGate`	GNSS Use Heading Outlier Gate	`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.
`gnssHeadingOutlierGateDeg`	GNSS Heading Outlier Gate (deg)	`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.
`_h_gravityVelocity`	Gravity & Velocity Updates	`heading`	—	—
`useGravityMeasurement`	Use Gravity Leveling Measurement	`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.
`gravityMeasurementError`	Gravity Measurement Error	`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).
`gravityMeasurementIntervalMs`	Gravity Measurement Interval (ms)	`number`	`250`	Minimum interval between gravity leveling updates in milliseconds.
`gravityNormTolerance`	Gravity Norm Tolerance (m/s²)	`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.
`useGnssVelocityMeasurement`	Use GNSS Velocity Measurement	`boolean`	`true`	Use the receiver Doppler velocity vector as a direct 3D velocity update. Improves tilt observability and speeds up recovery after dropouts.
`gnssVelocityError`	GNSS Velocity Error	`number`	`0.06`	GNSS Doppler velocity measurement noise std-dev in m/s.
`_h_stopped`	Stopped Detection & ZUPT	`heading`	—	—
`retainStateWhenStopped`	Retain State When Stopped	`boolean`	`true`	Freeze the filter state when the vehicle is detected as stationary. Prevents position drift from IMU noise when not moving.
`isStoppedThreshold`	Is-Stopped Threshold	`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.
`setHeightToZero`	Set Height to Zero	`boolean`	`false`	Constrain the vertical (Up) position component to zero. Useful for ground vehicles operating on a known flat plane to prevent altitude drift.
`enableZupt`	Zero-Velocity Updates When Stopped	`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.
`zuptIntervalMs`	ZUPT Interval (ms)	`number`	`250`	Minimum interval between zero-velocity updates in milliseconds.
`_h_init`	Initialization & Convergence	`heading`	—	—
`nMeasurementInit`	Initialization Measurement Count	`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.
`velocityThreshold`	Velocity Threshold	`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.
`velocityTolerance`	Velocity Tolerance	`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.
`filterConvergenceTimeMs`	Filter Convergence Time (ms)	`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.
`_h_rtk`	RTK & GNSS Quality	`heading`	—	—
`requireRtkFix`	Require RTK Fix	`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.
`useReportedAccuracy`	Use Reported Accuracy	`boolean`	`false`	Use the receiver-reported horizontal accuracy as a floor for the GNSS position measurement error, so degraded epochs are automatically de-weighted.
`useRtcmAgeDeweighting`	Use RTCM Age De-weighting	`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.
`rtcmAgeSigmaPerSec`	RTCM Age Sigma Per Second (m/s)	`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.
`rtcmAgeMaxS`	RTCM Age Hard Limit (s)	`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.
`rtkFixLostTimeoutMs`	RTK Fix Lost Timeout (ms)	`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.
`rtkSettleMs`	RTK Settle After Restore (ms)	`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.
`_h_mounting`	IMU Mounting & Frames	`heading`	—	—
`autoImuToCarRotation`	Auto IMU-to-Car Rotation	`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.
`imuToCarRotation`	IMU-to-Vehicle Rotation	`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.
`estimateGpsAntennaOffset`	Estimate GPS Antenna Offset	`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.
`gpsAntennaOffset`	GPS Antenna Offset	`vector3`	`{"x":0,"y":0,"z":0}`	Initial GPS antenna position offset from the IMU in meters (x=forward, y=left, z=up in vehicle frame). Used as starting value when estimateGpsAntennaOffset is enabled, or as fixed offset when disabled.
`globalOrigin`	Global Origin	`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.
`_h_advanced`	Timing & Advanced Process Noise	`heading`	—	—
`imuQueueDelayMs`	IMU Queue Delay (ms)	`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.
`estimateTimeDelay`	Estimate GNSS-IMU Time Delay	`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.
`omegaBiasRandomWalk`	Gyro Bias Random Walk	`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.
`accelBiasRandomWalk`	Accel Bias Random Walk	`number`	`0.003`	Random-walk process noise density for the accelerometer bias states in m/s² per sqrt(s). 0 disables.
`controlNoiseRefDt`	Control Noise Reference dt (s)	`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.
`_h_output`	Output & Diagnostics	`heading`	—	—
`outputWhenFilterNotReady`	Output When Filter Not Ready	`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.
`outputRawGnssData`	Output Raw GNSS Data	`boolean`	`false`	Include raw GNSS position data alongside fused output for comparison and debugging.
`debugFilter`	Debug Filter	`boolean`	`false`	Enable verbose debug logging for the fusion filter. Logs prediction/measurement steps, innovations, and state updates.
`persistFilterState`	Persist Filter State	`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.
`resetFilterState`	Reset Filter State	`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 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": {
    "gnssImuFusion": {
      "dataEndpoint": "inproc://gnssImuFusion_data",
      "inputEndpoints": [
        "inproc://gnss_data",
        "inproc://imu_data",
        "inproc://vehicleSpeed_data"
      ],
      "inputDataFilter": [
        "Gnss",
        "Imu",
        "VehicleSpeed"
      ],
      "settings": {
        "accelBiasRandomWalk": 0.003,
        "accelError": 0.2,
        "autoImuToCarRotation": false,
        "controlNoiseRefDt": 0.01,
        "debugFilter": false,
        "enableZupt": true,
        "estimateGpsAntennaOffset": true,
        "estimateTimeDelay": false,
        "filterConvergenceTimeMs": 3000,
        "globalOrigin": null,
        "gnssHeadingOutlierGateDeg": 10.0,
        "gnssPitchError": 0.003,
        "gnssPitchMinSpeed": 0.3,
        "gnssPitchOutlierGateDeg": 6.0,
        "gnssVelocityError": 0.06,
        "gpsAntennaOffset": {
          "x": 0,
          "y": 0,
          "z": 0
        },
        "gravityMeasurementError": 0.09,
        "gravityMeasurementIntervalMs": 250,
        "gravityNormTolerance": 0.5,
        "imuQueueDelayMs": 0,
        "imuToCarRotation": {
          "w": 1,
          "x": 0,
          "y": 0,
          "z": 0
        },
        "initializeFromGnssOrientation": true,
        "innovationGate": 6.0,
        "isStoppedThreshold": 0.005,
        "measurementError": 0.07,
        "measurementStep": 1,
        "nMeasurementInit": 100,
        "omegaBiasRandomWalk": 0.0005,
        "omegaError": 0.04,
        "orientationFromGnssError": 0.006,
        "outputRawGnssData": false,
        "outputWhenFilterNotReady": false,
        "persistFilterState": false,
        "requireRtkFix": true,
        "resetFilterState": "resetFilterState",
        "retainStateWhenStopped": true,
        "rtcmAgeMaxS": 30.0,
        "rtcmAgeSigmaPerSec": 0.05,
        "rtkFixLostTimeoutMs": 2000,
        "rtkSettleMs": 5000,
        "setHeightToZero": false,
        "smoothFit": true,
        "transformGnssOrientation": true,
        "useGnssHeadingOutlierGate": true,
        "useGnssOrientationMeasurement": true,
        "useGnssPitchMeasurement": true,
        "useGnssPitchOutlierGate": true,
        "useGnssPositionOutlierDeweighting": true,
        "useGnssVelocityMeasurement": true,
        "useGnssVelocityOutlierDeweighting": true,
        "useGravityMeasurement": true,
        "useReportedAccuracy": false,
        "useRtcmAgeDeweighting": true,
        "velocityThreshold": 3.0,
        "velocityTolerance": 0.4,
        "zuptIntervalMs": 250
      }
    }
  }
}

Field reference

Field	Purpose
`dataEndpoint`	Endpoint this node binds to publish its output. Omit to let the runtime generate one.
`inputEndpoints`	List of upstream endpoints this node connects to (`outEndpoint` / `dataEndpoint` values from upstream nodes).
`inputDataFilter`	Optional whitelist of data types. Messages whose type isn’t in the list are dropped at the subscriber.
`settings`	All user-configurable fields — see the Properties table above.

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.

Loading documentation…