0
$\begingroup$

Rosanswers logo

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/folders/0Bx8pkplHLcVUYVA1M2RGOWd6V2M?usp=sharing

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,    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
  print_diagnostics: true
  debug: false

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

  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

  odom0: odometry/gps
  odom0_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: 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: 1.0
  magnetic_declination_radians: 0.0 # It is already incorporated by the imu/gps unit 
  yaw_offset: 0 
  zero_altitude: false
  broadcast_utm_transform: true
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false

Sample messages:

/radar:


header: 
  seq: 1074189
  stamp: 
    secs: 1474298977
    nsecs: 705579613
  frame_id: base_link
twist: 
  twist: 
    linear: 
      x: 0.5234375
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01234567901234568, 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]

/imu:


header: 
  seq: 23381
  stamp: 
    secs: 1474298977
    nsecs: 696676291
  frame_id: base_link
orientation: 
  x: -0.00420055072755
  y: -0.0023727978114
  z: 0.983819246292
  w: 0.179098829627
orientation_covariance: [2.7416e-05, 0.0, 0.0, 0.0, 2.7416e-05, 0.0, 0.0, 0.0, 0.000304607]
angular_velocity: 
  x: -0.00396632356569
  y: -0.000390391069232
  z: 0.0021740719676
angular_velocity_covariance: [3.046e-06, 0.0, 0.0, 0.0, 3.046e-06, 0.0, 0.0, 0.0, 3.046e-06]
linear_acceleration: 
  x: 0.268662780523
  y: -0.0573329739273
  z: 9.85708904266
linear_acceleration_covariance: [6.1591e-05, 0.0, 0.0, 0.0, 6.1591e-05, 0.0, 0.0, 0.0, 6.1591e-05]

/gps:


header: 
  seq: 22829
  stamp: 
    secs: 1474298977
    nsecs: 697491504
  frame_id: base_link
status: 
  status: 0
  service: 0
latitude: 52.6131248474
longitude: 12.8844614029
altitude: 0.0
position_covariance: [8.7025, 0.0, 0.0, 0.0, 8.7025, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 2

Originally posted by andrewpalmer on ROS Answers with karma: 13 on 2017-02-20

Post score: 1

Original comments

Comment by Tom Moore on 2017-02-21:
Can you please move your configuration data into the question, and post sample input messages for all inputs? Thanks.

Comment by Tom Moore on 2017-02-21:
Also, how are you comparing the GPS data to the odometry data? What is shown in the plot?

Comment by andrewpalmer on 2017-02-22:
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.

Comment by andrewpalmer on 2017-02-27:
@tom-moore Just wondering if you spotted anything obviously wrong with my setup?

Comment by Tom Moore on 2017-02-27:
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!

Comment by andrewpalmer on 2017-02-27:
No worries, thanks for looking into it.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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, to fix this, we can do one or both of the following:

  1. Change your initial_estimate_covariance for yaw to be a value larger than 1e-9 in the map EKF instance. This will cause the yaw value to converge faster
  2. I can add a parameter to navsat_transform_node that causes it to sleep for a parameterized amount of time before it attempts to subscribe to any data and start computing transforms.

EDIT 1 in response to comments:

The difference in headings between the n_t_n position and the Haversine formula-calculated position is around 1.64 degrees. n_t_n calculates the local-frame position by converting the GPS coordinates to UTM coordinates, and then calculating a transform from the UTM frame to the robot's world frame (map or odom). However, that transform is dependent on a single reading from the robot's IMU, as well as the robot's pose at the time we compute the transform, and IMUs will have non-zero heading/yaw error. In this case, 1.64 degrees is a completely believable error for a magnetometer, so I'm not really surprised.

Note, however, that the relative motion is still the same from point A to point B in either case, so I believe that if you use n_t_n as intended and (a) fuse the /odometry/gps data into your EKF, and (b) use the /gps/filtered position as your global position, everything should work. Your plots and my tests seem to confirm this:

image description

In this plot, red is the raw GPS data, and yellow is the /gps/filtered output from n_t_n. This works because (a) the relative motion is correct, and (b) the same transform is used to convert to GPS coordinates that we use to convert from GPS coordinates.

Likewise, the /odometry/gps data is going to be consistent with the robot's world frame, even it doesn't align perfectly with the Haversine formula-generated position.

This also explains why the raw EKF output (sans GPS data) seems to align well: the IMU's heading error is likely Gaussian, so as the robot drives along, the filtered heading is more accurate than the single measurement we used for calculating the map->utm transform. Since you've made your IMU ROS-compliant and you are fusing the IMU's yaw in your odom-frame EKF, it lines up well.

Even if we were to make n_t_n use the Haversine formula instead, we'd need to know the robot's heading in the global/earth frame in order to transform between the robot's local frame and the global/earth frame. Note that, as not everyone will want to fuse the IMU's yaw in their robot's local-frame state estimate, we can't assume that our local frame is always aligned with the global/earth frame. So for some robot, driving straight forward from its starting pose may mean it has a pose, after some time, of (10, 0), whereas in the global/earth frame it could be at, e.g., (6.32, 7.75). The only way we can get the transform between those two frames is to know the orientation difference between them, and that is going to require an earth-referenced heading, which has to come from an IMU.

One thing you can try is to use the use_odometry_yaw parameter in n_t_n and increase the delay parameter. This will let the EKF fuse a bunch of yaw measurements from the IMU, and then n_t_n will use the (weighted) average of those measurements to generate the transform, which ought to be more accurate than just using one.

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

This answer was ACCEPTED on the original site

Post score: 4

Original comments

Comment by andrewpalmer on 2017-03-02:
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).

