Localisation inaccurate because navsat_transform is incorrectly calculating odometry

Question

I'm trying to localise my robot in an outdoor environment by integrating GPS data in ROS. I'm running two ekf_localisation instances and one navsat_transform instance. In addition, I'm running an instance of /move_base so I can send ActionGoals to control the UGV. But in general, the localisation is inaccurate, and the origin of my /map and /odom frames move relative to each other, and relative to the real world. I'm not entirely sure why.

In addition, I'm trying to use navsat_transform to create a /map frame at a specified latitude/longitude whose 'Y' axis always faces true north. But I'm having difficulty achieving this as well, and I suspect that one problem is causing or at least contributing to the other.

Here is all the details and diagnostic info:

Robot: Clearpath Jackal UGV

OS: Ubuntu 14.04 on laptop, Ubuntu server on robot

ROS distro: Indigo Igloo

Launch files: ekf_plus_navsat.launch, odom_demo.launch

Frame tree: frames

rqt_graph_output: Nodes and topics

Example bag files: box_mapFrame.bag, box_mapFrame_odometry_and_sensors_only.bag, box_odomFrame.bag

Example sensor inputs: sample_imu_data.txt, sample_odometry_data.txt, sample_gps_data.txt

All of my sensor data conforms to REP-103 and REP-105. The IMU reports 0.0 facing True North but I use the yaw_offset parameter in navsat_transform to compensate accordingly

There is a variety of strange behaviour I'm seeing, so I'll try to break it down sequentially. In the bag files above (more coming soon) I mark out a 4x4 m grid starting the robot in one corner, manually drive it round the square and back to its origin. So the first time it moves around, it should definitely be moving in a (rough) 4x4 grid. I then remotely command the robot to move around the square by issuing it ActionGoals sequentially in either /map frame or /odom frame (I.e. 4,0 -> 4,4 -> 0,4 -> 0,0). Where the robot ends up is no-where near where it started, regardless of whether I'm giving issuing ActionGoals in the /map or /odom frame.

I've documented the various errors as best I can below. Any help or advice will be greatly appreciated.

UPDATE 1:

I can finally edit this again when I couldn't for a few days?! Anyway...

Here are my updated ekf_launch files: ekf_and_gps_localisation.launch

And separate launch files for each node: ekf_local.launch, ekf_global.launch, navsat_transform.launch

• IMU appears to be faulty

[EDIT]: Ignore any of my comments regarding the IMU - as it turns out the raw mag data from both /imu/mag and /imu/magnetic_field are un-calibrated which made me think the IMU wasn't working (it is). So lesson learned: use /imu/data only to get an earth-referenced heading. In fairness the documentation could be a little clearer on this issue.

• Transforms missing when launching EKF/navsat nodes

The various EKF and navsat nodes are VERY picky about how they are launched. If you run the ekf_and_gps_localisation.launch by itself after bootup, the EKF nodes will launch properly, but when it tries to launch navsat_transform node it freezes and produces this error:

  `[ERROR] [1466993920.331485830]: Could not obtain transform from odom->base_link`

At which point the only option is to hit Ctrl+C, stopping the frozen launch file and killing the EKF nodes in the process. But I don't get this if I run each node individually, or if I run ekf_and_gps_localisation.launch when the robot boots. However, these methods still produce their own errors, as a different transform (imu_link->base_link) appears to be missing. This error can be seen in rosconsole:

• navsat_transform node incorrectly calculates the datum origin and orientation

As I mentioned in the preamble, I want /map to be centred at a specific latitude/longitude and facing True North. My understanding, based on the GPS Integration Tutorial is that I can use the datum parameter to set the origin of /map and that the heading sub-parameter dictates the heading of /map from True North (irrespective of the current or initial pose of the robot). So, according to the tutorial and REP-103, heading of 0.0 will result in the 'Y' axis of /map facing True North and the 'X' axis of /map facing True East.

