Test of fusioncore with bad results

[JR-pT]

Platform details

• Robot / chassis: custom differential drive, vesc motor controller, skateboard brushless motors 6374 with integrated HALL sensor (only 42 cpr)
• IMU: BNO085
• GPS receiver: RTK with EUREF correction data and nmea messages RMC, GGA with NavSatFix structure
• Drive type: differential

What you need

results of the sensorfusion are pretty wild
Do you find any mistakes in my cfg? Or have you any hints to improve my results?
I tried in a second test the same cfg with gps data, but the result was not better.

Config (based on your cfg wheels_indoor.yaml)

fusioncore:
ros__parameters:
base_frame: base_link
odom_frame: odom
publish_rate: 100.0
publish.force_2d: true # ground robot

motion_model: "DifferentialDrive"

# ── IMU ───────────────────────────────────────────────────────────────────
# Generic 6-axis MEMS defaults. Adjust from your IMU datasheet if available.
# has_magnetometer: set true if you have a 9-axis IMU (BNO085, VectorNav).
#   true  = yaw from magnetometer: available immediately, no drift (but
#           sensitive to motor/wiring interference on robot platforms)
#   false = yaw from gyro integration: drifts slowly, SLAM loop closure
#           corrects it. Safer default for most robots.
imu.has_magnetometer: true
imu.gyro_noise: 0.005             # rad/s : conservative MEMS default
imu.accel_noise: 0.1              # m/s²
imu.remove_gravitational_acceleration: false
# Set your IMU TF frame if your driver doesn't fill header.frame_id.
# Leave empty to use whatever the driver publishes in the message header.
imu.frame_id: "bno_link"
imu.topic: "/imu"

# ── Wheel encoders ────────────────────────────────────────────────────────
# Generic values for mid-quality differential drive.
# Tighten vel_noise if you have high-resolution encoders (≥500 CPR).
# Loosen if your surface is slippery, wheels are soft, or odom is noisy.
encoder.vel_noise: 0.05           # m/s
encoder.yaw_noise: 0.02           # rad/s

# ── Optional second velocity source ──────────────────────────────────────
# Uncomment to add LiDAR ICP or visual odometry as a second velocity source.
# FusionCore fuses both independently: wheel odom primary, ICP cross-check.
# encoder2.topic: "/kiss/odometry"    # KISS-ICP
# encoder2.topic: "/icp_odom"         # rtabmap icp_odometry

# ── GPS: disabled ─────────────────────────────────────────────────────────
gnss.base_noise_xy: 2.5
gnss.base_noise_z: 5.0
gnss.max_hdop: 4.0
gnss.min_satellites: 4
gnss.min_fix_type: 1
gnss.fix2_topic: ""
gnss.heading_topic: ""
gnss.azimuth_topic: ""

# ── Outlier rejection ─────────────────────────────────────────────────────
outlier_rejection: true
outlier_threshold_gnss: 16.27
outlier_threshold_imu: 15.09
outlier_threshold_enc: 11.34
outlier_threshold_hdg: 10.83

# ── Adaptive noise ────────────────────────────────────────────────────────
adaptive.imu: true
adaptive.encoder: true
adaptive.gnss: false              # no GPS
adaptive.window: 50
adaptive.alpha: 0.01

# ── ZUPT ──────────────────────────────────────────────────────────────────
# Critical indoors: suppresses IMU drift during standstill.
# Without GPS corrections, every second of stationary drift matters.
zupt.enabled: true
zupt.velocity_threshold: 0.05     # m/s
zupt.angular_threshold: 0.05      # rad/s
zupt.noise_sigma: 0.01

# Flat-terrain Z position constraint: 0.0 = disabled (default).
# Indoor robots on flat floors can set this to ~0.3 if GPS altitude noise
# is causing Z drift. Rarely needed indoors since GPS is not active.
ground_constraint.z_position_sigma: 0.0

# ── Startup bias init ─────────────────────────────────────────────────────
# Recommended indoors: no GPS to correct early drift.
# Robot must be stationary for 2 seconds after launch.
init.stationary_window: 2.0

# ── UKF process noise ─────────────────────────────────────────────────────
ukf.q_position: 0.01
ukf.q_orientation: 1.0e-9         # quaternion regularization only: do not increase
ukf.q_velocity: 0.1
ukf.q_angular_vel: 0.1
ukf.q_acceleration: 1.0
ukf.q_gyro_bias: 1.0e-5
ukf.q_accel_bias: 1.0e-5
ukf.q_encoder_wz_bias: 1.0e-7

outlier_threshold_vslam: 22.46
adaptive.ground_constraint: true

input.gnss_crs: "EPSG:4326"
output.crs: "EPSG:4978"
output.convert_to_enu_at_reference: true
reference.use_first_fix: false    # no GPS: start at origin
reference.x: 0.0
reference.y: 0.0
reference.z: 0.0

Field tested?

Yes on real hardware
Drive a line of 6 meters, turn around and go back
Odometry drifts in y axis one meter away, x axis good, imu has fitting orientation
Fusioncore results:
Position: x goes up to 30 meters and afterwards goes to -35 meter, y increases straight forward to 169 meter
Orientation: jumps and does not fit to imu nor to odom orientation

Additional context

I remapped the topics to my output topics and set the imu frame according to my settings

Fusioncore Log:
[INFO] [1784704071.013150281] [fusioncore]: FusionCore node created
[INFO] [1784704072.910055006] [fusioncore]: Configuring FusionCore...
[INFO] [1784704072.931470094] [fusioncore]: IMU lever arm: will auto-resolve from TF on first IMU message
[INFO] [1784704072.931536471] [fusioncore]: IMU gravity removal: disabled
[INFO] [1784704072.931560383] [fusioncore]: IMU frame override: bno_link
[INFO] [1784704072.931603060] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[INFO] [1784704072.958508929] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[INFO] [1784704072.958609298] [fusioncore]: PROJ: fixed reference origin (0.000, 0.000, 0.000) → lat=90.000000 lon=0.000000 alt=-6356752.31
[INFO] [1784704072.958765678] [fusioncore]: Motion model: DifferentialDrive
[INFO] [1784704072.980020166] [fusioncore]: Dual antenna heading disabled.
[INFO] [1784704072.980379875] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=100Hz
[INFO] [1784704072.980523009] [fusioncore]: Autostart enabled: activating in 200ms.
[INFO] [1784704072.987806163] [fusioncore]: Activating FusionCore...
[INFO] [1784704072.987859553] [fusioncore]: --- TF Validation ---
[INFO] [1784704072.998129787] [fusioncore]: [OK] bno_link -> base_link
[INFO] [1784704072.998189087] [fusioncore]: ---------------------
[INFO] [1784704073.002110341] [fusioncore]: IMU topic: /imu
[INFO] [1784704073.028663174] [fusioncore]: FusionCore active. Listening for sensors.
[INFO] [1784704073.031797897] [fusioncore]: Collecting 2.0s bias window before init...
[WARN] [1784704073.180606527] [rcl_lifecycle]: No transition matching 3 found for current state active
[INFO] [1784704074.124910024] [fusioncore]: Bias window done (gyro only, no orientation): gyro=[0.0000,0.0000,0.0000]
[INFO] [1784704074.125041639] [fusioncore]: Filter initialized at t=1784704075.332
[INFO] [1784704074.181788132] [fusioncore]: IMU lever arm auto-resolved from TF base_link -> bno_link: x=0.295 y=0.130 z=0.125 m
[INFO] [1784704146.026336218] [rclcpp]: signal_handler(SIGINT/SIGTERM)

[manankharwar]

@JR-pT Thanks for the excellent report.

What was happening. Look at this line in your log:

Bias window done (gyro only, no orientation): gyro=[0.0000,0.0000,0.0000]

"no orientation" should never appear for a BNO085. FusionCore had two disagreeing checks for whether an IMU's orientation is usable. The runtime path did it correctly (per the sensor_msgs/Imu spec, only a negative orientation_covariance[0] means "no orientation data"; all-zeros means "covariance unknown" and the quaternion is still valid). But the startup bias window wrongly demanded a strictly positive covariance. Your driver, like many, publishes a good quaternion with the covariance left at zero, so FusionCore threw your orientation away at init.

