ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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.


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.
  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: Don't use the datum parameter during launch. It actually is easier to debug and think about the reference frames if both the /odom and /map frame align in position and orientation, which will happen naturally if you run all your 'ekf_localizationandnavsat-transform` nodes at launch.
  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.

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.


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 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: Don't Be careful about how and when you use the datum parameter during launch. It actually parameter. I personally have found that the most effective method is easier to debug and think about the to reset the frame and then check the odometry in that reference frames if both the /odom and /map frame align in position and orientation, which will happen naturally frame. For example if you run all reset the frame to the current location of your 'ekf_localizationandnavsat-transform` nodes at launch. 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.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.

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.


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: image description

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

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

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

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: image description

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

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

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