Comment by Tom Moore on 2017-03-02:
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.

Comment by andrewpalmer on 2017-03-02:
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.

Comment by andrewpalmer on 2017-03-02:
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.

Comment by Tom Moore on 2017-03-02:
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.

Comment by Tom Moore on 2017-03-02:
May I see your Haversine code, or is that proprietary?

Comment by Tom Moore on 2017-03-02:
Oh, and can you tell me the angle between the n_t_n output and the Haversine-generated path?

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

Comment by andrewpalmer on 2017-03-02:
I moved that delay statement forward, but no change in the behaviour.

Comment by andrewpalmer on 2017-03-02:
I have other datasets where the odometry output does not line up as well with the GPS track. However, the error in those cases was due to an incorrect initial heading (the IMU was in a different mode and the heading would drift while stationary).

Comment by andrewpalmer on 2017-03-02:
And the angle difference between the input GPS path (using Haversine) and the output of the navsat_transform node is 0.0286 radians.

Comment by andrewpalmer on 2017-03-13:
Hi Tom, thanks again for the responses. I tried using the use_odometry_yaw setting, but that makes no difference. I also just tried using the UKF instead of EKF, that actually gives better results (in terms of the /gps/filtered lining up with the /gps topic).

Comment by andrewpalmer on 2017-03-13:
However, the odometry output of n_t_n is still offset by that small angle. Still very strange given the good performance of the odometry without the GPS.

Comment by Tom Moore on 2017-03-14:
No, as I pointed out above, the odometry without GPS is based on many yaw measurements from the IMU. The utm->odom transform that n_t_n uses is based on one yaw measurement, so all future poses that we pass through that transform will also be offset.

Comment by Tom Moore on 2017-03-14:
My point is that being offset from the Haversine data is totally inconsequential. The local-frame pose is still consistent with your data. As a test, stop fusing yaw from your IMU in your EKFs. Now make your robot face, say, southeast, drive, and run you graphs again.

Comment by Tom Moore on 2017-03-14:
The goal of n_t_n is not to produce poses that are the same as Haversine or UTM. It's to produce poses that are consistent with your robot's map frame. You just happen to have (and fuse) yaw data that is the same as the UTM frame.

Comment by Tom Moore on 2017-03-14:
...and the reason they are not exactly the same in this instance is because the single IMU measurement that is used to generate the map->utm transform has some error.

Comment by andrewpalmer on 2017-03-17:
Ah ok, thanks for the explanation.

Improve this answer
$\endgroup$

Your Answer

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