That matters more than it sounds, because the accelerometer bias can only be computed in the branch that has an orientation, it needs the attitude to know how much of the measured force is gravity. Without it the bias stays at zero, so the portion of gravity produced by your IMU's mounting tilt is left sitting in the acceleration channel. A constant acceleration error double-integrates. Even a 10° tilt leaves ~1.7 m/s², which is hundreds of metres over a minute. That's your 169 m in y, and the smooth constant direction is the signature. The x swinging +30 then −35 and the orientation not matching either source follow from the same bad initial state.

Your config was fine. This was on me.

To pick it up now:

cd <your_ws>/src/fusioncore && git pull
cd <your_ws> && colcon build --packages-select fusioncore_core fusioncore_ros
source install/setup.bash

(Or git checkout v0.3.3 for a pinned point. Docker :0.3.3 is live; apt will take a few days to clear rosdistro review.)

Confirm the fix took before you drive anywhere. Your startup log should now read:

Bias window done: gyro=[...] accel=[...] <-- fixed

instead of (gyro only, no orientation). If it still says "no orientation", tell me and we'll look at what your driver publishes.

One more thing worth checking. On 0.3.2 the wheel odometry topic was hardcoded to /odom/wheels and could only be changed with a launch remap, and a wrong topic fails silently, the filter runs happily and just never fuses that sensor. Since your odometry was reasonable on its own (1 m over 6 m out-and-back) but the fusion was wild, it's worth ruling out. 0.3.3 adds an encoder.topic parameter, so you can set it directly:

encoder.topic: "/your/odom/topic"

and the node now logs every topic it actually subscribed to at startup:

IMU topic: /imu
Encoder topic: /odom/wheels
GNSS topic: /gnss/fix (sensor_msgs/NavSatFix)

If that encoder line doesn't match what your robot publishes, that's a second thing to fix.

Two small notes on your config, neither is causing the divergence: has_magnetometer: true is correct for a BNO085, and the alarming lat=90.000000 alt=-6356752.31 line is harmless with GPS disabled (it's just the unused ECEF reference sitting at the Earth's centre).

Please do report back either way. This was a genuinely valuable find, thank you for taking the time to write it up properly.

[JR-pT]

@manankharwar Thanks for the quick response. I fetched v0.3.3 and had a try on friday.
I changed the cfg only by adding lines:
encoder.topic: "/vbot_base_controller/odom"
gnss.fix_topic: "/gps/fix"
Same test procedure: 6m straight forward , turn and back

Fusioncore Log:
[INFO] [1784884599.107320346] [fusioncore]: FusionCore node created
[INFO] [1784884600.988753484] [fusioncore]: Configuring FusionCore...
[INFO] [1784884600.997962221] [fusioncore]: IMU lever arm: will auto-resolve from TF on first IMU message
[INFO] [1784884600.998891857] [fusioncore]: IMU gravity removal: disabled
[INFO] [1784884600.998923807] [fusioncore]: IMU frame override: bno_link
[INFO] [1784884600.998972098] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[INFO] [1784884601.024942629] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[INFO] [1784884601.025028960] [fusioncore]: PROJ: fixed reference origin (0.000, 0.000, 0.000) → lat=90.000000 lon=0.000000 alt=-6356752.31
[INFO] [1784884601.025130149] [fusioncore]: Motion model: DifferentialDrive
[INFO] [1784884601.041322644] [fusioncore]: Dual antenna heading disabled.
[INFO] [1784884601.041370973] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=100Hz
[INFO] [1784884601.041440643] [fusioncore]: Autostart enabled: activating in 200ms.
[INFO] [1784884601.045369693] [fusioncore]: Activating FusionCore...
[INFO] [1784884601.045414993] [fusioncore]: --- TF Validation ---
[INFO] [1784884601.055632540] [fusioncore]: [OK] bno_link -> base_link
[INFO] [1784884601.055683675] [fusioncore]: ---------------------
[INFO] [1784884601.059365824] [fusioncore]: IMU topic: /imu
[INFO] [1784884601.063305911] [fusioncore]: Encoder topic: /vbot_base_controller/odom
[INFO] [1784884601.065085900] [fusioncore]: GNSS topic: /gnss/fix (sensor_msgs/NavSatFix)
[INFO] [1784884601.083893420] [fusioncore]: FusionCore active. Listening for sensors.
[INFO] [1784884601.099599868] [fusioncore]: Collecting 2.0s bias window before init...
[WARN] [1784884601.241558551] [rcl_lifecycle]: No transition matching 3 found for current state active
[INFO] [1784884602.106711687] [fusioncore]: Bias window done: gyro=[0.0000,0.0000,0.0000] accel=[0.0222,-0.0142,0.0864] rad/s, m/s²
[INFO] [1784884602.106870963] [fusioncore]: Filter initialized at t=1784884605.248
[INFO] [1784884602.149698563] [fusioncore]: IMU lever arm auto-resolved from TF base_link -> bno_link: x=0.295 y=0.130 z=0.125 m
[INFO] [1784884666.710164782] [rclcpp]: signal_handler(SIGINT/SIGTERM)

seems that the topics are ok and the accel sensor was added, too.
The results changed, but stayed bad. What changed is that the y values are going up and than go down, but are again much too high

