v0.2.0 released: quaternion state, full NCLT benchmark, second encoder input

[manankharwar]

FusionCore v0.2.0 is the biggest update since launch. Here is what changed and why each decision was made.

Quaternion orientation state

The filter previously tracked orientation as Euler angles. This caused gimbal lock during aggressive maneuvers and numerical drift during extended runs. The state is now a unit quaternion (QW, QX, QY, QZ). The measurement functions, motion model, and sigma point propagation all operate on the quaternion directly. Gimbal lock is gone by construction.

Static bias initialization window

Gyro and accelerometer biases now converge within the first few seconds of motion rather than drifting for minutes. The filter accumulates a short stationary window at startup to get an initial bias estimate before the robot starts moving. Real hardware testing showed this made a measurable difference in heading accuracy for the first 30 seconds of a run.

Adaptive noise floor

The adaptive covariance estimation can now shrink R below the configured sensor specs under low-noise conditions. That caused occasional filter overconfidence. A minimum floor is now enforced so the adapted covariance cannot drop below the values you configured. Stability improvement on long runs.

Second encoder input

You can now fuse a second wheel encoder or twist source independently via encoder2.topic. Contributed by @cedbossneo, who needed this for a differential drive with redundant odometry. Same measurement model as the primary encoder.

GNSS covariance floor

U-blox and similar receivers sometimes report near-zero covariance (essentially claiming centimeter accuracy when they are not in RTK mode). This caused self-rejection via the chi-squared gate. Contributed by @cedbossneo. The covariance is now floored at 2 cm sigma XY, 5 cm sigma Z before it enters the filter.

GNSS fix quality gating

gnss.min_fix_type lets you reject fixes below a quality threshold: GPS, DGPS, RTK_FLOAT, RTK_FIXED. If your application requires RTK, set it and the filter will not fuse lower-quality fixes.

Full NCLT benchmark

Six sequences, FusionCore vs robot_localization EKF and UKF, RTK GPS ground truth, reproducible in one command from the repo. RL-UKF diverged to NaN on all six sequences.

Install

sudo apt install ros-jazzy-fusioncore-ros

Contributors

Thanks to @cedbossneo for the second encoder input and the GNSS covariance floor. Both came from real hardware use and both are now part of the default build.