Unfortunately something is going wrong with navsat_transform when I try to set the datum. If I set the datum in the launch file, it doesn't set /map at the correct orientation or heading. If I use 'rosservice call /datum` after the node has launched then it at least calculates the origin correctly some of the time. Sometimes the origin is set to the correct latitude/longitude and other times it is out by a meter or so. You can actually see the incorrect calculation by watching the output in rosconsole:

Everything circled in red is output upon launching the navsat_transform node. Everything in green is output when I use rosservice call /datum to manually re-set the datum afterwards. I used the exact same latitude/longitude and heading in both cases, yet notice how the UTM coordinates given by the 'World frame->utm transform' message are different in each case. This obviously shouldn't happen, so navsat_transform is messing up the calculation somewhere. This issue is possibly related to the missing imu_link->base_link transform described in point 2.

As for the orientation, this is incorrect regardless of whether I set the datum in the launch via or via rosservice The 'Y' axis of /map always points to Magnetic North, irrespective of the value of heading in the datum parameter. As I said, the GPS Integration Tutorial led me to believe that a heading of 0.0 would result in /map facing True North, not Magnetic North. But even if I set heading to the value of the magnetic declination, it still points to Magnetic North. In fact, it doesn't matter what the value of heading is, the orientation of /map is still the same. I've also noticed that the yaw_offset parameter in navsat_transform has no effect on this as well. I would have expected that changing yaw_offset to 0.0 would result in the 'Y' axis of /map pointing West, rather than North (since this is the default state of most IMUs including mine), but this is not the case.

[EDIT]: As it turns out, the setting the datum in the launch file ends up calling rosservice anyway. So regardless of when it is set, the same method is used which means the only thing that's different between each call is the timing.

• Rviz doesn't display odometry properly

There is a BIG discrepancy between what Rviz displays and what is actually being measured, which I'm struggling to resolve. The below images shows what Rviz displays (right) and what you get if you plot each odometry set in Excel (left)

The data and the colours are exactly the same

• Blue for 'local' (which is /odometry/filtered/local measured in the /odom frame)
• Red for 'global' (which is /odometry/filtered/global measured in the /map frame)
• Green for 'gps' (which is /odometry/gps, also measured in the /map frame).

Note that Rviz is not displaying 'gps' odometry even though it has been added to the list of displayed data, and the the fact that 'global' is being displayed proves that the /map->/odom works properly.

The 'local' and 'global' data shouldn't overlap because they come from different frames. However 'global' and 'gps' should be identical because they are in the same frame and the gps data is being fused in the second EKF to produce the 'global' odometry data. Besides other things this clearly demonstrates that something is going very wrong with the second EKF.

[EDIT]: Tom has kindly pointed out that Rviz will transform each odometry data set into whatever reference frame you are currently viewing. Hence why /odometry/filtered/global and /odometry/filtered/local match in Rviz but not in Excel. This can be somewhat misleading but at least it proves that the /map->/odom transform is working correctly.

UPDATE 2:

As requested, I've collected some new data. I used the launch files given at the top of "Update 1". I manually drove the robot approximately along a 85m long line that runs parallel to the meridian (i.e. lines up with True North/South). I collected two bag files, one running north along the line, and one running south back to the starting point. I made sure that the /odom frame roughly pointed True North, so the topic /odometry/filtered/local best represents the actual path of the robot (with some drift). Both /odom and /map had their origins at the same point.

Bag files: line_north.bag, line_south.bag

---

Originally posted by M@t on ROS Answers with karma: 2327 on 2016-06-17

Post score: 0

---

Original comments

Comment by Tom Moore on 2016-06-20:
Can you please post sample input messages for each sensor? It can be a bit quicker sometimes to rapidly troubleshoot issues if I don't have to pull down the bag first.

Comment by Tom Moore on 2016-06-20:
Also, the map and odom frame will move away from one another. Also also, please turn off debug mode! You're taxing your CPU a LOT by turning that on, and outputting a massive text file somewhere. :)

Comment by Tom Moore on 2016-06-20:
It looks like a pretty solid launch file. Post some raw input data and I'll try to take a look at the bag soon.

Comment by M@t on 2016-06-21:
I now suspect that I haven't set up navsat_transform quite right because the /odometry/gps data doesn't match the raw gps input. Regardless, I've uploaded sample sensor data and more bag files. Thanks for pointing out the debug file size - serves me right for not reading the documentation properly.

Comment by M@t on 2016-06-21:
As for the launch file, I largely copied it from one you had posted in a different answer. I was struggling to understand the GPS integration tutorial, but having a full working example to look at helped enormously, especially regarding how the /map, /odom and /utm frames are set up.

Comment by M@t on 2016-06-23:
I can't edit my own question now for some reason, but I'm convinced that my IMU and navsat_transform are the cause of the problem. the origin of /map is never where it's supposed to be - its always out by at least 2 meters, often more.

Comment by M@t on 2016-06-24:
The XYZ output of a vector magnetometer points in the direction of the magnetic field, isn't that correct? So as the robot rotates 360, the XY component on that vector should pass through every quadrant of a circle shouldn't it? I.e (X+,Y+) -> (X-,Y+) -> (X-,Y-) -> (X+,Y-) from /imu/magnetic_field

Comment by M@t on 2016-06-24:
Mine doesn't, it skips a whole quadrant. There is never any situation where X is +ve and Y is -ve. Unless I'm fundamentally misunderstanding something (and I may be), I'm pretty sure this is not the behaviour I should be seeing, and it must be contributing to the problem.

Comment by M@t on 2016-06-24:
So if I spin the robot in a circle several times, here is the plotted data: XYZ, heading (should be oscillating between 0-360)

Comment by M@t on 2016-06-26:
Investigation regarding the imu is ongoing but it appears to be working correctly (for now). I have noticed that running the launch files on boot-up results in the /map frame origin not being set correctly, but it is set when I use the set_datum service.

Comment by M@t on 2016-06-26:
Running the whole ekf launch file after startup (as opposed to during) produces this error when launching navsat_transform:

[ERROR] [1466993920.331485830]: Could not obtain transform from odom->base_link

This is the problem - navsat_transform can't calculate /maps origin without that tf

Comment by M@t on 2016-06-28:
...no it's not. If I run each node one at a time, I don't get that error (most of the time). But I'm still seeing the same behaviour. /odom moves relative to /map and /map moves relative to the robot/move goals. Nothing about /map is accurate or consistent.

Comment by Tom Moore on 2016-06-29:
Hi Matt. Thanks for the info and updated files. I will look into this soon.

Comment by M@t on 2016-06-29:
It's the heading. I've tried changing the magnetic declination, yaw offset, datum heading... nothing affects the orientation of /map. /map always points to magnetic north, never true north like I want it to. Doesn't matter if I set the datum in the launch file or via rosservice.

Comment by M@t on 2016-06-29:
The heading is critical for the UTM calculation, so this implies that navsat_transform isn't getting the heading correct, which could be why the origin of /map moves all over the place.

Comment by asimay_y on 2016-06-30:
hi, it's very interesting. can you set use_odometry_yaw to true? so that the navsat node angle data is related with your odom frame. and question 2, did you try to set delay to longer time?

Comment by M@t on 2016-06-30:
@asimay_y: I could but then I wouldn't have any earth-referenced heading, the IMU is my only source. I've tried changing delay but it doesn't seem to have an effect.

Comment by M@t on 2016-06-30:
@Tom Moore: I've been looking through the source code and I see that navsat_transform uses a gps_frame_id -> base_link_frame_id conversion. I'm using an auxiliary GPS whose location on the robot does not match the tf frame of the original GPS, could this contribute to the behavior I'm seeing?

Comment by M@t on 2016-06-30:
Well, for all practical purposes it doesn't seem to make a difference. I've updated the navsat_link origin so it matches my auxiliary GPS (hacky but easier than creating a whole new frame for it). But the behavior is still the same and /map's orientation is still glued to magnetic north.

Answer 1

TL;DR: Calibrate your IMU... stay away from big metal objects... make sure your magnetic declination and yaw have the correct value and sign... and don't touch any of the more advanced parameters like datums until you get the basics working... and don't trust Rviz, if in doubt plot it in Excel or similar. Also, if you're having a similar issue and you're in a rush, maybe just skip to the graphs at the bottom of this essay and try to replicate them.

---

I've solved the problem. But it was caused by a combination of different factors, some of which were due to key misunderstandings about the way robot_localization handles reference frames and certain parameters. So I will document everything in full to try and I will update it with examples soon.

Problem

When I first began this escapade I identified 5 separate problems:

1. Incorrect datum: When trying to set the origin of the /map frame at launch using the 'datum' parameter, navsat_transform never calculated the position correctly (i.e. the origin is always a least a few meters from where it should be). However, when using rosservice call /datum <tab> later, it appears to work.
2. Locked orientation: I was trying to use three parameters in navsat_transform to change the orientation of /map: yaw offset, mag declination and heading. However nothing seemed to work. (/map points to Magnetic North by default, I wanted it to point to True North)
3. Faulty IMU: The IMU outwardly appeared to be faulty, based on looking at the filtered /imu/magnetic_field data.
4. Rviz: Rviz was reporting odometry data in ways that didn't seem to match the actual data or the actual path of the robot.
5. Bad EKF output: Finally but most importantly: The 2nd instance of the ekf_localization node which fuses odometry, imu and GPS data appeared to be seriously off as the odometry output didn't match reality or any other data inputs.

Cause

The cause for each of these problems is as follows:

1. Incorrect datum: I'm not 100% sure about this as I still haven't fixed it, but my imu_link -> base_link tansform occasionally fails, and so what I think is happening is that navsat_transform is getting a bad transform when it subscribes to the /imu/data topic to calculate the LL -> UTM transform, which then ruins the calculation of /map's origin. In addition, there is a bit of natural error in the IMU's heading, so this will only contribute to the problem.
2. Locked orientation: It is currently impossible for navsat_transform to in any way change the orientation of the /map frame. There are however three parameters that may appear to do so. These parameters do not change the visible orientation of the frame but instead add an angle offset to all the odometry that navsat_transform reports in that frame. So you will never see the 'Y' axis of /map point anywhere but Magnetic North. The parameters are:

• heading: This, along with the 'latitude' and 'longitude' values define the datum parameter, which let you re-set the orientation of the /map frame. However, the third parameter 'heading' does not affect the actual heading of the /map frame, instead it simply assumes that your robot starts at that heading within the /map frame.
• mag declination: This parameter exists purely so that navsat_transform can calculate where True North is for the purposes of the LL -> UTM conversion, it has no effect on the orientation of /map whatsoever but it does massively affect the odometry in that frame if you get it wrong (see below).
• yaw offset: This parameter exists purely to compensate for the fact that most IMUs read 0 when facing North, whereas ROS conventions expect 0 facing East (see REP-105). Again, this parameter has no effect on the orientation of /map but WILL affect the odometry calculated in that frame.

I keep saying that these parameters won't change the orientation, but add an angle to the odometry that navsat_transform reports in that frame. By this I am referring to /odometry/gps which is the key output from navsat_transform that you want to feed to an instance of ekf_localization if you want to fuse your GPS data with your IMU and odometry (read: wheel encoder) data. 3. Faulty IMU: For some of my data, the IMU appeared to be erratic or otherwise didn't read 0 when facing North properly. When trying to debug this issue I ended up looking at the raw magnetometer data which I incorrectly assumed had been calibrated (it had not been, the data was calibrated at a later stage). This ended up being a red herring which wasted a lot of time. The real problem was that the effect of metal objects on my IMU's magnetometer was far greater than I anticipated. Some of my data had been collected in a car park, and as it turns out, a few nearby cars were affecting the IMU. And because all my fused odometry data used the IMU for orientation, this made the odometry data erroneous as well. 4. Rviz: Rviz takes whatever odometry data is being displayed and converts it into the reference frame you are currently viewing. This means you can't actually view the data in separate reference frames properly, which makes debugging issues a lot harder if they aren't all in the same reference frame. It can effectively made bad data seem OK but at the very least does sort of confirm that the relative transform works properly. 5. Bad EKF output: And finally, the biggest issue of them all...was caused by a combination of all of the above. My initial launch files used a positive magnetic declination instead of a negative one, and even though I corrected this early on, the IMU distortion, me playing around with the datum and yaw parameters meant that there were so many factors distorting my EKF outputs that every time I did correct something, it didn't appear to improve the output so I promptly reverted the fix.

Solution

So the most effective solution I have found to each of these problems is:

1. Incorrect datum: Be careful about how and when you use the datum parameter. I personally have found that the most effective method is to reset the frame and then check the odometry in that reference frame. For example if you reset the frame to the current location of your robot, the odometry should read 0,0,0. If not, try resetting it again.

2. Locked orientation: The orientation of the /map frame can't actually be changed with navsat_transform so don't even try - otherwise you're in for a world of sadness and disappointment.

3. Faulty IMU: If something about its data looks funky re-calibrate it in an open field away from any potential disturbances. Also, when moving around STAY AWAY FROM GIANT METAL OBJECTS.

4. Rviz: Avoid using Rviz to debug issues across different reference frames. The mental gymnastics ain't worth it and will confuse you further. For clarity, print the odometry in each reference frame to a .csv file and plot it in something like Excel. Use this command to output the contents of a ros topic to a .csv file:

   rostopic echo -b filename.bag -p /topic > output.csv

5. Bad EKF output: When running nodes from robot_localization its best to start with the basics and avoid some of the more complicated parameters like datum and imu0_relative`. Make sure you steer clear of any interference (metal objects for your IMU, LOS-blocking objects like trees or buildings for your GPS), ideally starting out in the middle of an open park or field. Also triple-check that you have the correct magnetic declination sign and yaw offset. If you're not sure, the easiest way to check is to conduct several test runs, each one changing a different parameter.

