Localisation inaccurate because navsat_transform is incorrectly calculating odometry

asked 2016-06-17 00:29:27 -0600

2327 ●55 ●80 ●81

updated 2016-07-04 23:44:10 -0600

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 ...

(more)

edit retag flag offensive close merge delete

Comments

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.

Tom Moore ( 2016-06-20 02:16:34 -0600 )edit

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. :)

Tom Moore ( 2016-06-20 02:18:40 -0600 )edit

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.

Tom Moore ( 2016-06-20 02:20:03 -0600 )edit

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.

M@t ( 2016-06-21 17:45:24 -0600 )edit

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.

M@t ( 2016-06-21 17:48:57 -0600 )edit

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.

M@t ( 2016-06-23 20:56:55 -0600 )edit

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

M@t ( 2016-06-24 05:00:09 -0600 )edit

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.

M@t ( 2016-06-24 05:07:37 -0600 )edit

see more comments

3 Answers

Sort by » oldest newest most voted

answered 2016-07-10 18:57:19 -0600

M@t

2327 ●55 ●80 ●81

updated 2016-08-27 05:16:11 -0600

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:

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.
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)
Faulty IMU: The IMU outwardly appeared to be faulty, based on looking at the filtered /imu/magnetic_field data.
Rviz: Rviz was reporting odometry data in ways that didn't seem to match the actual data or the actual path of the robot.
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:

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.
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 ...

(more)

edit flag offensive delete link

Comments

good work!! excellent. I will try on my system later. Does this solve the related drfit issue of odom & map if data correct?

asimay_y ( 2016-08-28 21:49:23 -0600 )edit

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

M@t ( 2016-08-30 19:19:54 -0600 )edit

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!

asimay_y ( 2016-08-31 02:36:48 -0600 )edit

plot with your second bag file

asimay_y ( 2016-08-31 02:48:40 -0600 )edit

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°

M@t ( 2016-08-31 17:08:27 -0600 )edit

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?

asimay_y ( 2016-09-06 02:58:40 -0600 )edit

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 asimay_y@126.com

asimay_y ( 2016-09-06 02:59:07 -0600 )edit

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!

asimay_y ( 2016-09-06 03:03:24 -0600 )edit

see more comments

answered 2016-06-17 04:35:26 -0600

asimay_y

255 ●6 ●13 ●17

updated 2016-06-21 23:00:36 -0600

hi, @M@t, I'm stucking in this too. but from your launch file, odom_demo.launch,

<remap from="odom" to="odometry/filtered/global" />

I think you should :

remap odom topic to odometry/filtered/local, which is move_base input.
update the robot_localization package to new version.

please share me the result. thanks!

**update:**

sorry, is it correct? I think it should be /odometry/filtered/local topic feed into move_base according to Navigation wiki, which is in your odom_demo.launch file. Please correct me if i'm wrong.

image description

edit flag offensive delete link

Comments

And can you try to make a map, ex. use gmapping to make a map, and use amcl to let your car drive in the outdoor's map? (combined with your gps.)

asimay_y ( 2016-06-17 04:49:05 -0600 )edit

Yes that's right, I want to use the global odometry because it should (in theory) be much more accurate than local because it will have the gps data fused with it. As for a map, I'm afraid I don't have a camera available, so I can't create one. the odom_demo launch file uses a blank map.

M@t ( 2016-06-21 23:53:49 -0600 )edit

ok. because i run in rosbag simulation env, they are totally behave same, wish you or i could test the actually behavior in real env to verify it. Your question seems same to mine: the origin of my /map and /odom frames move relative to each other. in my case, only /odom moved , /map is good.

asimay_y ( 2016-06-22 02:34:44 -0600 )edit

I am using a real Jackal, so what you're seeing is real data, not simulated. I think the problem is with my IMU and navsat_transform. The origin of /map never seems to be in the right place, i.e. it never matches the datum coordinates I give it.

M@t ( 2016-06-23 16:32:04 -0600 )edit

mine is real data also, just run in simulation env. I focus on this issue also. I'm also trying to run your bag in simulation env. if you figure out, let me know. thanks!

asimay_y ( 2016-06-23 20:54:02 -0600 )edit

hi, I test your bag file, and plot the velocity data of odometry/filtered/local and odometry/filtered/global, it seems the velocity data is very strange. hope it can help you. please see belows: global_veloc

asimay_y ( 2016-06-23 22:29:17 -0600 )edit

local_velocity

asimay_y ( 2016-06-23 22:32:14 -0600 )edit

