Estimations increase dramatically

[cmakelabs]

While rio runs well on the provided datasets, however when I try to launch it with my own rosbag, the estimate position and velocity seem to increase very rapidly, and converge to wrong numbers, as the robot real speed is 0.5 m/s (while the ones shown below will increase indefinitely as the bag is playing) :

pose: 
  pose: 
    position: 
      x: -160.44983290133843
      y: -14.600831309434668
      z: -47.69175573838993
    orientation: 
      x: -0.02611747507882512
      y: -0.032748962918190724
      z: 0.9235699756457728
      w: 0.38113471154690987
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 8.807195182027714
      y: 12.875944052624204
      z: -2.5869651071525483
    angular: 
      x: 0.05528554320335389
      y: -0.012584030628204346
      z: 0.0011943281861022117
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

There is also this series of warnings, that I could not debug properly, as the timings seem to be too close and the mismatch is only in the last fractions of time, not sure if this what causing the wrong estimation:

[ WARN] [1721221984.697609407]: Failed to split propagation, skipping CFAR detections.
[ WARN] [1721221984.697652414]: Split time: 1695304190.525563847
[ WARN] [1721221984.697669591]: Last IMU time: 1695304190.519120621
[ WARN] [1721221984.797759724]: Failed to split propagation, skipping CFAR detections.
[ WARN] [1721221984.797818564]: Split time: 1695304190.625555836
[ WARN] [1721221984.797849152]: Last IMU time: 1695304190.618970971
[ WARN] [1721221984.897949333]: Failed to split propagation, skipping CFAR detections.
[ WARN] [1721221984.898010507]: Split time: 1695304190.725679948
[ WARN] [1721221984.898040557]: Last IMU time: 1695304190.718575103

Would sincerely appreciate your answer

[rikba]

Hi there, most likely it diverges when only IMU messages are considered. Did you implement the buffer #6 ?

[cmakelabs]

Have not got time unfortunately, will ping you with pull-request once I got time to do it

[rikba]

I think a buffer will solve the problem with early arriving radar messages. I may have time to implement it at some point as well. You can contact me by mail at [email protected], and I can look at your data set.

[cmakelabs]

Thank you very much, I have sent you an email with the dataset attached

[Dcasadoherraez]

Any updates on the buffers @rikba?

[rikba]

Daniels PR should solve this issue. Sorry, did not have time to test and merge it. #12