Revision history [back]

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. 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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

I'll check out the bag file and make recommendations.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll check out the bag file and start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

   
   <rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

   You need this:

   
   <rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make recommendations.

   it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

   
   <rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

   You need this:

   
   <rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

<rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam> true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

You need this:

ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

<rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

You need this:

<rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>

ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:this (having a hell of a time getting XML to display, so removing angle braces):

   param name="gpsodom0" value="/odometry/gps" rosparam param="gpsodom0_config" [true, true, false, false, false, false, false, false, false, false, false, false, false, false, false] /rosparam param name="gpsodom0_differential" value="false"

   You need this (having a hell of a time getting XML to display, so removing angle braces):this:

   param name="odom0" value="/odometry/gps" rosparam param="odom0_config" [true, true, false, false, false, false, false, false, false, false, false, false, false, false, false] /rosparam param name="odom0_differential" value="false"

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this (having a hell of a time getting XML to display, so removing angle braces):

   param this:

   <param name="gpsodom0" value="/odometry/gps"
   rosparam param="gpsodom0_config" [true, value="/odometry/gps"/>  
   <rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false] /rosparam
   param false]</rosparam>
    <param name="gpsodom0_differential" value="false"
   value="false"/>
   

   You need this:

   param

   <param name="odom0" value="/odometry/gps" 
   rosparam param="odom0_config" [true, value="/odometry/gps"/>  
   <rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false] /rosparam 
    param false]</rosparam>
   <param name="odom0_differential" value="false"
   value="false"/>
   

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

   <param name="gpsodom0" value="/odometry/gps"/>  
   <rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
    <param name="gpsodom0_differential" value="false"/>
   

   You need this:

   <param name="odom0" value="/odometry/gps"/>  
   <rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
   <param name="odom0_differential" value="false"/>
   

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

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/imu topic, which appears to have orientation data.

   The first bullet may solve your problem. Please let me know if it doesn't, though, as I would expect your configuration to produce reasonable results.

Sorry, just thought of one additional thing: are you sending any cmd_vels (geometry_msgs/Twist) to your robot? If so, if you can get that data into a geometry_msgs/TwistWithCovarianceStamped message, you can fuse that into your state estimate as well (just give it a large covariance). After you ensure that your acceleration signs are correct, you can use that velocity data to help keep the acceleration from accumulating into massive velocities.

EDIT in response to new questions. I have not yet looked at the bag, and when I do, I will likely have a much more definitive answer, and will update this.

1. Re: your comment below (2/3): no. Following REP-105, odom and map are more or less the same, except that one (map) takes in global position data, and the other (odom) uses only continuous data that isn't subject to discrete jumps. At start, the map and odom frames are completely aligned. As the robot moves, they begin to drift from one another, as the map frame receives global position data, and the odom frame starts to drift from the effects of erroneous measurements. I treat the UTM grid as a completely separate frame. There is a map->utm transform that is stored internally to navsat_transform_node, but I'm not yet broadcasting it (to be remedied...).

2. When you report drift rates of 60m after two minutes, are you saying that (A) you measured the actual drift of the vehicle, (B) the covariance on X and Y is 60m, or (C) that the vehicle is stationary and you're seeing a position of 60m? I'm pretty sure you mean (C), but I wanted to be clear.

3. Re: your question about whether the filter should be able to keep the position static, the answer is that it's only going to be as good as the input data. Your accelerations are noisy, yes, but they are almost certainly not zero-mean, so even if the filter is able to sort out the "true" value of the acceleration, it won't account for biases. In other words, if your accelerometer is reporting X linear acceleration of 0.2 m/s^2 +/- 0.04 m/s^2, the EKF will figure out that your true acceleration is 0.2 m/s^2, but won't account for the fact that it ought to be 0. BUT (and I think this may be good news)...

4. I'm very suspicious of your GPS data. The fact that you are getting ekf_localization_node positions into the tens or hundreds of meters indicates that your GPS data is almost certainly not being fed into the filter. Can you post the value of /odometry/gps? Global position measurements will always constrain your drift, so even if you have massive false velocities, every GPS measurement will bring your estimate right back home.

EDIT 2:

OK, found some good stuff. I'll start with the biggest issue.

1. I should have caught this before, but your GPS odometry configuration for your "map" instance of robot_localization needs a small but important change. You have this:

   <param name="gpsodom0" value="/odometry/gps"/>  
   <rosparam param="gpsodom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
    <param name="gpsodom0_differential" value="false"/>
   

   You need this:

   <param name="odom0" value="/odometry/gps"/>  
   <rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
   <param name="odom0_differential" value="false"/>
   

   ekf_localization_node only looks for parameters that start with "odom", "twist", "imu", and "pose." It will completely ignore a "gpsodom" prefixed topic.

2. Your cmd_vel topic has no frame_id or time stamp.

3. In your configuration for navsat_transform_node, you should probably remove your <remap from="/odometry/filtered" to="/odometry/imu"/> line, which will make it listen to the odometry from your "map" instance of ekf_localization_node, which is publishing the /odometry/filtered topic. The output frame_id of navsat_transform_node will match whatever odometry topic to which it is subscribed, and you want that frame_id to be "map."

There may be more things to tweak, but let's start with those. I'm still having trouble with the message time stamps. Are all the messages originating on the same machine?

EDIT 3 (response to update 5):

You are experiencing drift that is purely a result of your GPS data. I took the liberty of plotting the raw GPS data for both runs and centering it around 0 so you can see the drift in meters.

I didn't rotate it into your frame, but your run that showed (14, -16) has approximately the same magnitude (21.26 meters) as the plot below (whose magnitude is ~20.88 meters).

Disregard the warning message. That's a minor bug that really only makes that message print, but thanks for bringing it to my attention!