Ask Your Question

How to use robot_localization /odometry/filtered

asked 2016-03-17 20:14:29 -0500

baronep gravatar image

updated 2016-03-18 13:23:59 -0500

So I have two robot_localization nodes setup. The first fuses the vehicle odometry and the imu data. The second will add in GPS and hector_slam pose estimates.

My question is, should I fuse the output of the first node (/odometry/filtered) into the second node or should I re-fuse the vehicle odometry and imu data? If the former, the second (map) robot_localization node never publishes anything if I fuse the output of the first directly. (Also, the navsat node doesn't work with the /odometry/filtered data either).

Here is a sample /odometry/filtered message (output from the first node).

  seq: 235
    secs: 1455846048
    nsecs: 782704923
  frame_id: odom
child_frame_id: base_link
      x: 0.060692385786
      y: 8.51043448483e-06
      z: 0.0
      x: 0.0
      y: 0.0
      z: 7.45214107357e-05
      w: 0.999999997223
  covariance: [0.39148006559512866, -1.0221015344746652e-08, 0.0, 0.0, 0.0, 7.160358808761269e-08, -1.0221015344746746e-08, 0.39152977031124553, 0.0, 0.0, 0.0, 6.453229401104118e-05, 0.0, 0.0, 3.332912064876806e-07, 1.341899911334871e-24, 5.233156817556127e-19, 0.0, 0.0, 0.0, 1.3418999113348712e-24, 3.3324912207294784e-07, -1.6669589116712293e-32, 0.0, 0.0, 0.0, 5.233156817556124e-19, -1.6669590914679116e-32, 3.3324912207294784e-07, 0.0, 7.160358808761269e-08, 6.453229401104114e-05, 0.0, 0.0, 0.0, 0.0773158946732772]
      x: -0.000285365096838
      y: 0.0
      z: 0.0
      x: 0.0
      y: 0.0
      z: 5.54621444365e-05
  covariance: [0.0019138416154638686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0019138416154638686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.332701589929017e-07, 1.9950504214512513e-31, 1.2764323815697058e-27, 0.0, 0.0, 0.0, 1.9950504214512495e-31, 3.330812024673111e-07, -1.34664748772665e-39, 0.0, 0.0, 0.0, 1.276432381569705e-27, -1.5229681057398406e-38, 3.330812024673111e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04978277263316132]

And here is my localization launch file ...

<!-- Launch file for ekf_localization_node -->

<!-- Layer 1 Localization: Odometry Frame -->
    <node pkg="tf" type="static_transform_publisher" name="tach_odom123" args="0 0 0 0 0 0 1 odom tach_odom 100" />
    <node pkg="rsl_rover" type="" name="IOC" />
    <!--    <node pkg="rsl_rover" type="" name="VirtYaw" output="screen"/> -->
    <node pkg="imu_complementary_filter" type="complementary_filter_node" name="complementary_filter_node" >
    <remap from="imu/data_raw" to="imu/data_cov" />
    <remap from="imu/mag" to="imu/mag" />
    <remap from="imu/data" to="imu/data_filtered" />
    <param name="do_bias_estimation" value="true"/>
    <param name="do_adaptive_gain" value="true"/>
    <param name="use_mag" value="false"/>
    <param name="gain_acc" value="0.01"/>
    <param name="gain_mag" value="0.01"/>

    <node pkg="robot_localization" type="ekf_localization_node" name="odom_localization" clear_params="true" output="screen">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <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="odom0" value="/VehicleTach"/>
      <param name="imu0" value="imu/data_filtered"/>

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

                  <!--      <rosparam param="odom1_config ...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2016-05-09 19:38:43 -0500

Tom Moore gravatar image

I don't recommend fusing the output of the odom->base_link instance into the map->odom instance. The main reason is that the output of the first EKF will be in the odom frame. To get it into the map from of the second EKF, it needs a map->odom transform, which is exactly what the second EKF is meant to be producing. You get into a chicken-or-egg problem. I usually just fuse the raw inputs (better to use velocities int his case).

I'd be careful with the threshold params. You can end up rejecting valid measurements if they're not tuned well.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2016-03-17 20:14:29 -0500

Seen: 3,799 times

Last updated: May 09 '16