I have a log (copied the text of a csv file) of wheel odometry, fusioncore and imu every second, don't know if it helps:
[INFO],"[1784884611,577002543]",[logging_subscriber],pos_x,0,pos_y,0,o_x,0,o_y,0,o_z,0,o_w,1,fc_x,0,fc_y,0,fc_ox,0,fc_oy,0,fc_oz,0,fc_ow,0,imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,63",imu_o_w,"0,78",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,29",imu_la_z,"9,89",yaw,"78,150719",,"1,5707963267949",90,"1,5707963267949",90
[INFO],"[1784884612,546759546]",[logging_subscriber],pos_x,0,pos_y,0,o_x,0,o_y,0,o_z,0,o_w,1,fc_x,"1,893187",fc_y,"-3,251725",fc_ox,"0,015562",fc_oy,"-0,00512",fc_oz,"0,360569",fc_ow,"0,932589",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,63",imu_o_w,"0,78",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,29",imu_la_z,"9,89",yaw,"78,150719",,"1,5707963267949",90,"0,833199899601175","47,7388377378712"
[INFO],"[1784884613,546734131]",[logging_subscriber],pos_x,"0,014772",pos_y,"0,00018",o_x,0,o_y,0,o_z,"0,00869",o_w,"0,999962",fc_x,"1,821417",fc_y,"-3,313491",fc_ox,"0,047384",fc_oy,"-0,009256",fc_oz,"0,4083",fc_ow,"0,91157",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,63",imu_o_w,"0,78",imu_av_x,"-0,04",imu_av_y,0,imu_av_z,"0,14",imu_la_x,"2,61",imu_la_y,"1,14",imu_la_z,"9,59",yaw,"78,150719",,"1,55341611223456","89,004187058661","0,730768676390436","41,8699609575334"
[INFO],"[1784884614,546667653]",[logging_subscriber],pos_x,"0,392549",pos_y,"0,01717",o_x,0,o_y,0,o_z,"0,026067",o_w,"0,99966",fc_x,"1,688494",fc_y,"3,917694",fc_ox,"0,029861",fc_oy,"-0,010515",fc_oz,"0,64514",fc_ow,"0,763408",imu_o_x,"0,03",imu_o_y,"-0,01",imu_o_z,"0,65",imu_o_w,"0,76",imu_av_x,"-0,01",imu_av_y,"0,02",imu_av_z,"-0,03",imu_la_x,"0,27",imu_la_y,"0,87",imu_la_z,"6,83",yaw,"81,100601",,"1,51865643122309","87,0126040394826","0,168412543616572","9,64932796629246"
[INFO],"[1784884615,546668904]",[logging_subscriber],pos_x,"0,820838",pos_y,"0,022432",o_x,0,o_y,0,o_z,"-0,017379",o_w,"0,999849",fc_x,"6,703884",fc_y,"17,228859",fc_ox,"0,030219",fc_oy,"-0,008806",fc_oz,"0,623344",fc_ow,"0,781314",imu_o_x,"0,03",imu_o_y,"-0,01",imu_o_z,"0,63",imu_o_w,"0,78",imu_av_x,0,imu_av_y,"0,05",imu_av_z,"-0,01",imu_la_x,"0,12",imu_la_y,"-0,32",imu_la_z,"8,44",yaw,"78,168907",,"1,60555607760143","91,9915870181407","0,224916139775018","12,8867455534831"
[INFO],"[1784884616,546569710]",[logging_subscriber],pos_x,"1,249001",pos_y,"0,008339",o_x,0,o_y,0,o_z,"-0,026067",o_w,"0,99966",fc_x,"8,050267",fc_y,"23,22904",fc_ox,"0,029256",fc_oy,"-0,010198",fc_oz,"0,623713",fc_ow,"0,781039",imu_o_x,"0,03",imu_o_y,"-0,01",imu_o_z,"0,62",imu_o_w,"0,79",imu_av_x,"-0,04",imu_av_y,"0,01",imu_av_z,"-0,03",imu_la_x,"-0,04",imu_la_y,"0,18",imu_la_z,"10,28",yaw,"76,739281",,"1,6229362223667","92,9873959605175","0,223928350104156","12,830149374296"
[INFO],"[1784884617,546738481]",[logging_subscriber],pos_x,"1,676627",pos_y,"0,017081",o_x,0,o_y,0,o_z,"0,052116",o_w,"0,998641",fc_x,"8,167268",fc_y,"22,579154",fc_ox,"0,029929",fc_oy,"0,000416",fc_oz,"0,658149",fc_ow,"0,752292",imu_o_x,"0,03",imu_o_y,0,imu_o_z,"0,66",imu_o_w,"0,75",imu_av_x,0,imu_av_y,"0,23",imu_av_z,"0,06",imu_la_x,0,imu_la_y,"0,18",imu_la_z,"5,03",yaw,"82,587395",,"1,46651708926153","84,0252397984961","0,134182398992803","7,68808514722807"
[INFO],"[1784884618,546669015]",[logging_subscriber],pos_x,"2,096408",pos_y,"0,06481",o_x,0,o_y,0,o_z,"0,043436",o_w,"0,999056",fc_x,"8,61262",fc_y,"28,897069",fc_ox,"0,018392",fc_oy,"-0,001812",fc_oz,"0,653173",fc_ow,"0,756983",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,67",imu_o_w,"0,74",imu_av_x,0,imu_av_y,"-0,07",imu_av_z,"-0,11",imu_la_x,"0,23",imu_la_y,"-0,13",imu_la_z,"12,77",yaw,"84,115547",,"1,48389700518896","85,0210356294296","0,147308306202142","8,44014423260356"
[INFO],"[1784884619,546590669]",[logging_subscriber],pos_x,"2,524517",pos_y,"0,076537",o_x,0,o_y,0,o_z,"0,017379",o_w,"0,999849",fc_x,"9,750142",fc_y,"38,677268",fc_ox,"0,019627",fc_oy,"0,002505",fc_oz,"0,661644",fc_ow,"0,749557",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,64",imu_o_w,"0,77",imu_av_x,"-0,1",imu_av_y,0,imu_av_z,"0,09",imu_la_x,"0,23",imu_la_y,"0,29",imu_la_z,"10,93",yaw,"79,605141",,"1,53603657598837","88,0084129818593","0,124796290225241","7,15030072879606"
[INFO],"[1784884620,546695409]",[logging_subscriber],pos_x,"2,958452",pos_y,"0,092577",o_x,0,o_y,0,o_z,"-0,00869",o_w,"0,999962",fc_x,"6,288644",fc_y,"57,797626",fc_ox,"0,014916",fc_oy,"-0,000261",fc_oz,"0,62913",fc_ow,"0,777157",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,64",imu_o_w,"0,77",imu_av_x,"0,04",imu_av_y,"0,08",imu_av_z,"-0,25",imu_la_x,"0,58",imu_la_y,"-0,28",imu_la_z,"9,77",yaw,"79,605141",,"1,58817654135523","90,995812941339","0,209968454983451","12,0303063014344"
[INFO],"[1784884621,546821876]",[logging_subscriber],pos_x,"3,377218",pos_y,"0,067947",o_x,0,o_y,0,o_z,"-0,043436",o_w,"0,999056",fc_x,"5,560595",fc_y,"64,545856",fc_ox,"0,021481",fc_oy,"0,000768",fc_oz,"0,608648",fc_ow,"0,793149",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,61",imu_o_w,"0,79",imu_av_x,"0,13",imu_av_y,"-0,07",imu_av_z,"-0,03",imu_la_x,0,imu_la_y,"1,06",imu_la_z,"8,75",yaw,"75,135948",,"1,65769564840084","94,9789643705704","0,262167552890318","15,021094305888"
[INFO],"[1784884622,546745408]",[logging_subscriber],pos_x,"3,79942",pos_y,"0,056013",o_x,0,o_y,0,o_z,0,o_w,1,fc_x,"5,757193",fc_y,"73,375208",fc_ox,"0,019416",fc_oy,"-0,000401",fc_oz,"0,637575",fc_ow,"0,770144",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,64",imu_o_w,"0,77",imu_av_x,"0,1",imu_av_y,"-0,08",imu_av_z,"0,06",imu_la_x,"0,54",imu_la_y,"0,48",imu_la_z,"10,43",yaw,"79,605141",,"1,5707963267949",90,"0,188164420584324","10,7810271540063"
[INFO],"[1784884623,546685276]",[logging_subscriber],pos_x,"4,224758",pos_y,"0,065101",o_x,0,o_y,0,o_z,"0,017379",o_w,"0,999849",fc_x,"6,443261",fc_y,"84,489568",fc_ox,"0,020479",fc_oy,"0,000586",fc_oz,"0,645275",fc_ow,"0,763675",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,65",imu_o_w,"0,76",imu_av_x,"-0,07",imu_av_y,"-0,07",imu_av_z,"-0,03",imu_la_x,"0,5",imu_la_y,"0,33",imu_la_z,"9,81",yaw,"81,083961",,"1,53603657598837","88,0084129818593","0,168084689059329","9,63054328386829"
[INFO],"[1784884624,546577836]",[logging_subscriber],pos_x,"4,652809",pos_y,"0,082581",o_x,0,o_y,0,o_z,"0,026067",o_w,"0,99966",fc_x,"6,881958",fc_y,"98,895705",fc_ox,"0,021489",fc_oy,"0,001036",fc_oz,"0,675126",fc_ow,"0,737389",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,65",imu_o_w,"0,76",imu_av_x,0,imu_av_y,"-0,01",imu_av_z,"0,04",imu_la_x,"0,2",imu_la_y,"0,1",imu_la_z,"6,45",yaw,"81,083961",,"1,51865643122309","87,0126040394826","0,0885567016873473","5,07392525428406"
[INFO],"[1784884625,546627846]",[logging_subscriber],pos_x,"5,080924",pos_y,"0,09847",o_x,0,o_y,0,o_z,"0,017379",o_w,"0,999849",fc_x,"6,910458",fc_y,"102,768646",fc_ox,"0,01992",fc_oy,"0,000201",fc_oz,"0,648763",fc_ow,"0,76073",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,65",imu_o_w,"0,76",imu_av_x,"-0,1",imu_av_y,"-0,09",imu_av_z,"-0,01",imu_la_x,"0,04",imu_la_y,"0,21",imu_la_z,"9,09",yaw,"81,083961",,"1,53603657598837","88,0084129818593","0,158932887632109","9,10618368714678"
[INFO],"[1784884626,546685664]",[logging_subscriber],pos_x,"5,509107",pos_y,"0,112461",o_x,0,o_y,0,o_z,"0,00869",o_w,"0,999962",fc_x,"7,440634",fc_y,"106,683503",fc_ox,"0,019686",fc_oy,"-0,000933",fc_oz,"0,64567",fc_ow,"0,763362",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,65",imu_o_w,"0,76",imu_av_x,"0,07",imu_av_y,"0,13",imu_av_z,"-0,09",imu_la_x,"0,16",imu_la_y,"0,75",imu_la_z,"11,92",yaw,"81,083961",,"1,55341611223456","89,004187058661","0,167054654288527","9,57152663874964"
[INFO],"[1784884627,546563232]",[logging_subscriber],pos_x,"5,933036",pos_y,"0,080881",o_x,0,o_y,0,o_z,"-0,060792",o_w,"0,99815",fc_x,"9,19418",fc_y,"113,66866",fc_ox,"0,020265",fc_oy,"-0,001452",fc_oz,"0,600712",fc_ow,"0,799207",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,6",imu_o_w,"0,8",imu_av_x,"-0,01",imu_av_y,"-0,04",imu_av_z,"-0,01",imu_la_x,"0,43",imu_la_y,"0,41",imu_la_z,"10,04",yaw,"73,739792",,"1,69245528558732","96,9705448787616","0,282112306482223","16,1638445101326"
[INFO],"[1784884628,546734373]",[logging_subscriber],pos_x,"5,991663",pos_y,"0,073454",o_x,0,o_y,0,o_z,"-0,043436",o_w,"0,999056",fc_x,"14,416411",fc_y,"130,394617",fc_ox,"0,019176",fc_oy,"-0,000514",fc_oz,"0,619587",fc_ow,"0,784694",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,6",imu_o_w,"0,8",imu_av_x,"-0,05",imu_av_y,"0,01",imu_av_z,"0,12",imu_la_x,"0,35",imu_la_y,"0,52",imu_la_z,"11,93",yaw,"73,739792",,"1,65769564840084","94,9789643705704","0,23443488459244","13,4321294577833"
[INFO],"[1784884629,546753695]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"12,480043",fc_y,"117,253687",fc_ox,"0,009963",fc_oy,"0,000301",fc_oz,"0,58961",fc_ow,"0,807627",imu_o_x,"0,01",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,309664486877262","17,7424681631514"
[INFO],"[1784884630,546699556]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"12,478296",fc_y,"117,39006",fc_ox,"0,019795",fc_oy,"0,000202",fc_oz,"0,587909",fc_ow,"0,808685",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,23",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,313938676571727","17,9873611934826"
[INFO],"[1784884631,546618297]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"12,408602",fc_y,"117,259045",fc_ox,"0,019603",fc_oy,"0,00023",fc_oz,"0,582807",fc_ow,"0,812374",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,326524339649493","18,7084665702122"
[INFO],"[1784884632,546769547]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"12,346012",fc_y,"116,594136",fc_ox,"0,019064",fc_oy,"0,000475",fc_oz,"0,571734",fc_ow,"0,820217",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,353641952912828","20,2621913606693"
[INFO],"[1784884633,546543345]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"12,853511",fc_y,"115,138642",fc_ox,"0,015923",fc_oy,"-0,001092",fc_oz,"0,465914",fc_ow,"0,884686",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,601555388640246","34,4665849124381"
[INFO],"[1784884634,546535635]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"13,07052",fc_y,"113,426679",fc_ox,"0,015519",fc_oy,"-0,006204",fc_oz,"0,305048",fc_ow,"0,95219",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,951003684511976","54,4884974239271"
[INFO],"[1784884635,546749148]",[logging_subscriber],pos_x,"5,906545",pos_y,"0,083214",o_x,0,o_y,0,o_z,"-0,069464",o_w,"0,997584",fc_x,"13,105153",fc_y,"113,29974",fc_ox,"0,019678",fc_oy,"0,000597",fc_oz,"0,584956",fc_ow,"0,810826",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,23",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,70983623394307","97,9663998634813","0,321224908788037","18,4048315480293"
[INFO],"[1784884636,546631713]",[logging_subscriber],pos_x,"5,924055",pos_y,"0,080453",o_x,0,o_y,0,o_z,"-0,08679",o_w,"0,996227",fc_x,"13,207246",fc_y,"113,325118",fc_ox,"0,019056",fc_oy,"0,00058",fc_oz,"0,573024",fc_ow,"0,819317",imu_o_x,"0,02",imu_o_y,0,imu_o_z,"0,59",imu_o_w,"0,81",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"72,367195",,"1,74459504758393","99,9579331859842","0,350493307126303","20,0817872459197"
[INFO],"[1784884637,546632099]",[logging_subscriber],pos_x,"6,22554",pos_y,"-0,031705",o_x,0,o_y,0,o_z,"-0,37311",o_w,"0,927787",fc_x,"-0,871491",fc_y,"112,426366",fc_ox,"0,028002",fc_oy,"-0,056852",fc_oz,"0,105102",fc_ow,"0,99244",imu_o_x,"0,02",imu_o_y,"-0,01",imu_o_z,"0,36",imu_o_w,"0,93",imu_av_x,"0,14",imu_av_y,"0,03",imu_av_z,"-0,53",imu_la_x,"-0,15",imu_la_y,"0,83",imu_la_z,"10,58",yaw,"42,134758",,"2,3355139311584","133,815091249384","1,36239673596881","78,0595829934119"
[INFO],"[1784884638,546737059]",[logging_subscriber],pos_x,"6,243671",pos_y,"-0,061779",o_x,0,o_y,0,o_z,"-0,704769",o_w,"0,709436",fc_x,"-37,779061",fc_y,"76,144162",fc_ox,"0,01247",fc_oy,"-0,011992",fc_oz,"0,045961",fc_ow,"0,998793",imu_o_x,"0,02",imu_o_y,"-0,01",imu_o_z,"0,03",imu_o_w,1,imu_av_x,"0,03",imu_av_y,"0,03",imu_av_z,"-1,05",imu_la_x,"-0,3",imu_la_y,"-1,2",imu_la_z,"9,05",yaw,"3,463376",,"3,13499128437517","179,621769404995","1,47912718114697","84,7477448428035"
[INFO],"[1784884639,546751296]",[logging_subscriber],pos_x,"6,071239",pos_y,"-0,178372",o_x,0,o_y,0,o_z,"-0,982176",o_w,"0,187965",fc_x,"-31,830205",fc_y,"81,692823",fc_ox,"0,025233",fc_oy,"-0,011873",fc_oz,"0,705017",fc_ow,"0,708642",imu_o_x,0,imu_o_y,0,imu_o_z,"-0,5",imu_o_w,"0,87",imu_av_x,"0,07",imu_av_y,"-0,09",imu_av_z,"-1,13",imu_la_x,"-0,57",imu_la_y,"-0,66",imu_la_z,"4,49",yaw,"-60,113472",,"-1,94897563309346","-111,668078150093","0,00562788585232731","0,322454106919741"
[INFO],"[1784884640,546631389]",[logging_subscriber],pos_x,"6,029452",pos_y,"-0,193124",o_x,0,o_y,0,o_z,"-0,986742",o_w,"0,162299",fc_x,"-29,957717",fc_y,"86,933374",fc_ox,"-0,00002",fc_oy,"-0,009302",fc_oz,"-0,564441",fc_ow,"0,825421",imu_o_x,0,imu_o_y,"-0,01",imu_o_z,"-0,57",imu_o_w,"0,82",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,27",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"-69,473518",,"-1,89683616680279","-108,680706785573","2,77044782275501","158,73496760507"
[INFO],"[1784884641,546744242]",[logging_subscriber],pos_x,"6,172503",pos_y,"-0,156663",o_x,0,o_y,0,o_z,"-0,995632",o_w,"0,093364",fc_x,"-25,216236",fc_y,"99,594636",fc_ox,"-0,001845",fc_oy,"0,008511",fc_oz,"-0,729883",fc_ow,"0,683517",imu_o_x,0,imu_o_y,"-0,01",imu_o_z,"-0,61",imu_o_w,"0,79",imu_av_x,"0,11",imu_av_y,"0,03",imu_av_z,"-0,3",imu_la_x,"-1,03",imu_la_y,"2,55",imu_la_z,"8,02",yaw,"-75,147057",,"-1,75779669955419","-100,714332126481","-3,0759396518779","-176,238360089543"
[INFO],"[1784884642,546608711]",[logging_subscriber],pos_x,"6,604472",pos_y,"-0,136307",o_x,0,o_y,0,o_z,"-0,99853",o_w,"-0,054203",fc_x,"-16,367405",fc_y,"101,732027",fc_ox,"-0,01633",fc_oy,"0,009715",fc_oz,"-0,805713",fc_ow,"-0,592002",imu_o_x,0,imu_o_y,"-0,01",imu_o_z,"-0,72",imu_o_w,"0,69",imu_av_x,"-0,07",imu_av_y,"-0,18",imu_av_z,"-0,37",imu_la_x,"0,23",imu_la_y,"-0,55",imu_la_z,"9,97",yaw,"-92,132614",,"-1,46233719481315","-83,7857494877938","-0,303381529574302","-17,3824812268309"
[INFO],"[1784884643,546542777]",[logging_subscriber],pos_x,"6,344859",pos_y,"-0,066347",o_x,0,o_y,0,o_z,"-0,988798",o_w,"-0,14926",fc_x,"-23,366827",fc_y,"100,092283",fc_ox,"-0,002764",fc_oy,"-0,000658",fc_oz,"-0,765783",fc_ow,"0,643093",imu_o_x,0,imu_o_y,0,imu_o_z,"-0,78",imu_o_w,"0,62",imu_av_x,"-0,14",imu_av_y,"-0,08",imu_av_z,"-0,15",imu_la_x,"-0,15",imu_la_y,"0,56",imu_la_z,"9,89",yaw,"-102,63414",,"-1,27115664719768","-72,8319109844274","-2,96786991287227","-170,04642015144"
[INFO],"[1784884644,546561230]",[logging_subscriber],pos_x,"5,931493",pos_y,"0,066921",o_x,0,o_y,0,o_z,"-0,987464",o_w,"-0,157847",fc_x,"-20,428869",fc_y,"84,751097",fc_ox,"-0,017072",fc_oy,"-0,023032",fc_oz,"-0,748062",fc_ow,"0,66301",imu_o_x,"-0,01",imu_o_y,"-0,02",imu_o_z,"-0,79",imu_o_w,"0,62",imu_av_x,"-0,02",imu_av_y,"-0,04",imu_av_z,"0,01",imu_la_x,"0,54",imu_la_y,"0,18",imu_la_z,"11,16",yaw,"-104,256134",,"-1,25377684153367","-71,836121471122","-3,02085563415276","-173,082278355269"
[INFO],"[1784884645,546594119]",[logging_subscriber],pos_x,"5,513114",pos_y,"0,193972",o_x,0,o_y,0,o_z,"-0,990058",o_w,"-0,140662",fc_x,"-23,288442",fc_y,"83,112079",fc_ox,"-0,009718",fc_oy,"-0,020158",fc_oz,"-0,775591",fc_ow,"0,630839",imu_o_x,"-0,01",imu_o_y,"-0,02",imu_o_z,"-0,78",imu_o_w,"0,63",imu_av_x,"-0,04",imu_av_y,"-0,05",imu_av_z,"0,05",imu_la_x,"0,2",imu_la_y,"0,33",imu_la_z,"9,78",yaw,"-102,479439",,"-1,28853653482218","-73,8277051937228","-2,93608621522224","-168,225348418774"
[INFO],"[1784884646,546631565]",[logging_subscriber],pos_x,"5,099435",pos_y,"0,305306",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-23,815172",fc_y,"89,986291",fc_ox,"0,000326",fc_oy,"-0,018303",fc_oz,"-0,780583",fc_ow,"0,624785",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"0,1",imu_av_y,"-0,01",imu_av_z,0,imu_la_x,"0,54",imu_la_y,"0,52",imu_la_z,"8,78",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,9204504423457","-167,329484623523"
[INFO],"[1784884647,546838364]",[logging_subscriber],pos_x,"4,687505",pos_y,"0,411723",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-24,820844",fc_y,"90,335599",fc_ox,"0,0025",fc_oy,"-0,018937",fc_oz,"-0,76541",fc_ow,"0,643259",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"0,14",imu_av_y,"0,05",imu_av_z,0,imu_la_x,"0,43",imu_la_y,"1,17",imu_la_z,"12,27",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,96826749674045","-170,069200029089"
[INFO],"[1784884648,546679601]",[logging_subscriber],pos_x,"4,276002",pos_y,"0,51978",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-25,757749",fc_y,"83,794663",fc_ox,"0,000334",fc_oy,"-0,019932",fc_oz,"-0,764728",fc_ow,"0,644045",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"-0,04",imu_av_y,"0,25",imu_av_z,"-0,01",imu_la_x,"0,5",imu_la_y,"0,94",imu_la_z,"9,89",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,97028975645803","-170,185066975987"
[INFO],"[1784884649,546564354]",[logging_subscriber],pos_x,"3,861876",pos_y,"0,629479",o_x,0,o_y,0,o_z,"-0,992353",o_w,"-0,123434",fc_x,"-27,6576",fc_y,"73,498226",fc_ox,"-0,000962",fc_oy,"-0,019332",fc_oz,"-0,782721",fc_ow,"0,622071",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,0,imu_av_y,"0,02",imu_av_z,"0,02",imu_la_x,"0,39",imu_la_y,"-0,21",imu_la_z,"9,93",yaw,"-100,720711",,"-1,3232972696825","-75,8193485939922","-2,91349395248163","-166,930907114086"
[INFO],"[1784884650,546706881]",[logging_subscriber],pos_x,"3,447777",pos_y,"0,739277",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-32,167241",fc_y,"54,866618",fc_ox,"-0,001746",fc_oy,"-0,022172",fc_oz,"-0,762946",fc_ow,"0,646079",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"0,11",imu_av_y,"0,04",imu_av_z,"-0,02",imu_la_x,"0,43",imu_la_y,"-0,47",imu_la_z,"10,92",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,97559377924724","-170,488965096249"
[INFO],"[1784884651,546667722]",[logging_subscriber],pos_x,"3,031017",pos_y,"0,850569",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-32,510078",fc_y,"52,487539",fc_ox,"0,000287",fc_oy,"-0,019892",fc_oz,"-0,767425",fc_ow,"0,64083",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"0,06",imu_av_y,"0,06",imu_av_z,"0,02",imu_la_x,"0,12",imu_la_y,"-0,09",imu_la_z,"10,96",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,96189739156532","-169,7042198875"
[INFO],"[1784884652,546666259]",[logging_subscriber],pos_x,"2,626606",pos_y,"0,962235",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-32,934685",fc_y,"50,653812",fc_ox,"0,000721",fc_oy,"-0,01851",fc_oz,"-0,761556",fc_ow,"0,647835",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"-0,05",imu_av_y,"0,05",imu_av_z,"-0,01",imu_la_x,"0,23",imu_la_y,"1,29",imu_la_z,"9,51",yaw,"-100,720711",,"-1,30591692895858","-74,8235284240122","-2,98023108502808","-170,754663145803"
[INFO],"[1784884653,546748896]",[logging_subscriber],pos_x,"2,21431",pos_y,"1,078621",o_x,0,o_y,0,o_z,"-0,990058",o_w,"-0,140662",fc_x,"-34,619116",fc_y,"41,601578",fc_ox,"-0,000844",fc_oy,"-0,019582",fc_oz,"-0,772569",fc_ow,"0,634628",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,64",imu_av_x,"0,2",imu_av_y,"0,01",imu_av_z,"-0,01",imu_la_x,"0,2",imu_la_y,"0,52",imu_la_z,"8,97",yaw,"-100,720711",,"-1,28853653482218","-73,8277051937228","-2,94578453919158","-168,781021450568"
[INFO],"[1784884654,546695966]",[logging_subscriber],pos_x,"1,80458",pos_y,"1,193113",o_x,0,o_y,0,o_z,"-0,992353",o_w,"-0,123434",fc_x,"-36,898542",fc_y,"30,15473",fc_ox,"-0,002782",fc_oy,"-0,022304",fc_oz,"-0,754889",fc_ow,"0,655467",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,76",imu_o_w,"0,65",imu_av_x,"0,12",imu_av_y,"0,03",imu_av_z,"0,08",imu_la_x,"0,08",imu_la_y,"0,79",imu_la_z,"7,52",yaw,"-98,972626",,"-1,3232972696825","-75,8193485939922","-3,00033480890402","-171,906521676391"
[INFO],"[1784884655,546633307]",[logging_subscriber],pos_x,"1,395249",pos_y,"1,285052",o_x,0,o_y,0,o_z,"-0,994348",o_w,"-0,106169",fc_x,"-39,427849",fc_y,"30,343261",fc_ox,"-0,000699",fc_oy,"-0,020771",fc_oz,"-0,750417",fc_ow,"0,660638",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,75",imu_o_w,"0,66",imu_av_x,"0,03",imu_av_y,"0,15",imu_av_z,"0,02",imu_la_x,"0,35",imu_la_y,"0,64",imu_la_z,"7,86",yaw,"-97,241806",,"-1,35805731942351","-77,8109527398172","-3,01408157441711","-172,694153322247"
[INFO],"[1784884656,546776724]",[logging_subscriber],pos_x,"0,978296",pos_y,"1,369653",o_x,0,o_y,0,o_z,"-0,996043",o_w,"-0,088872",fc_x,"-39,427384",fc_y,"25,109147",fc_ox,"0,000206",fc_oy,"-0,019244",fc_oz,"-0,737266",fc_ow,"0,675328",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,74",imu_o_w,"0,67",imu_av_x,"-0,02",imu_av_y,"-0,07",imu_av_z,"0,01",imu_la_x,"0,62",imu_la_y,"0,14",imu_la_z,"10,39",yaw,"-95,529755",,"-1,39281748479782","-79,8025635109416","-3,05358706657335","-174,957651290387"
[INFO],"[1784884657,546640579]",[logging_subscriber],pos_x,"0,558869",pos_y,"1,456392",o_x,0,o_y,0,o_z,"-0,991243",o_w,"-0,132053",fc_x,"-40,307352",fc_y,"15,84121",fc_ox,"-0,002962",fc_oy,"-0,022842",fc_oz,"-0,781603",fc_ow,"0,62335",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,76",imu_o_w,"0,65",imu_av_x,"0,06",imu_av_y,"0,03",imu_av_z,"-0,19",imu_la_x,"0,35",imu_la_y,"0,98",imu_la_z,"8,93",yaw,"-98,972626",,"-1,30591692895858","-74,8235284240122","-2,91672995605461","-167,116316461307"
[INFO],"[1784884658,546727254]",[logging_subscriber],pos_x,"0,23039",pos_y,"1,554111",o_x,0,o_y,0,o_z,"-0,988798",o_w,"-0,14926",fc_x,"-46,014701",fc_y,"-1,437508",fc_ox,"0,000614",fc_oy,"-0,019491",fc_oz,"-0,767543",fc_ow,"0,640701",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,78",imu_o_w,"0,63",imu_av_x,"0,04",imu_av_y,"0,01",imu_av_z,0,imu_la_x,"-1,22",imu_la_y,"0,02",imu_la_z,"9,55",yaw,"-102,48436",,"-1,27115664719768","-72,8319109844274","-2,96156582731036","-169,685222655054"
[INFO],"[1784884659,546661101]",[logging_subscriber],pos_x,"0,23322",pos_y,"1,553263",o_x,0,o_y,0,o_z,"-0,990058",o_w,"-0,140662",fc_x,"-45,441424",fc_y,"-3,955392",fc_ox,"-0,000834",fc_oy,"-0,019752",fc_oz,"-0,773626",fc_ow,"0,633334",imu_o_x,0,imu_o_y,"-0,02",imu_o_z,"-0,77",imu_o_w,"0,63",imu_av_x,0,imu_av_y,0,imu_av_z,0,imu_la_x,"0,23",imu_la_y,"0,25",imu_la_z,"9,89",yaw,"-100,886841",,"-1,28853653482218","-73,8277051937228","-2,9424355676247","-168,589139514076"