Extra explanation and advice to anyone with a similar problem

TL;DR: look at the graphs below.

Basically, all of this boils down to making sure that every sensor input you're trying to fuse is being reported in the same reference frame, or at least in reference frames with the same orientation. The mag declination and yaw offset parameters in navsat_transform exist purely to make sure that the UTM coordinates of the robot can be correctly converted into an odometry stream with the correct orientation. My problem, was that I didn't have these parameters correct, so the /odometry/gps stream was reported at an angle. My system may not be configured correctly, because according to REP-103 and REP-105, the /odom and /map reference frames should be orientated with their Y axis facing Magnetic North and their X axis facing Magnetic East. And indeed, this was the assumption I was operating under and even though I checked this at the time but got it wrong.

As it turns out, for my setup, for whatever reason, the /odom and /map frames align orient themselves at initialization to point with the X axis facing North, and their Y axis facing West. So even though on paper I had the correct mag declination and yaw offset, they didn't produce a odometry stream that matched everything else. This is definitely wrong, but as long as you're consistent with all you angular offsets, everything should come out in the wash (I hope - it has so far). Basically, if you're ever in doubt as to what should be the correct values, do what I did.

The yaw offset will (almost certainly) be either 90 degrees or 0. Your mag declination (if you've got the correct angle fro your region) will be negative or positive. This gives you four possible combinations. SO, drive your robot in a square, log the data from each odometry stream, plot it in excel and just use whatever combination produces the correct graph. E.G:

If you get the sign on the mag declination correct but the yaw offset incorrect, you get:

If you get the sign on the mag declination incorrect but the yaw offset correct, you get:

If you get both incorrect, you get: (Which, annoyingly looks better than getting just one wrong)

And finally, if you get the combination right, everything should line up like so:

---

Originally posted by M@t with karma: 2327 on 2016-07-10

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by asimay_y on 2016-08-28:
good work!! excellent. I will try on my system later. Does this solve the related drfit issue of odom & map if data correct?

Comment by M@t on 2016-08-30:
Doesn't solve it completely, but it reduces the drift to a reasonable level. If the odometry in the /map frame (global - red) and /odom frame (local - blue) are wildly different eg because of the above. Then the /odom and /map frames will appear to drift severely in RViz

Comment by asimay_y on 2016-08-31:
I tried to use your bag file to experiment, so from your bag file you are in new zealand, so your mag declination should be about -23.82°, right? I want to know the final right data combination of your experiment. Thanks!

Comment by asimay_y on 2016-08-31:
plot with your second bag file

Comment by M@t on 2016-08-31:
Ah sorry, I should have mentioned that the bag files in the question are wrong, so don't use them - I obviously had the wrong variable combination. Also, each plot in my answer comes from a different bag file (I'm having difficulty running the ekf nodes over recorded data). And yes, -23.8°

