# Why does the accuracy of navsat_transform change with heading?

## The Problem (in short)

I am using the excellent robot_localization package to localize my robot in an outdoor environment. I'm trying to make the localization as accurate as possible, and it's currently accurate to <25cm (my GPS is accurate to ~20mm so that's what I'm aiming for). However this last 25cm is dependent on the heading of the robot. I.e. the error changes depending on the direction the robot is facing, as demonstrated if I simply rotate 360 degrees on the spot:

I am trying to find out why this is happening and how I can fix the issue so accuracy is independent of heading. This error is periodic, and consistent across several tests, so I think it is being cause by as systematic error in navsat_transform or in the way the frame transforms are being handled. All info and data is given under "System Information" at the bottom of this page.

## The Problem (in full)

I have three sensor inputs: wheel odometry, IMU data, and GPS data. I am using one instance of the ekf_localisation node to fuse wheels and IMU data ("local" odometry) and a second instance to fuse everything ("global" odometry). I am also using an instance of navsat_transform to convert my GPS data to a GPS odometry stream ("gps" odometry) which is what is being used in the global EKF node. When I rotate the robot on the spot, this is the odometry reported for each odometry stream:

Keep in mind that the local odometry is reported in the local /odom reference frame while the global and GPS odometry are both reported in the global /map frame. This leads me to believe that the problem is related to the navsat_transform node or one of the frame transforms that it uses.

I think there is actually a positive feedback loop in here since the GPS odometry is fused to produce the global odometry, but the position of the GPS odometry is partially determined by the global EKF which determines the location of the /map frame that the GPS odometry is relative to.

Now I know that my GPS is not the problem, because when I directly calculate the GPS UTM coordinates of the center of the robot (/base_link) it is different to the UTM coordinates obtained via sensor fusion (I'm effectively just looking at the origin of /base_link relative to the /utm frame which is broadcast by navsat_transform). This is what I get:

So the directly calculated data indicates that the robot is turning on the spot (as expected) however the UTM coordinates of /base_link relative to /utm match the previous graph. Also, when I directly calculate the UTM coordinates of /base_link I'm using the exact same WGS84->UTM calculations used by robot_localisation and I'm using the exact same magnetic declination as what I've specified in the launch file for navsat_transform.

Now when I watch the reference frames in RViz, the /base_link and /odom do not move relative ...

edit retag close merge delete

4

I don't have an answer, but I wanted to commend you on your thorough analysis and included plots and hypotheses. Let's hope you get a good answer(s) as well.

( 2016-08-19 06:26:14 -0600 )edit

I think this problem can also solve the drift case of map&odom when integrate with r_l GPS fusion. watching this. please also include @joq know.

( 2016-08-20 01:32:31 -0600 )edit

because your question is related with integrate errors change with yaw, I know from your launch file, there are two Vyaw elements are integrated into r_l, and the covariance is not very sure, without clearly understand the r_l integrate algorithm, can you only integrate 1 exact Vyaw to verify it?

( 2016-08-20 01:55:18 -0600 )edit

good work! but about the drift between map and odom, my view is different, I think the map is start of map, odom is start of car, so the two start points is fixed and should not drift when under map frame without consider the fusion error. but of course the start point is obviously drift in r_l..

( 2016-08-21 22:10:02 -0600 )edit

Thanks! And yes, in theory they should be fixed. But in practice that's how RViz deals with the difference between the global and local odometry. The alternative would be to fix /odom, /map and have two visualizations of the robot - one for each odometry set (it would look like my odometry graphs)

( 2016-08-21 22:54:53 -0600 )edit

That's part of the reason I print out all the odometry data and plot it in Excel - because then all the odometry is relative to the same origin point. I generally find it easier to debug issues and think about reference frames that way.

( 2016-08-21 23:13:03 -0600 )edit
1

but proves conclusively that it is a bug in navsat_transform.

It would be nice if you could report that on the proper issue tracker. Visibility of bug reports on ROS Answers is really low / non-existent, so this will likely get lost.

( 2016-09-01 02:05:58 -0600 )edit
1

I think cra-ros-pkg/robot_localization/issues is the correct tracker.

( 2016-09-01 02:07:47 -0600 )edit

Sort by » oldest newest most voted

Wow, great question.

Can you tell me what your base_link->auxgps_link transform is? Up until 2.2.3, I wasn't accounting for the base_link->gps transform, so the pose estimate you'd get from the GPS would be w.r.t. the GPS itself, not the robot center. This was clearly an issue for large bots with GPS sensors mounted far from the origin. The way I am correcting this now is as follows:

1. Get the robot's current rotation
2. Use that to rotate the base_link->gps transform
3. Apply rotated transform

This is because the offset that needs to be applied will obviously change with heading. Unlike a LIDAR, the data being measured by the GPS is not e.g., a range measurement originating at the GPS, such that applying the transform via tf will work.

However, I haven't had any good bag files to test the change, so it's certainly possible that I did something stupid. Still, can you tell me where the sensor is mounted on your robot, and the transform you used?

EDIT: I was able to verify the phenomenon. The problem was indeed with the treatment of the base_link->auxgps_link transform. In step (1) above, I was getting the inverse rotation that I needed. Given that the X linear offset for the sensor was -0.12 meters, this meant that the offset was effectively being doubled at headings of pi/2 and -pi/2. So rotating in place would cause the error to go from 0 to approximately -0.24 to 0 to 0.24. I have a PR for the change and will merge shortly.

more

Hi Tom, thanks again for your help! I've replied as an update to the question. Also, should this discussion be moved to the issue tracker?

( 2016-09-04 18:34:15 -0600 )edit

is this the Gimbal lock issue of roll pitch yaw?

( 2016-09-13 05:19:06 -0600 )edit

No. That is unrelated.

( 2016-09-13 05:45:57 -0600 )edit

Quick question @Tom Moore, the r_l source has been updated with the fix, but have the binaries also been updated with the fix? I'm using the most up-to-date r_l binaries (v2.3.0) and I'm still seeing the same incorrect behaviour (would have tested this sooner but life got in the way).

( 2016-10-09 22:25:50 -0600 )edit

@M@t I haven't done a release yet, no. I have one other issue I want to close before I release, and even then you'll have to wait until OSRF does a package sync. I'd keep using source for now.

( 2016-10-10 17:16:27 -0600 )edit

Ah, thought that might be the case. Thanks Tom!

( 2016-10-11 15:29:17 -0600 )edit