Perhaps you have again a hint for me
Thanks and Greetings
JR

[JR-pT]

Sorry forgot
Wheel odom position : pos_x, pos_y
Wheel odom Orientation: o_x,o_y,o_z_o_w

same for fusionscore results
and afterwards the data of the imu topic

[manankharwar]

@JR-pT I dug into your logs and CSV properly and found it. This was another real FusionCore bug, and your data is what exposed it. Kudos to you!!!

Root cause first, then what to change on your side, then what shipped.

The root cause is in your own log, two lines apart:

[INFO] [1784884602.106...] Bias window done: ...
[INFO] [1784884602.106...] Filter initialized at t=1784884605.248

The wall clock on the left says 602.1, but the filter initialized at sensor time 605.2. Your IMU stamps its messages about 3.1 seconds ahead of your machine's clock (your earlier 0.3.2 log shows the same thing, +1.2 s that day). Your encoder stamps correctly. So your two sensors disagree about what time it is by roughly 3 seconds.

What FusionCore did with that, and why it is our bug: the filter clock rode the IMU, since it was the leading stream. Every encoder message then arrived looking 3 seconds old. The backward-time-jump guard re-based the filter clock backward to fuse it, and the next IMU message re-integrated that entire 3 second window forward through the motion model again, at the IMU rate. Re-integrating a 3 second window dozens of times per second turns a 0.4 m/s robot into roughly 10 m/s of position divergence.