Comment by asimay_y on 2016-09-06:
hi, M@t, I got very bad graph from my robot. screen shot is here:excel data . What do you think the issue is? (my running way is : north -> west-> south ->east. ) is this the INS equipment error?

Comment by asimay_y on 2016-09-06:
Could you please share me your local odometry msg code? Thanks very much! I want to compare and check is there something wrong in my odom code...My email is [email protected]

Comment by asimay_y on 2016-09-06:
even though i change the yaw offset in nav_sat config file, the plot data seems very strange, at here:plot data, I think the INS equipment has something wrong maybe. how do you think? thanks!

Comment by M@t on 2016-09-06:
I suggest you create a new question for this, because it definitely looks like there's more going on here. In both charts the GPS/global data looks good, but that local data looks like your IMU or wheel odometry is erratic, and there's big jumps between data points which shouldn't happen.

Comment by M@t on 2016-09-06:
Also not sure what you mean by local odometry msg code. But it looks like the data you're fusing in your local EKF is being disrupted by something. Your IMU may not be calibrated or something nearby is disrupting it - big metal objects like cars can make my IMU behave in a similar way.

Comment by asimay_y on 2016-09-13:
hi,M, I find something interesting, and I think your solution above is not ending up. I can get the four picture above, but I don't think this is over, because the actual moving orientation is not right compared to the axes of excel although the 3 line can overlap. according to package imu_filter,

Comment by asimay_y on 2016-09-13:
"For historic reasons, the old default is "nwu" (North-West-Up)". that why your odom&map (or Jackey) has NWU issue. but for r_l package only accept ENU imu data, so we still need transform for output of imu_filter package. for more info please refer to REP-145.

Comment by asimay_y on 2016-09-13:
if use default output orientation data of imu_filter package, I think this drawing is right: here . of course we need transform to let the three line overlap and the axes of position in excel should same with the actually moving orientation.

Comment by asimay_y on 2016-09-13:
this means positive x should point to east, and positive y should point to north.

Comment by M@t on 2016-09-13:
If you're still having issues then you should definitely post your own question for it, these comments are too short to debug this sort of issue. From the latest graph you posted it looks like you just need to change the navsat_transform yaw_offset parameter from 0 to 90 degrees (or vice versa).