That's velocity isn't it? Not odometry? It should be straight. This is what I get if I plot odometry (just the first half of the bag file) plotted_odometry. Red is from GPS (so very accurate) blue is from global instance of EKF.

M@t ( 2016-06-24 05:17:10 -0600 )edit

see more comments

answered 2016-07-02 11:28:45 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2016-07-05 04:19:50 -0600

From the first question: "...the origin of my /map and /odom frames move relative to each other"

They will. The only way in which this would not happen would be if your odometry-frame state matched your map-frame state perfectly, which it clearly won't.

From the update:

(No response)
"The various EKF and navsat nodes are VERY picky about how they are launched. If you run the ekf_and_gps_localisation.launch file I always get this error:"

That may just be because the transform isn't available for a single time step. Does it keep printing it, or does it print it once and then start functioning?
Something is going wrong with navsat_transform when I try to set the datum. I've tried changing the magnetic declination, yaw offset, datum heading... nothing affects the orientation of /map

I'll play back your bag data using your launch files and see what's going on. Also, I don't quite understand what you mean by "map always points to magnetic north." You appear to be setting a datum with a heading of zero. This is equivalent to not using any datum, and starting navsat_transform_node at the specified lat/long, and with your IMU reading 0 at that position. If your IMU reads 0 at magnetic north, then when you set the datum with a yaw of 0, you are implicitly pretending that you're starting your robot at that lat/long and facing magnetic north.
I think you may be misunderstanding how rviz works. The "Fixed Frame" specifies in what frame you want to view the data, so all poses will be transformed into that frame. If the map and odom frame data are right on top of each other, that means the map->odom transform is working correctly, even if the poses themselves are not actually the same.
When you use the parameters in the launch file, it just calls the service anyway:

https://github.com/cra-ros-pkg/robot_...

The only variable that you haven't shown here is what your robot's pose is when you first start the node. If the robot's heading is even fractionally different between those runs, then when we compute the transform from map->utm, it won't be the same. The UTM coordinates are on the order of tens of thousands of meters, so small variations will make a difference.

EDIT 1 in response to update/comments:

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.

In which part of the tutorial does it claim that the datum heading is an offset from true north? I'll fix ... (more)

edit flag offensive delete link

Comments

Sorry, I did a very poor job of explaining the problem. I've heavily edited the original descriptions to try and make them clearer, and I've combined problems 3 and 5 because I see now that they are effectively the same issue.

M@t ( 2016-07-03 19:16:50 -0600 )edit

2) Prints 2-5 times then freezes.

3) My understanding based on the tutorial was that the datum heading was from True North. Regardless, Changing heading should affect the orientation of /map in theory. But in practice the 'Y' axis of /map always points to Magnetic North

M@t ( 2016-07-03 19:26:17 -0600 )edit

4) That explains a lot! I personally find that very counter-intuitive but I can see how it's useful.

5) So in effect the same method is being used, the only difference is the timing of the call? I assume then that something is wrong with the /map->/utm transform during launch.

M@t ( 2016-07-03 19:32:32 -0600 )edit

Response to Answer Edit 1: In figure 1, where 'theta' is the angle between the red 'X' axis of /map and the blue dotted line it says "Desired direction of 0 heading for map frame". I assumed this was referring to the heading of the datum parameter. And since that dotted blue axis matches...

M@t ( 2016-07-04 15:40:45 -0600 )edit

...the UTM grid (which by definition must point to True North) I assumed that the datum heading was therefore from True North. If this isn't the case then I am very sorry - that's my mistake for misreading the diagram. Regarding the yaw output from the EKF changing, yes it definitely will as...

M@t ( 2016-07-04 19:07:47 -0600 )edit

...there is the usual 1-2 degrees of jitter coming in from the IMU. The imu_link->base_link transform is coming from the /robot_state_publisher node. I haven't touched that node and it runs on bootup so there shouldn't be anything interfering with it. And regardless of whether /map faces...

M@t ( 2016-07-04 19:13:00 -0600 )edit

...True or Magnetic North, shouldn't the heading value still change it? That's a key part of my problem that I'm trying to solve.

M@t ( 2016-07-04 19:59:21 -0600 )edit

add a comment

Localisation inaccurate because navsat_transform is incorrectly calculating odometry

UPDATE 1:

Comments

3 Answers

Problem

Cause

Comments

Comments

Comments

Question Tools

Stats

Related questions

Localisation inaccurate because navsat_transform is incorrectly calculating odometry edit

UPDATE 1:

Comments

3 Answers

Problem

Cause

Comments

Comments

Comments

Question Tools

Stats

Related questions

Localisation inaccurate because navsat_transform is incorrectly calculating odometry