The fix on your side, which makes your setup work today: get both sensors stamping from the same clock.

ros2 topic echo /imu --field header.stamp --once
ros2 topic echo /vbot_base_controller/odom --field header.stamp --once

Those should agree within milliseconds; yours will differ by about 3 seconds. If the BNO085 driver runs on a companion board or a second machine, sync the clocks with chrony or NTP, or better, have the driver stamp with the node clock (node.get_clock().now()) rather than a device clock. With the stamps agreeing, your existing config should behave completely differently.

What shipped in FusionCore (released today as v0.3.4): a sensor whose stamps lag the filter clock while its own stream still advances is now rejected as stale instead of corrupting the clock, so this failure mode cannot diverge anymore. A genuine time-base reset (bag replay restart, clock correction) still re-bases as before, so the original backward-dt crash fix is preserved.

It also cannot fail silently now. The node warns at startup when a sensor's header.stamp is more than 1 s from the node clock, warns at runtime with the measured inter-sensor offset, and /fusion/debug/filter_health carries imu_stale_reject_count and encoder_stale_reject_count. On your setup the first ten seconds of log would have named the problem. There is also a new max_measurement_delay parameter for sensors with genuinely large known latency.

One unrelated note: your log shows GNSS topic: /gnss/fix, so the gnss.fix_topic: "/gps/fix" line never reached the node. Worth checking which yaml your launch actually loads. GPS was not part of this run either way, which is what made the diagnosis clean.

