0
$\begingroup$

Rosanswers logo

Hi everyone, I am trying to fuse 2 gps, 1 IMU, robot's odometry using robot_localization package. My navsat_transform launch file is as follows:

<?xml version="1.0" encoding="ISO-8859-15"?>
<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_novatel" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_novatel.yaml" />
      <remap from="odometry/gps" to="novatel/odometry"/>
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/navsat_fix"/>
      <remap from="gps/filtered" to="navsatfix/filtered"/> 
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/> 
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_mtig" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_mtig.yaml" />
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/fix"/>
      <remap from="odometry/gps" to="mtig/odometry"/>
      <remap from="gps/filtered" to="fix/filtered"/>
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/>
  </node>

  <node name="bl_novatel_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 base_link gps 100" output="log"/>
  <node name="novatel_fix_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 gps base_imu 100" output="log"/>
  <node name="front_left_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0.41 0 0 0 0 1 front_frame frame_left_link 100" output="log"/>
  <node name="front_right_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 -0.41 0 0 0 0 1 front_frame frame_right_link 100" output="log"/>
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rasberry_navigation)/workshop_rviz.rviz"/>
</launch>

My dual_navsat_ekf launch file is as follows:

   <?xml version="1.0" encoding="ISO-8859-15"?>
    <launch>
      <rosparam command="load" file="$(find rasberry_navigation)/config/dual_ekf_navsat_2gps.yaml" />
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true">
        <remap from="odometry/filtered" to="rl/odometry"/>  
      </node>
 
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
        <remap from="odometry/filtered" to="odometry/filtered_map"/>
      </node> 
      <include file="$(find rasberry_navigation)/navsat_transform_2_gps.launch"/>
    </launch>

My dual_navsat_ekf config file is as follows:

ekf_se_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: odometry
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false  #turn off for displaying in rviz

  imu0: imu/data
  imu0_config: [false, false, false,
                false, false, true,
                false, false, false,
                false, false, false,  #for rosbag angular velocity for yaw is zero
                true,  false, false]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

ekf_se_map:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

  # odom0: odometry
  # odom0_config: [false, false, false,
  #                false, false, false,
  #                true,  true,  false,
  #                false, false, false,
  #                false, false, false]
  # odom0_queue_size: 10
  # odom0_nodelay: true
  # odom0_differential: false
  # odom0_relative: true  #use odometry from robot

  odom0: rl/odometry
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false #turn off for displaying in rviz

  odom1: novatel/odometry
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false #turn off for displaying in rviz

  odom1: mtig/odometry
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false  #need to test
  odom1_relative: true #turn off for displaying in rviz

  imu0: imu/data
  imu0_config: [false, false, false,
                false, false, true,
                false, false, false,
                false, false, false,
                true,  false, false]
  imu0_nodelay: true
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

  initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

navsat_transform:
  frequency: 30
  delay: 0
  magnetic_declination_radians: 0.01338085759 
  yaw_offset: 1.570796327  # IMU reads 0 facing magnetic north, not east, assumed here
  zero_altitude: true
  broadcast_utm_transform: true
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false

My colleague recorded a rosbag and I am playing around with it. My colleague forgot to configure the Xsens to output acceleration so all accel measurements are zero. The Xsens is G700 and connected to a GPS receiver. The other GPS is Novatel.

I created a launch file for navsat_transform node to take in 2 gps topics and out put 2 odometry gps: novatel/odometry and mtig/odometry.

In my dual_navsat_ekf config file, the setup is as follows:

  1. ekf_se_odom: fusing robot's odometry, imu only. Output is rl/odometry
  2. ekf_se_map: fusing rl/odometry, imu, novatel/odometry and mtig/odometry

A sample of IMU output:

header: 
  seq: 82
  stamp: 
    secs: 1508938325
    nsecs:  36952018
  frame_id: "base_imu"
orientation: 
  x: 0.0114564243704
  y: -0.00510906334966
  z: -0.0420803017914
  w: 0.999035537243
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
linear_acceleration: 
  x: 0.0429290048778
  y: 0.128124058247
  z: 4.90896892548
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

A sample of robot's odometry:

header: 
  seq: 14881
  stamp: 
    secs: 1508938332
    nsecs: 625300772
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -2.70217663414
      y: 7.9073277398
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.374185456175
      w: 0.927353893822
  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: 0.619496662975
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.411721697445
  covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]

A sample of XSens GPS:

header: 
  seq: 217
  stamp: 
    secs: 1508938337
    nsecs: 995480060
  frame_id: "base_imu"
status: 
  status: 0
  service: 1
latitude: 53.2269481
longitude: -0.5471649
altitude: 106.353
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0

A sample of Novatel GPS:

header: 
  seq: 1093
  stamp: 
    secs: 1508938349
    nsecs: 857268095
  frame_id: "gps"
status: 
  status: 1
  service: 1
latitude: 53.2270033333
longitude: -0.547361666667
altitude: 57.89
position_covariance: [2.8899999999999997, 0.0, 0.0, 0.0, 2.8899999999999997, 0.0, 0.0, 0.0, 11.559999999999999]
position_covariance_type: 1

My question is:

  1. the output of ekf_se_map "jumps" too fast. A sample of /odometry/filtered_map is:

    header: seq: 1489 stamp: secs: 1508938355 nsecs: 525320053 frame_id: "map" child_frame_id: "base_link" pose: pose: position: x: -1439.24479633 y: 175.285682209 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.973889445396 w: 0.22702279213 covariance: [0.06133544000567167, 4.160398009897091e-05, 0.0, 0.0, 0.0, -0.002007318449623554, 4.1603980098970896e-05, 0.061228410533689256, 0.0, 0.0, 0.0, -0.0005269412079955384, 0.0, 0.0, 9.515801060911913e-07, -3.573125220704926e-14, -7.213858844717548e-14, 0.0, 0.0, 0.0, -3.573125220704925e-14, 9.998260721387534e-07, -5.3234361681745826e-20, 0.0, 0.0, 0.0, -7.21385884471755e-14, -5.323436168174739e-20, 9.998260721386725e-07, 0.0, -0.002007318449623553, -0.0005269412079955382, 0.0, 0.0, 0.0, 0.03609827941209225] twist: twist: linear: x: 0.921143671178 y: -0.302948592204 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.028470897715 covariance: [0.048388591169977936, 7.541061796259873e-08, 0.0, 0.0, 0.0, -5.28308719053119e-07, 7.541061796259885e-08, 0.05747913614730334, 0.0, 0.0, 0.0, -4.317307046027775e-06, 0.0, 0.0, 9.9947848851853e-07, 6.4698697093442575e-25, 7.7446768211025595e-25, 0.0, 0.0, 0.0, 6.46986970934426e-25, 9.99826072124398e-07, -5.670205565570612e-32, 0.0, 0.0, 0.0, 7.744676821102559e-25, -5.670201564987862e-32, 9.99826072124398e-07, 0.0, -5.283087190531205e-07, -4.3173070460277815e-06, 0.0, 0.0, 0.0, 0.05840292247496301]

I have no idea why this happened. Evidently, Novatel GPS jumped around but not this much. And as in #q206043 when I tried to fuse 1 GPS topic or 2 GPS, /odometry/filtered_map always "jumps" in the same manner.

Any idea how I could fuse 2 different GPS?


Originally posted by tuandl on ROS Answers with karma: 358 on 2018-03-08

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Okay, here are a few things which look irregular for your setup (there might be others, but these are obvious ones):

  1. In both of your navsat transform nodes the odometry source you are using is rl/odometry, which is coming from the output of your first EKF (ekf_se_odom). You should be using the output of your second EKF (ekf_se_map) as the input into the navsat transform node, which in your case would be odometry/filtered_map.
  2. In the configuration of your second EKF (ekf_se_map), you have both GPS odometry topics being fused under the same "odom1" heading. One of these needs to be changed to "odom2", otherwise r_l won't know you are using two separate GPS sources.
  3. Again in the configuration of your second EKF (ekf_se_map), you are fusing the output of your first EKF (rl/odometry). This can cause problems - typically what people do is just fuse the raw wheel odemetry & IMU measurements instead of the output from the first EKF.

I think in general it would probably be a good idea to start with a basic setup (i.e., with one instance of r_l, only fusing odometry & IMU data), and then once you're comfortable with the output you can add in more measurement sources. Trying to do everything at once will be a pain to de-bug, there are a lot of things which could be configured incorrectly.


Originally posted by stevejp with karma: 929 on 2018-03-08

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by tuandl on 2018-03-09:
@stevejp, thank you for your reply. It did fix my problem. I misinterpreted the documentation where it says

typically the output of a robot_localization state estimation node

I thought it requires r_l ouput to get odom->basel_link transform.

$\endgroup$

Your Answer

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