Ask Your Question
1

Navsat_transform diverges from GPS points

asked 2017-02-20 11:15:19 -0500

andrewpalmer gravatar image

updated 2017-02-22 03:39:09 -0500

Hi, I'm trying to use the navsat_transform_node as per the example provided in the robot_localization package. However, the output of the node always diverges (quite significantly) from the original GPS points (see image below - odom.csv is the odometry without GPS, odom_fused.csv incorporates the GPS, odom_gps.csv is the output of the navsat_transform_node, gps.csv is the input GPS points, and gps_filtered.csv is the filtered GPS output from navsat_transform_node). I used the Haversine formula to convert the GPS coordinates into the local odom frame (assuming that the first GPS point is the true start position of the robot). The odometry output (from ekf_se_odom) lines up nicely with the GPS data, so the heading information from the IMU seems to be correct. However, the output from navsat_transform is slightly rotated from the GPS data, and consequently the ekf_se_map output does not line up with the GPS data. Interestingly, the filtered GPS output is still quite close to the original GPS.

results

I'm pretty sure that the cause of the error is that the navsat_transform_node is using a heading that is slightly off to initialise itself. However, I have no idea how to go about fixing this. Any suggestions? I have uploaded the bagfile, launch and parameter files here - https://drive.google.com/drive/folder...

Thanks in advance for your help!


EDIT:

Configuration data:


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

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

  twist0: radar
  twist0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  twist0_queue_size: 10
  twist0_nodelay: false
  twist0_differential: false
  twist0_relative: false

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

  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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you please move your configuration data into the question, and post sample input messages for all inputs? Thanks.

Tom Moore gravatar imageTom Moore ( 2017-02-21 03:28:46 -0500 )edit

Also, how are you comparing the GPS data to the odometry data? What is shown in the plot?

Tom Moore gravatar imageTom Moore ( 2017-02-21 03:30:16 -0500 )edit

Hi Tom, thanks for your quick response. I have put the configuration data and sample input message into the question. For comparing the GPS and odometry data, I convert the GPS into the local odom frame using the Haversine formula. I also added a small explanation of the graph in the question.

andrewpalmer gravatar imageandrewpalmer ( 2017-02-22 03:35:15 -0500 )edit

@tom-moore Just wondering if you spotted anything obviously wrong with my setup?

andrewpalmer gravatar imageandrewpalmer ( 2017-02-27 07:11:43 -0500 )edit

Sorry for the delay! I'm in the middle of a rather busy period at work, but will investigate this as soon as I can. Thanks!

Tom Moore gravatar imageTom Moore ( 2017-02-27 07:31:31 -0500 )edit

No worries, thanks for looking into it.

andrewpalmer gravatar imageandrewpalmer ( 2017-02-27 07:41:23 -0500 )edit

1 Answer

Sort by » oldest newest most voted
4

answered 2017-03-01 08:21:42 -0500

Tom Moore gravatar image

updated 2017-03-13 05:16:11 -0500

OK, I looked into this more deeply, and I think the issue is a race condition between the EKF and navsat_transform_node.

First, it's important to note that the EKF is doing weighted averaging, so when it starts, the robot's state will have an orientation of 0, with a variance defined by the initial_estimate_covariance parameter. As such, the first measurement it receives from the IMU will not make the filter jump "all the way" to the IMU's value. In case I'm not being clear, here's a time-based illustration. Note that I actually took these values from your configuration and bag file.

  • Time 0: EKF has yaw of 0 with variance of 1e-9
  • Time 1: EKF receives IMU measurement with yaw value of 158.144 and variance 0.00030460. This gets fused, and thanks to the relative covariances (and process noise), the EKF outputs a yaw of 54.263 with variance 0.000122
  • Time 2: EKF receives IMU measurement with yaw value of 158.144 and variance 0.00030460. After fusion, the EKF outputs a yaw of 155.142 with variance 0.000129
  • Time 3: EKF receives IMU measurement with yaw value of 158.144 and variance 0.00030460. After fusion, the EKF outputs a yaw of 157.893 with variance 0.000218