If you get a chance to re-run with the clocks synced, or on 0.3.4, I would genuinely like to hear the result. Reports like yours, with logs and per-second data, are what make bugs like this findable at all. Thank you for taking the time, twice.

[JR-pT]

Thanks for the quick response, again. I fetched v0.3.4 and tried it with same settings.
Fusioncore Log:
[INFO] [1785231735.925277176] [fusioncore]: FusionCore node created
[INFO] [1785231739.073865649] [fusioncore]: Configuring FusionCore...
[INFO] [1785231739.104339063] [fusioncore]: IMU lever arm: will auto-resolve from TF on first IMU message
[INFO] [1785231739.104405384] [fusioncore]: IMU gravity removal: disabled
[INFO] [1785231739.104423805] [fusioncore]: IMU frame override: bno_link
[INFO] [1785231739.104467721] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[INFO] [1785231739.356675891] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[INFO] [1785231739.356743764] [fusioncore]: PROJ: using first GPS fix as local reference origin
[INFO] [1785231739.357883838] [fusioncore]: Motion model: DifferentialDrive
[INFO] [1785231739.395659056] [fusioncore]: Dual antenna heading disabled.
[INFO] [1785231739.395726211] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=100Hz
[INFO] [1785231739.396013856] [fusioncore]: Autostart enabled: activating in 200ms.
[INFO] [1785231739.401978369] [fusioncore]: Activating FusionCore...
[INFO] [1785231739.402050410] [fusioncore]: --- TF Validation ---
[INFO] [1785231739.412705399] [fusioncore]: [OK] bno_link -> base_link
[INFO] [1785231739.412788406] [fusioncore]: ---------------------
[INFO] [1785231739.418017500] [fusioncore]: IMU topic: /imu
[INFO] [1785231739.426673113] [fusioncore]: Encoder topic: /vbot_base_controller/odom
[INFO] [1785231739.428459840] [fusioncore]: GNSS topic: /gps/fix (sensor_msgs/NavSatFix)
[WARN] [1785231739.453607451] [fusioncore]: IMU header.stamp is +3.71s from this node's clock. If any other sensor stamps with a different clock, the sensor that runs ahead will drive the filter clock and the rest will be rejected as stale (watch the stale_reject counts in /fusion/debug/filter_health). Fix the driver's timestamps: stamp with node time, or sync machine clocks (chrony/NTP).
[INFO] [1785231739.453727558] [fusioncore]: Collecting 2.0s bias window before init...
[INFO] [1785231739.491558225] [fusioncore]: FusionCore active. Listening for sensors.
[WARN] [1785231739.595957087] [rcl_lifecycle]: No transition matching 3 found for current state active
[INFO] [1785231742.760013000] [fusioncore]: Bias window done: gyro=[-0.0003,0.0002,0.0000] accel=[0.0459,0.0428,0.0524] rad/s, m/s²
[INFO] [1785231742.761556546] [fusioncore]: Filter initialized at t=1785231745.170
[INFO] [1785231742.820984408] [fusioncore]: IMU lever arm auto-resolved from TF base_link -> bno_link: x=0.295 y=0.130 z=0.125 m
[INFO] [1785231742.852012048] [fusioncore]: GNSS lever arm auto-resolved from TF base_link -> gnss_link: x=-0.275 y=-0.110 z=0.130 m
[INFO] [1785231742.852076317] [fusioncore]: GNSS reference set: lat=47.988970 lon=12.013164
[WARN] [1785231742.852114834] [fusioncore]: GNSS fix rejected: DELAY_TOO_LARGE (hdop=0.06, d2=-1.0, threshold=16.3)
[WARN] [1785231743.481915688] [fusioncore]: STALE sensor rejections climbing (imu=0 encoder=14): the lagging sensor is NOT being fused. IMU stamp minus encoder stamp is currently +3.44s; sensors must share a time base. Fix the sensor drivers' clocks (stamp with node time, or chrony/NTP across machines).
[WARN] [1785231748.042157180] [fusioncore]: GNSS fix rejected: DELAY_TOO_LARGE (hdop=0.06, d2=0.0, threshold=16.3)
[INFO] [1785231748.225182608] [rclcpp]: signal_handler(SIGINT/SIGTERM)

