robot_localization: navsat_transform's output is not aligned with odometry

Question

Hi,

I'm working with Parrot's Bebop2 (using bebop_autonomy) and trying to adjust robot localization for it. I created designated imu and twist topics that align with the ENU conventions and include covariances for all the relevant topics.

I currently have one instance of robot localization that fuses the continuous information (imu and twist), one instance of navsat_transform_node and later I would like to add a second instance of robot localization that fuses the outputs of the two latter.

When plotting the outputs of the first instance of robot_localization (/odometry/filtered) and navsat_transform, I get that they are rotated and the rotation angle is dependent on the drone's initial yaw angle. For example, when the drone is facing south during flight time, I get the following:

gps_vs_odom

This is my main launch file (for robot localization):

<launch>
  <arg name="tf_prefix" default="" />
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_continuous">
    <rosparam command="load" file="$(find bebop_localization)/config/robot_localization_continuous.yaml" />
    <param name="tf_prefix" value="$(arg tf_prefix)" />
    <remap from="odometry/filtered" to="odometry/filtered/continuous"/>
  </node>
  <include file="$(find bebop_localization)/launch/navsat_transform.launch" />
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global">
    <rosparam command="load" file="$(find bebop_localization)/config/robot_localization_global.yaml" />
    <param name="tf_prefix" value="$(arg tf_prefix)" />
    <remap from="odometry/filtered" to="odometry/filtered/global"/>
  </node>
</launch>

and this is my launch file for navsat_transform:

<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true" respawn="true">
    <rosparam command="load" file="$(find bebop_localization)/config/navsat_transform.yaml" />
    <remap from="imu/data" to="imu"/>
    <remap from="gps/fix" to="fix"/>
    <remap from="odometry/filtered" to="odometry/filtered/continuous"/>
  </node>
</launch>

Below are the config (yaml) files:

robot_localization_continuous.yaml:

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
print_diagnostics: true
publish_tf: true
publish_acceleration: false

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

twist0: speed
twist0_config: [false, false, false,
                false, false, false,
                true , true , true ,
                false, false, false,
                false, false, false]
twist0_queue_size: 2
twist0_nodelay: false
twist0_differential: false
twist0_relative: false

imu0: imu
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              false, false, false,
              false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8

navsat_transform.yaml:

frequency: 30

magnetic_declination_radians: 0

yaw_offset: 0.0

zero_altitude: false

wait_for_datum: false

My code is accessible under https://github.com/CearLab/bebop_autonomy/tree/indigo-devel/bebop_localization.

Thanks!

---

Originally posted by Omer on ROS Answers with karma: 26 on 2017-02-20

Post score: 0

---

Original comments

Comment by Tom Moore on 2017-02-21:
Can you post your launch file/config file for r_l, sample input messages, and some messages showing your IMU's output when facing north, south, east, and west? Images are also helpful.

Comment by Omer on 2017-02-22:
Thanks Tom for your response. I added more information to my post above. I also shared some sample messages, one bag file and the plotting script here.

Comment by Tom Moore on 2017-03-01:
Cool, I will check this out.

Answer 1

OK, so a few points:

1. For the IMU data, how are you accounting for magnetic declination? I assume you're handling it all within the IMU driver itself.
2. You have imu0_relative turned on for your EKF instance. That's going to make your orientations relative to the first IMU measurement.
3. Don't fuse the output of one EKF into the other. Instead, in the second instance, just fuse the same inputs as you did for the first, plus the output of navsat_transform_node.

EDIT 1: Here's a plot of your raw GPS data in Google Earth:

According to this data, you started in the top-left of the rectangle, then flew south, then east, then north, then west, then repeated the rectangle. The general shape maps decently to the fused IMU + velocity data (without GPS):

During the initial south traversal (the left side of the first rectangle), your drone's Y velocity was negative, implying that your vehicle was flying sideways, and in the direction of its right side:

header: 
  seq: 191
  stamp: 
    secs: 1487237096
    nsecs: 119077919
 frame_id: bebop/base_link
twist: 
  twist: 
    linear: 
      x: 0.132194340229
      y: -4.89818191528
      z: -0.0242414772511
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 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]

However, if your vehicle was flying sideways with negative Y velocity (i.e., right) and was also flying south, that means it must have been facing east. If it was facing east, and you corrected your IMU to be REP-103-compliant, the IMU should read approximately 0. However, during that same traversal (around the same time stamp of the above velocity message), I saved a sample IMU message:

header: 
  seq: 192
  stamp: 
    secs: 1487237096
    nsecs: 338866472
  frame_id: bebop/base_link
orientation: 
  x: 0.0387072677682
  y: 0.0236335869622
  z: -0.685949077348
  w: 0.726234854764
orientation_covariance: [1e-07, 0.0, 0.0, 0.0, 1e-07, 0.0, 0.0, 0.0, 1e-07]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

That heading equates to -86.684 degrees, whereas it ought to be 0.

In other words, either your body-frame velocity is wrong, or your IMU is wrong (or maybe both, I'd have to see updated data).

If you fly again, it'll help if you make note of which way the drone was flying (i.e., flying forwards/sideways, and to which direction).

Oh, and just a heads-up: your GPS data appears to be pretty severely lagged. It follows the same shape as the drone's path, but is delayed significantly.

---

Originally posted by Tom Moore with karma: 13689 on 2017-03-01

This answer was NOT ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Omer on 2017-03-01:\

1. I recently updated magnetic_declination_radians in navsat_transform.yaml. No significant change.
2. I repeated my experiment with imu0_relative false. The results are similar.
3. OK, I will change that. But that still doesn't explain why the output of navsat_transform_node is rotated, no?

Answer 2

Thanks Tom for this profound investigation. My problem was the frame_id in my speed topic; it should have been bebop/odom and NOT bebop/base_link since the speed is given relative to an earth reference frame. Now the trajectories are all aligned.

With regards to your comment on the lagging gps data: are you referring to the /fix topic or to the /odometry/gps topic? how will this influence robot localization? is there anything I can do to workaround it?

---

Originally posted by Omer with karma: 26 on 2017-03-11

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by Tom Moore on 2017-03-14:
You should update the original question, but I believe there was lag in the /fix topic. Also, I would not feed velocities to the filter in the odom frame. It has to then use the very transform it is generating to process the velocity data. The input velocities should be in the base_link frame.