...and so on. I'm not sure what the exact measurements were for every time step, but the point remains: when the filter first starts, it takes a few cycles for it to converge.

Now, navsat_transform_node needs to use the robot's initial pose in its world frame when it computes the transform from GPS (really, UTM) coordinates to the local world frame. This is useful for situations such as a robot that starts out indoors, drives around for a long time, then exits the building and starts getting GPS data. We have to back out what "would have" been the robot's real origin in UTM coordinates, so we need to know what it's world-frame pose is. All of this is just to say that the first pose that navsat_transform_node receives from the EKF is important.

I tried running against your bag many times, and I noticed this:

Run 1:

[ INFO] [1488377517.611041303, 1474298974.116952336]: Initial odometry pose is Origin: (1.6559646589424334433e-06 -2.2005416239386978415e-06 0)
Rotation (RPY): (0, -0, 2.7380782949293247519)
...
[ INFO] [1488377517.611119666, 1474298974.116952336]: World frame->utm transform is Origin: (-593683.64210805098992 -5811996.7498338120058 0)
Rotation (RPY): (0, 0, -0.040689807571359995486)

Run 2:

[ INFO] [1488377475.027280772, 1474298974.127188345]: Initial odometry pose is Origin: (2.1434320053116332058e-06 -2.2693153123689388409e-06 0)
Rotation (RPY): (0, -0, 2.7713237153209711039)
...
[ INFO] [1488377475.027519704, 1474298974.127188345]: World frame->utm transform is Origin: (-400136.08036857377738 -5828521.0477593624964 0)
Rotation (RPY): (0, 0, -0.0074387560166968369826)

Each time I run, the transform is a bit different. Note that the final map->utm transform differs by about 2 degrees in these examples, and that's just between those two runs. With the wrong timing, the difference could be more extreme.

So ... (more)

edit flag offensive delete link more

Comments

Hi Tom, thanks again for looking into this. So does the delay parameter not influence when it starts computing the transforms?

I tried using a larger initial_estimate_covariance and get better results ( http://i.imgur.com/oqvXjYn.png ).

andrewpalmer gravatar imageandrewpalmer ( 2017-03-02 04:29:00 -0500 )edit

The delay parameter ought to work as well (to be totally honest, I forgot I had added it!). Try increasing it. If that fails, I will move the sleep call to before the subscriptions are made.

Tom Moore gravatar imageTom Moore ( 2017-03-02 04:49:57 -0500 )edit

Hi Tom, I have done some experimentation varying the delay parameter and the initial uncertainties. Varying the initial uncertainty definitely helps a little bit, varying the delay just seems to change the time at which the GPS starts being included, but not the angle.

andrewpalmer gravatar imageandrewpalmer ( 2017-03-02 07:12:47 -0500 )edit

Here are some results for a larger portion of the data showing that there is still a large offset regardless of the parameters used (delay and initial yaw uncertainty) http://imgur.com/a/mBBaQ . I can upload the full bagfile for that if you want.

andrewpalmer gravatar imageandrewpalmer ( 2017-03-02 07:13:45 -0500 )edit

Nice experiments! Can you do me a favor and move the delay statement from line 176 to this line in navsat_transform.cpp and try again? It's odd how well the radar + IMU tracks the GPS data.

Tom Moore gravatar imageTom Moore ( 2017-03-02 07:52:09 -0500 )edit

May I see your Haversine code, or is that proprietary?

Tom Moore gravatar imageTom Moore ( 2017-03-02 07:52:26 -0500 )edit

Oh, and can you tell me the angle between the n_t_n output and the Haversine-generated path?

Tom Moore gravatar imageTom Moore ( 2017-03-02 07:58:25 -0500 )edit

The code for plotting the data is here - https://drive.google.com/open?id=0Bx8... . It just uses the haversine python package. The .csv are created using "rostopic echo -p /odometry/filtered > odom.csv", etc.

andrewpalmer gravatar imageandrewpalmer ( 2017-03-02 09:00:50 -0500 )edit

Your Answer

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

Add Answer

Question Tools

3 followers

Stats

Asked: 2017-02-20 11:15:19 -0500

Seen: 431 times

Last updated: Mar 13 '17