Navsat_transform diverges from GPS points

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

andrewpalmer
13 ●1 ●2 ●6

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

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)

Comments

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

Tom Moore ( 2017-02-21 03:28:46 -0600 )edit

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

Tom Moore ( 2017-02-21 03:30:16 -0600 )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 ( 2017-02-22 03:35:15 -0600 )edit

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

andrewpalmer ( 2017-02-27 07:11:43 -0600 )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 ( 2017-02-27 07:31:31 -0600 )edit

No worries, thanks for looking into it.

andrewpalmer ( 2017-02-27 07:41:23 -0600 )edit

add a comment

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

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

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

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

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 ( 2017-03-02 04:29:00 -0600 )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 ( 2017-03-02 04:49:57 -0600 )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 ( 2017-03-02 07:12:47 -0600 )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 ( 2017-03-02 07:13:45 -0600 )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 ( 2017-03-02 07:52:09 -0600 )edit

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

Tom Moore ( 2017-03-02 07:52:26 -0600 )edit

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

Tom Moore ( 2017-03-02 07:58:25 -0600 )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 ( 2017-03-02 09:00:50 -0600 )edit

see more comments

Navsat_transform diverges from GPS points

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Navsat_transform diverges from GPS points edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Navsat_transform diverges from GPS points