robot_localization: navsat_transform's output is not aligned with odometry

asked 2017-02-20 03:19:25 -0500

Omer
26 ●7 ●8 ●10

updated 2017-02-22 06:03:23 -0500

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

Thanks!

edit retag flag offensive close merge delete

Comments

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.

Tom Moore ( 2017-02-21 03:27:42 -0500 )edit

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.

Omer ( 2017-02-22 06:06:25 -0500 )edit

Cool, I will check this out.

Tom Moore ( 2017-03-01 08:52:36 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2017-03-11 09:29:09 -0500

Omer
26 ●7 ●8 ●10

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?

edit flag offensive delete link

Comments

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.

Tom Moore ( 2017-03-14 05:15:54 -0500 )edit

add a comment

answered 2017-03-01 09:36:47 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2017-03-02 07:18:47 -0500

OK, so a few points:

For the IMU data, how are you accounting for magnetic declination? I assume you're handling it all within the IMU driver itself.
You have imu0_relative turned on for your EKF instance. That's going to make your orientations relative to the first IMU measurement.
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:

image description

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

image description

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

edit flag offensive delete link

Comments

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

Omer ( 2017-03-01 12:33:06 -0500 )edit

add a comment

robot_localization: navsat_transform's output is not aligned with odometry

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

robot_localization: navsat_transform's output is not aligned with odometry edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

robot_localization: navsat_transform's output is not aligned with odometry