So it seems, that your patch works. After that i tried to fix the time gap, but still wondering why it exists, because both things are running on the same machine and using the same time base. Nevertheless get it working and had much better results. The result of fusionscore have mirrors nearly the same odom position of x and y to y and x, but that might be possible due to an turned orientation of approximately 90 degrees. Need further examination. But looks much better.
My next test was adding the gps, just turned adaptive.gnss and reference.use_first_fix to true. An i got this:
[INFO] [1785234959.100324908] [fusioncore]: FusionCore node created
[INFO] [1785234960.698062950] [fusioncore]: Configuring FusionCore...
[INFO] [1785234960.718192655] [fusioncore]: IMU lever arm: will auto-resolve from TF on first IMU message
[INFO] [1785234960.718252006] [fusioncore]: IMU gravity removal: disabled
[INFO] [1785234960.718268831] [fusioncore]: IMU frame override: bno_link
[INFO] [1785234960.718310685] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[INFO] [1785234960.964812237] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[INFO] [1785234960.964879849] [fusioncore]: PROJ: using first GPS fix as local reference origin
[INFO] [1785234960.965901923] [fusioncore]: Motion model: DifferentialDrive
[INFO] [1785234961.003388285] [fusioncore]: Dual antenna heading disabled.
[INFO] [1785234961.003585490] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=100Hz
[INFO] [1785234961.003696699] [fusioncore]: Autostart enabled: activating in 200ms.
[INFO] [1785234961.009138539] [fusioncore]: Activating FusionCore...
[INFO] [1785234961.009203094] [fusioncore]: --- TF Validation ---
[INFO] [1785234961.020221947] [fusioncore]: [OK] bno_link -> base_link
[INFO] [1785234961.020282000] [fusioncore]: ---------------------
[INFO] [1785234961.024982247] [fusioncore]: IMU topic: /imu
[INFO] [1785234961.030322107] [fusioncore]: Encoder topic: /vbot_base_controller/odom
[INFO] [1785234961.032557584] [fusioncore]: GNSS topic: /gps/fix (sensor_msgs/NavSatFix)
[INFO] [1785234961.102660423] [fusioncore]: FusionCore active. Listening for sensors.
[INFO] [1785234961.115090852] [fusioncore]: Collecting 2.0s bias window before init...
[WARN] [1785234961.203796146] [rcl_lifecycle]: No transition matching 3 found for current state active
[INFO] [1785234963.324232870] [fusioncore]: Bias window done: gyro=[0.0000,0.0000,0.0000] accel=[-0.0671,0.0345,0.1332] rad/s, m/s²
[INFO] [1785234963.326270723] [fusioncore]: Filter initialized at t=1785234963.324
[INFO] [1785234963.467910989] [fusioncore]: GNSS lever arm auto-resolved from TF base_link -> gnss_link: x=-0.275 y=-0.110 z=0.130 m
[INFO] [1785234963.468018930] [fusioncore]: GNSS reference set: lat=47.989010 lon=12.013201
[WARN] [1785234963.468106700] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.40, d2=-1.0, threshold=16.3)
[INFO] [1785234963.468333064] [fusioncore]: Heading not yet validated: lever arm inactive. Distance traveled: 0.0m (need 5.0m), or provide dual antenna / IMU orientation.
[INFO] [1785234963.527430500] [fusioncore]: IMU lever arm auto-resolved from TF base_link -> bno_link: x=0.295 y=0.130 z=0.125 m
[WARN] [1785234968.667950365] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234973.672679016] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234978.868380793] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234984.072662087] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234989.268864147] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234994.469063320] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[WARN] [1785234999.470278655] [fusioncore]: GNSS fix rejected: VDOP_HIGH (hdop=2.00, d2=-1.0, threshold=16.3)
[INFO] [1785235067.705098116] [rclcpp]: signal_handler(SIGINT/SIGTERM)

seems that the fixes are rejected due to some DOP values, but in the NavSatFix structure are no DOP values, only in the nmea messages.
Perhaps another hint?
Greetings JR

[manankharwar]

@JR-pT Thank you for running it twice and posting both logs. The startup warning and the stale counters on their own is the new diagnostics doing their job, and "much better results" after syncing the clocks closes the original bug. GREAT!

On your open questions:

Why a clock gap on a single machine: one machine, but the driver chooses the time base. A constant +3.4 to +3.7 s offset is the classic signature of an IMU driver stamping from the sensor's internal tick counter (which is mapped to an epoch once at startup) rather than calling the node clock per message. Worth a look at how your BNO085 driver builds header.stamp; stamping with node time at read is the robust choice imo.

The x/y swap: expected, not a bug. With has_magnetometer: true your BNO085 gives magnetically referenced yaw, so FusionCore's output lives in a world East/North frame, while your wheel odometry frame is "x = wherever the robot faced at boot". If it faced roughly north, forward is +y in the fused frame and +x in odom, which is the ~90 degrees you saw. Quick test: start the robot facing east and the two should line up.

The VDOP_HIGH rejections: you are right that NavSatFix has no DOP fields. For NavSatFix input FusionCore derives pseudo-DOPs from the message covariance: hdop = sqrt of the horizontal variance, vdop = sqrt of position_covariance[8], both in meters. Your fixes are being vetoed because the driver reports altitude variance above 36 m² (vdop > the 6.0 default). Two things:

1. Set gnss.max_vdop: 20.0. Your robot runs publish.force_2d, so vertical quality should not veto a horizontally good fix. This parameter was added in 0.3.4 for exactly this case, found on my own rover.

2. Check what your driver actually publishes: ros2 topic echo /gps/fix --field position_covariance --once. Also note your earlier log showed hdop 0.06 (RTK-grade) while this run shows 2.0 to 2.4, so either the receiver had no RTK fix during this test or the driver's covariance is coarse. If [8] is a placeholder like 1e6, fix the driver's covariance rather than raising max_vdop further.

One more thing I can see that is much better in the report: the rejection warning only printed hdop, so a VDOP_HIGH rejection hid the offending value. As of a0566f5 on main it prints both, with a comment explaining the covariance-derived pseudo-DOPs.

If the next run looks good with GPS in the loop, I would love to see the trajectory. And if you are up for it, a short line on your setup for the adopters list would mean a lot, no pressure either way.

[JR-pT]

Test's are running, but again some notices and questions :-)
From time to time i find this in the log:
[WARN] [1785395974.932089390] [fusioncore]: STALE sensor rejections climbing (imu=2 encoder=0): the lagging sensor is NOT being fused. IMU stamp minus encoder stamp is currently -0.46s; sensors must share a time base. Fix the sensor drivers' clocks (stamp with node time, or chrony/NTP across machines).

