robot_localization:The /odometry/filtered frequency is not stable

asked 2020-01-10 04:21:48 -0600

april_zhao425
1 ●2 ●3 ●4

I'm going to fusing the data of visual odom and imu. Question 1 By calculating the difference between the timestamps of two adjacent frames, it was found that the fused data frequency was unstable, but the result of rostopic hz /odometry/filtered was stable.Why is that? How do you get a stable output frequency?

Question 2 I plotted the /odometry/filtered data and the visual odom data in Matlab.As shown below. http://bbit.top/lib/exe/detail.php?id... The x-coordinate is the timestamp, and the y-coordinate is the quaternion x. The blue dot is visual odom data, the green dot is /odometry/filtered data. The /odometry/filtered data is more unevenly distributed than the visual odom data. My config is shown below.

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>
  <param name="transform_timeout" value="0.0"/>

  <param name="odom0" value="/loop_fusion/odometry_rect"/>
  <param name="imu0" value="/mynteye/imu/data_raw"/>

  <rosparam param="odom0_config">[true, true, true,
                                  true, true, true,
                                  false,  false,  false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 false,  false,  false,
                                 false, false, false,
                                 true,  true,  false,
                                 false,  false,  false]</rosparam>

  <param name="odom0_differential" value="false"/>
  <param name="imu0_differential" value="false"/>

  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="false"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <param name="print_diagnostics" value="true"/>

  <param name="odom0_queue_size" value="5"/>
  <param name="imu0_queue_size" value="20"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="process_noise_covariance">[0.03, 0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0.03, 0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0.04, 0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0.03, 0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0.03, 0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0.06, 0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0.025, 0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0.025, 0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0.05, 0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0.002, 0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0.002, 0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0.004, 0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0.01, 0,    0,
                                              0,    0,    0,   0,    0,    0 ...

(more)

Comments

What does “unstable” mean? Can you provide numbers or examples. Also what’s the last commit hash you’re using?

stevemacenski ( 2020-01-10 15:53:39 -0600 )edit

@stevemacenski Sorry for the late reply. The following values are the frequencies obtained by calculating the difference between the timestamps of two adjacent frames. I set the frequency to 50, but they don't look like "stable" 50hz from the timestamp. And, some adjacent timestamps are the same, so the frequency obtained is inf. The last commit hash is 2d2976a737bbea00f638d9519783aaa9645a2c64. Author: Tom Moore tmoore@locusrobotics.com Date: Mon Aug 26 10:06:08 2019 +0100

Update issue templates

49.7020227754802 66.3571699784837 99.4028676383458 49.7757523972278 33.1675655158234 39.7930229689857 49.7267715508554 66.3571699784837 Inf 28.4251673940741 33.1675655158234 49.7261820078722 49.7515449854694 49.7757523972278 Inf 28.4172713537538 39.8088856407969 49.7261820078722 66.3571699784837 39.7930229689857 199.207029209214

april_zhao425 ( 2020-01-13 23:31:29 -0600 )edit

add a comment

answered 2020-01-22 03:48:24 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2020-01-24 04:02:07 -0600

This can be indicative of having old measurement data. If you get two identical time stamps in a row, it's because the filter published its state, and then received an old measurement, backed up in its history, inserted the measurement, and then fused the subsequent measurements, and re-published the last state. Try eliminating one input to see if that fixes the timing. If it does, then make sure your measurements are correctly stamped. You can also turn off the history buffer.

EDIT in response to comments.

First, it's probably a lot easier to post links to code, rather than copying and pasting it here. It also lets me know what code branch you're using. The code in question isn't from melodic-level, so I'll assume you're using kinetic-devel.

The longer answer requires us to walk through one cycle for the filter. This refers to the code branch you are using, rather than the latest stuff, which works on a timer (but would, I think, have the same behaviour).

At time t0, we call spinOnce(), which fires all the callbacks for the sensor messages we received. We throw them all into a measurement queue.
We walk through that queue, fusing each measurement in temporal order.
After fusing the measurements, we are now at time t1, because fusing the measurements obviously takes time. We publish the state at this time.
The elapsed time is e = t1 - t0. If the configured frequency of the filter is N, our cycle time is 1/N. But we ate up some of our cycle time fusing measurements, so we only need to sleep for 1/N - e seconds. I should note that the ros::Rate object does all this logic for you, but I wasn't aware of that when I wrote these, I think. But I digress...
Go back to step 1.

Hopefully you can see the problem here. In step 2, if there are three measurements in the first cycle and eight measurements in the second cycle, then there will be an uneven amount of time between thw two publications. We can't know how many measurements we'll have, so we can't know how long it will take to fuse them all.

There are (at least) two alternatives, though I'm open to hearing more:

Do all the fusing, then wait until the end of the cycle, and publish the last computed state. This is a bad idea, because you'd be publishing old information from time t1 at some later time, t2.
Sleep until the end of the cycle, and then predict the state using the filter's prediction stage to get the state at time t2, and publish that instead. This is also not great, for two reasons. First, the filter will have to go back in its history in the next cycle, because while you were sleeping (before the predict/publish), more measurements arrived in the queue. So now you have to rewind the ...

(more)

edit flag offensive delete link

Comments

Thanks for you reply. After that, I changed the imu0_queue_size parameter to make it larger. The problem of two identical time stamps has been solved. The reason I think is that my IMU frequency is too high. Now I find a piece of code in ros_filter.cpp that forces the computation time of each frame to be a fixed 1/ frequency.The relevant code is shown below. ros::Duration loop_elapsed_time = ros::Time::now() - loop_end_time; if (loop_elapsed_time > loop_cycle_time) { ROS_WARN_STREAM_DELAYED_THROTTLE(1.0, "Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed_time.toSec() << " seconds. Try decreasing the rate, limiting sensor output frequency, or " "limiting the number of sensors."); } else { ros::Duration sleep_time = loop_cycle_time - loop_elapsed_time; sleep_time.sleep(); } loop_end_time = ros::Time::now();

I think that's why the frequency of calculating timestamps is not stable, but the "rostopic hz" is stable. Please point out if I'm wrong.

april_zhao425 ( 2020-01-23 01:17:51 -0600 )edit

add a comment

robot_localization:The /odometry/filtered frequency is not stable

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization:The /odometry/filtered frequency is not stable edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization:The /odometry/filtered frequency is not stable