# robot_localization using UTM grid as map

Hi all,

I'm trying to use robot_localization in Groovy to fuse information from several sensors (IMU, turn rate sensor and GPS). I'm using version x.1.6 from the hydro-devel branch (Check previous question regarding problems running in Groovy).

I've followed the tutorials from robot_localization ( http://wiki.ros.org/robot_localization ) and GPS integration ( http://wiki.ros.org/robot_localization ). Two instances of ekf_localization_node are running. One fuses IMU and turn rate sensor data, and the second one fuses IMU, turn rate and GPS. world_frame of the 1st instance is set to odom, and world_frame of 2nd one is set to map.

IMU sensor provides roll, pitch, yaw and X,Y,Z accelerations. Turn rate sensor provides Z axis angular speed (yaw rate). GPS data is feed through the navsat_transform_node.

All testing has been made with the platform static (it is an USV that is for the moment dry-docked).

Everything seem to be working, as the respective TFs and topics are published but I have two issues:

1.- I expected that "map" frame would be the UTM grid and the map->odom TF would give me basically a filtered transformation between the UTM grid and odom (the output of the now deprecated utm_transform_node, gone through the EKF with the IMU and turn rate sensor). However, what it seems to give me is the drift of the odom frame with respect of the initial point which is (0,0) and not the original UTM coordinates that should be something around (582409, 4793246).

Is this correct? Is there a way to get that the second instance of the ekf_localization_node gives me that transformation (UTM map --> odom)?

If I use the utm_transform node, whould be the map->utm transform equivalent to what I'm looking for? It seems that navsat_transform_node doesn't publish the odom-->utm transform.

2.- Is it enough an IMU with accelerations to have good estimations with ekf_localization_node? The IMU I'm using (RPY and accelerations, plus yaw rate from other sensor) in static is giving some accelerations, probably because it is not perfectly planar, and the EKF seems to be integrating them without any filtering, so in a few minutes it is giving me speeds of Km/s. I undertand that this should be normal, as no other source of positioning is present, but it doesn't seem to be compensated at all by the GPS input in the second instance.

Is there any roll-pitch gravity compensation introduced in the EKF or is it necessary to introduce it before feeding the EKF with the accelerations?

I understand that it is still a feature under development, so I don't know if this question should be put here or in another forum.

Thank you.

UPDATE:

I've fixed the errors pointed out in the comments (negative Z acceleration when static and using /imu/pseaa in navsat_transform_node). Results are now much better than before, but still the accelerations add up and in a couple of minutes I get a drift ...

edit retag close merge delete

Sort by » oldest newest most voted

Whew! Nice question. This answer might be a bit lengthy, so bear with me:

1. First, some history. The utm_transform_node originally published an odom->utm transform to the tf tree. Assuming your UTM data, which was reported as a nav_msgs/Odometry message, had a frame_id of utm, you could simply include it as an input to ekf_localization_node, and it would apply the transform before fusing the position. However, when a GPS device first starts up, it will naturally report some erroneous positions until it gets position information from enough satellites to give reasonable accuracy. The UTM data coming from utm_odometry_node in gps_common doesn't concern itself with that, so I added a new input to utm_transform_node, namely, the raw gps_common/GPSFix message. I used that information to determine if the fix was valid before computing the transform. At this point, though, I had redundant information, and so I nixed the requirement for UTM data, deprecated utm_transform_node, and created navsat_transform_node. navsat_transform_node still keeps a world_frame->utm transform internally, but it doesn't publish it. Instead, it outputs a nav_msgs/Odometry message with the data already converted to the world (in this case, map) coordinate frame. This provides two benefits:

• Users can just start up the node and get global positions that are consistent with their robot's starting position and orientation.
• rviz behaves a little strangely when you have a transform that's on the order of tens of thousands of meters, and this eliminated that issue.

Having said that, if you need navsat_transform_node to publish that transform, I'd be happy to add it as an optional, configurable feature. Just submit a feature request.

2. This is a little harder to answer without a bag file, but I do see a few issues in your configuration and sensor data:

• First, double-check your sensor data. To answer your question, yes, I am compensating for acceleration due to gravity. There's a parameter called remove_gravitational_acceleration, and it defaults to true. I should really make that a per-sensor configuration, but I digress. However, your IMU appears to be reporting negative Z acceleration on what I'm assuming is level ground. Is your IMU flipped upside-down, and if so, is that reflected in your base_footprint->imu transform? Double-check all of your signs for acceleration, because my correction could very well be causing your accelerations to increase. My assumptions about directions of acceleration readings are the same as this. If the IMU is on a level surface, it should report Z acceleration of +9.81 m/s^2. If you roll it +90 degrees, it should report a Y acceleration of +9.81 m/s^2. If you pitch it +90 degrees, it should report an X acceleration of -9.81 m/s^2.
• Second, your navsat_transform_node configuration uses the /imu/pseaa topic, but in your ekf_localization_node configuration, you have its yaw value set to false. If that sensor doesn't have an absolute heading reference, it won't work. I would switch that to the imu ...
more

(1/3) Thank you for pointing these errors. The negative Z acceleration is probably a mistake by me when converting the raw IMU data to an IMU message, and using the pseaa instead the imu probably was the 112342345235 change... I will fix them and try it again right now. (follows in next comment)

( 2014-11-14 01:54:30 -0500 )edit

(2/3) Regarding the first issue, I will submit the feature request, but, for the time being, if I use utm_transform_node will I get map->odom with map in UTM coordinates?

( 2014-11-14 01:57:57 -0500 )edit

(3/3) Finally, cmd_vels are actually sent to the robot, but being a USV catamaran with differential thruster steering, the inertia and drift are so huge that I thought that it would do more damage than good. Moreover, the USV is dry-docked, and I will be unable to move it at least until next week.

( 2014-11-14 02:01:31 -0500 )edit

Edited the question to add a sample of /odometry/gps message. Just noticed that it doesn't have any timestamp. Maybe the messages are being discarded because of that? Regarding your second question, yes, I mean (C). As mentioned, the platform is dry-docked and it can't be moved.

( 2014-11-14 07:34:26 -0500 )edit

The timestamp thing is actually a bug and I've created an issue for it (thanks for pointing that out!), but it shouldn't account for why it's not being integrated. I'll let you know when I check out the bag.

( 2014-11-14 08:46:41 -0500 )edit

Question: is there a reason why your time stamps on your messages seem to disagree with ROS time? Even when I play back the bag file with --clock and have use_sim_time set to true, there is nearly an hour's difference in each message's time stamp and the ros:Time::now() time.

( 2014-11-14 09:27:29 -0500 )edit

(1) and (2), Wow, pretty big mistakes. I think that "gpsodom" was copypasted from some tutorial, but I didn't realise that it wasn't literal...

( 2014-11-17 01:55:26 -0500 )edit

(3) I though that navsat_tranform_node needed the input of the ekf instance already running, as the second instance starts running after it. Seems I was (again) mistaken. I will try theses changes and report back. Thank you very much.

( 2014-11-17 01:57:59 -0500 )edit