0
$\begingroup$

Rosanswers logo

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).

header: 
  seq: 235
  stamp: 
    secs: 1455846048
    nsecs: 782704923
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.060692385786
      y: 8.51043448483e-06
      z: 0.0
    orientation: 
      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]
twist: 
  twist: 
    linear: 
      x: -0.000285365096838
      y: 0.0
      z: 0.0
    angular: 
      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 -->
<launch>
    <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="imu_overrid_covariance.py" name="IOC" />
    <!--    <node pkg="rsl_rover" type="virt_yaw_sensor.py" 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>

    <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">[false, false, false,
                                      false, false, false,
                                      false,  false, false,
                                      false, false, true,
                      false, false, false]</rosparam> -->

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

      
      <param name="odom0_differential" value="false"/>
      <!--      <param name="odom1_differential" value="false"/> -->
      <param name="imu0_differential" value="false"/>

     
      <param name="odom0_relative" value="false"/>
      <!--      <param name="odom1_relative" value="false"/> -->
      <param name="imu0_relative" value="true"/>

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

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

      <!-- ======== ADVANCED PARAMETERS ======== -->

      
      <param name="odom0_queue_size" value="2"/>
      <!--<param name="odom1_queue_size" value="2"/> -->
      <param name="imu0_queue_size" value="10"/>

      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <!--      <param name="odom1_pose_rejection_threshold" value="5"/>
      <param name="odom1_twist_rejection_threshold" value="1"/> -->
      <param name="imu0_pose_rejection_threshold" value="0.3"/>
      <param name="imu0_twist_rejection_threshold" value="0.1"/>
      <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>

      <param name="debug"           value="false"/>
      <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <rosparam param="process_noise_covariance">[0.05, 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.06, 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.04, 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,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 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,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


      <!--  Placeholder for output topic remapping
      <remap from="odometry/filtered" to=""/>
      -->

    </node>
    

   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="false" output="screen">

      <param name="magnetic_declination_radians" value="0"/>
      <param name="yaw_offset" value="0"/>
      <remap from="/imu/data" to="/imu/data" />
      <remap from="/gps/fix" to="/fix" />
      <remap from="/odometry/filtered" to="/odometry/filtered" />

    </node> 

    <!-- Layer 2 Localization: Map Frame -->
    <node pkg="robot_localization" type="ekf_localization_node" name="map_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="map"/>

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

      <param name="odom0" value="/VehicleTach"/> 
      <param name="imu0" value="imu/data_filtered"/>
      <!--      <param name="odom1" value="/odometry/gps" /> -->
      <!--      <param name="odom2" value="/VirtYaw" /> -->
      <!--<param name="pose0" value="/poseupdate" /> -->


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

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

<!--      <rosparam param="odom1_config">[true,  true, false,
                                      false, false, false,
                                      false,  false, false,
                                      true, true, false,
                      false, false, false]</rosparam> -->

                  <!--      <rosparam param="odom2_config">[false,  false, false,
                                      false, false, false,
                                      false,  false, false,
                                      false, false, true,
                      false, false, false]</rosparam> -->

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

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>
      <!--      <param name="odom1_differential" value="true" /> -->
      <param name="odom2_differential" value="false" />
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative" value="true"/>
      <!--      <param name="odom1_relative" value="false"/> -->
      <param name="odom2_relative" value="false"/>
      <param name="pose0_relative" value="false"/>
      
      <param name="imu0_remove_gravitational_acceleration" value="false"/>

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

      <!-- ======== ADVANCED PARAMETERS ======== -->
      
      <param name="odom0_queue_size" value="2"/>
      <!--      <param name="odom1_queue_size" value="2"/> -->
      <!--<param name="odom2_queue_size" value="2"/>-->
      <param name="imu0_queue_size" value="10"/>

      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <!--      <param name="odom1_pose_rejection_threshold" value="5"/>
      <param name="odom1_twist_rejection_threshold" value="1"/> -->
      <!--      <param name="odom2_pose_rejection_threshold" value="5"/>
      <param name="odom2_twist_rejection_threshold" value="1"/>-->
      <param name="imu0_pose_rejection_threshold" value="0.3"/>
      <param name="imu0_twist_rejection_threshold" value="0.1"/>
      <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>

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

      <rosparam param="process_noise_covariance">[0.05, 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.06, 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.04, 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,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 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,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


      <remap from="odometry/filtered" to="odometry/filtered_discont"/>

    </node>

</launch>

Originally posted by baronep on ROS Answers with karma: 106 on 2016-03-17

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Tom Moore with karma: 13689 on 2016-05-09

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.