Second, the max_vdop parameter seems working:
[INFO] [1785392457.582537113] [fusioncore]: FusionCore node created
[INFO] [1785392459.144582678] [fusioncore]: Configuring FusionCore...
[INFO] [1785392459.158661393] [fusioncore]: IMU lever arm: will auto-resolve from TF on first IMU message
[INFO] [1785392459.158720815] [fusioncore]: IMU gravity removal: disabled
[INFO] [1785392459.158736704] [fusioncore]: IMU frame override: bno_link
[INFO] [1785392459.158785680] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[INFO] [1785392459.410250604] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[INFO] [1785392459.410318379] [fusioncore]: PROJ: using first GPS fix as local reference origin
[INFO] [1785392459.411511210] [fusioncore]: Motion model: DifferentialDrive
[INFO] [1785392459.447462034] [fusioncore]: Dual antenna heading disabled.
[INFO] [1785392459.447537245] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=100Hz
[INFO] [1785392459.447645061] [fusioncore]: Autostart enabled: activating in 200ms.
[INFO] [1785392459.453342283] [fusioncore]: Activating FusionCore...
[INFO] [1785392459.453402345] [fusioncore]: --- TF Validation ---
[INFO] [1785392459.464200651] [fusioncore]: [OK] bno_link -> base_link
[INFO] [1785392459.464264278] [fusioncore]: ---------------------
[INFO] [1785392459.469340356] [fusioncore]: IMU topic: /imu
[INFO] [1785392459.475981703] [fusioncore]: Encoder topic: /vbot_base_controller/odom
[INFO] [1785392459.477738310] [fusioncore]: GNSS topic: /gps/fix (sensor_msgs/NavSatFix)
[INFO] [1785392459.539581293] [fusioncore]: Collecting 2.0s bias window before init...
[INFO] [1785392459.546873335] [fusioncore]: FusionCore active. Listening for sensors.
[WARN] [1785392459.647783405] [rcl_lifecycle]: No transition matching 3 found for current state active
[INFO] [1785392461.529754219] [fusioncore]: Bias window done: gyro=[0.0000,-0.0009,0.0000] accel=[0.0263,-0.0209,0.0612] rad/s, m/s²
[INFO] [1785392461.531449021] [fusioncore]: Filter initialized at t=1785392461.529
[INFO] [1785392461.643349156] [fusioncore]: GNSS lever arm auto-resolved from TF base_link -> gnss_link: x=-0.275 y=-0.110 z=0.130 m
[INFO] [1785392461.643445578] [fusioncore]: GNSS reference set: lat=47.989011 lon=12.013204
[INFO] [1785392461.651936712] [fusioncore]: Heading not yet validated: lever arm inactive. Distance traveled: 0.0m (need 5.0m), or provide dual antenna / IMU orientation.
[INFO] [1785392461.742074688] [fusioncore]: IMU lever arm auto-resolved from TF base_link -> bno_link: x=0.295 y=0.130 z=0.125 m
[WARN] [1785392544.545175363] [fusioncore]: STALE sensor rejections climbing (imu=2 encoder=0): the lagging sensor is NOT being fused. IMU stamp minus encoder stamp is currently -0.20s; sensors must share a time base. Fix the sensor drivers' clocks (stamp with node time, or chrony/NTP across machines).
[INFO] [1785392606.126980952] [rclcpp]: signal_handler(SIGINT/SIGTERM)

and third I started nav2 (some adjustments of your launch file were necessary, i.e. disable the collision monitor)
and i tried to set waypoints via followGpsWaypoints in python code and got following output:
[INFO] [1785397800.073623713] [waypoint_follower]: Converting GPS waypoints to map Frame..
[INFO] [1785397802.074127224] [waypoint_follower]: /fromLL service client: waiting for service to appear...
[INFO] [1785397803.074392088] [waypoint_follower]: /fromLL service client: waiting for service to appear...
do you have a hint for this problem?
Greets JR

[manankharwar]

Thanks for staying on this. Two of the three turned out to be bugs on my side, and you found both. All fixed and released as 0.3.5.

1. The /fromLL hang: my bug, not your config

FusionCore was advertising /fromLL with the wrong service type.

ROS 2 matches services on name and type. nav2_waypoint_follower has robot_localization::srv::FromLL compiled into its header:

// nav2_waypoint_follower/waypoint_follower.hpp:209
std::unique_ptr<nav2_util::ServiceClient<robot_localization::srv::FromLL,
  std::shared_ptr<nav2_util::LifecycleNode>>> from_ll_to_map_client_;

FusionCore was serving fusioncore_ros/srv/FromLL. Same service name, identical fields, different type. So from Nav2's point of view the service genuinely did not exist, and because a service client waits rather than failing, it sits there logging waiting for service to appear forever instead of telling you the type is wrong.

That is also why this survived so long: a manual ros2 service call /fromLL fusioncore_ros/srv/FromLL ... works fine, because you hand it the type yourself. Only a real client with the type compiled in exposes it, and you are the first person to run Nav2 GPS waypoints against FusionCore.

Identical fields under a different type name is not drop-in compatibility, so /fromLL now advertises robot_localization/srv/FromLL:

$ ros2 service list -t | grep fromLL
/fromLL [robot_localization/srv/FromLL]

Verified end to end with the real waypoint follower, not just by inspecting the type:

[waypoint_follower]: Converting GPS waypoints to map Frame..
[waypoint_follower]: Converted all 1 GPS waypoint to map frame
[waypoint_follower]: Received follow waypoint request with 1 waypoints.

One consequence worth stating plainly: fusioncore_ros now depends on robot_localization for that single .srv definition. No robot_localization code is linked or executed and you do not run a robot_localization node. Nav2 pulls the package in for exactly this reason anyway, so if you are doing GPS waypoints it was already installed. There was no dependency-free option: rclcpp has no generic service API, so serving a type means compiling against its definition.

2. The collision monitor: also my bug, and worse than it looks

You should not have had to work that out yourself. I went looking after your note, and the bundled Nav2 config was broken for any robot without a laser.

fusioncore_nav2.launch.py includes nav2_bringup's navigation_launch.py, which starts collision_monitor unconditionally (there is no launch argument to disable it) and includes it in the set of nodes its lifecycle manager brings up. My bundled nav2_params.yaml had no collision_monitor section, and that node refuses to configure without one:

[ERROR] [collision_monitor]: Error while getting parameters:
        parameter 'observation_sources' is not initialized

The reason it takes the whole stack down rather than just logging: the collision monitor sits in the command path. It subscribes to cmd_vel_smoothed and republishes to cmd_vel, which is what your base controller listens to. So planning and control run perfectly, produce velocities, and nothing reaches the wheels.

Fixed in 0.3.5, and it took some finding. observation_sources and polygons are both mandatory and an empty list is rejected by the parameter parser, so there is no way to declare "no sources". The shipped section instead declares one polygon and one source and disables both, which lets the node activate and hand velocity straight through. Verified against the shipped file: it reaches active, and 0.42 m/s published on cmd_vel_smoothed comes out of cmd_vel unchanged with the angular component intact.

Being blunt about what that is: a pass-through with no obstacle protection whatsoever. It matches the rest of that config, which assumes GPS navigation with no map and no lidar, but nobody should mistake a running collision_monitor for a working one. If you have a laser, set both enabled: True, point scan.topic at your real topic, and size the polygon to your robot. The docs say this explicitly now.

What did you end up doing on your side? If you have a collision_monitor block working against a real sensor, I would rather ship yours than the disabled placeholder.

3. The STALE warning was over-alarming, and that is on me

Look at your own numbers: imu=2 encoder=0, and it stayed at 2. Two dropped IMU samples across an entire run is nothing. The warning fired on any increase in the counter, so two stragglers produced the same wall of text as a completely broken clock and sent you hunting a problem you had already solved. Going from +3.44 s to -0.20/-0.46 s means your clock issue is fixed.

The warning now keys off a rate, fires only above 1 rejection per second sustained, states the measured rate, and prints the offset next to the max_measurement_delay it is being compared against. Isolated drops go to debug. A genuine time-base mismatch rejects nearly every sample, so it shows up as tens per second with counters climbing without bound. Two per run does not look like that.

If you want those last two fused instead of dropped, your -0.46 s is sitting right on the default max_measurement_delay: 0.5. A residual that size reads like real transport latency rather than clock skew, and retrodiction exists precisely for that: it rewinds the filter and fuses the late sample at its true timestamp instead of discarding it.

max_measurement_delay: 1.0

That should take you to zero. The same thing bit me on my own rover recently over a WiFi hotspot, where fixes were arriving 1 to 3 s late and every one was being thrown away as too old.

The test that should have existed

Worth saying, because the bug is less interesting than why nothing caught it. /fromLL shipped with the wrong type from v0.2.1 all the way through 0.3.4, across seven releases, and nothing complained. The only thing exercising it was a manual ros2 service call, which passes the type by hand and therefore always passes.

There is now a launch test that starts the real node, asserts the advertised type string, and calls the service with a robot_localization-typed client to check the conversion on both axes. I confirmed the test can actually fail by pointing it at the old type first. 114 tests total.

---

Everything above is in 0.3.5, released just now, so no source build needed:
https://github.com/manankharwar/fusioncore/releases/tag/v0.3.5

Out of curiosity, what is the robot? BNO IMU, vbot_base_controller, GPS waypoints outdoors. If you tell me the sensor set and what you are ultimately trying to get it to do, I can probably save you a few more rounds of this.