navsat_transform_node odometry estimate looks to be rotated differently from local map frame, resulting in incorrect estimation

Question

Hello all,

I am trying to get robot_localization and navsat_transform implemented on a Husky UGV to do outdoor navigation and control and I am running into problems with the odometry output from navsat_transform as it does not properly estimate the vehicle position within the local map frame relative to where the robot actually drove. This estimate in (x,y) will change depending on the direction the vehicle is facing before driving as if the vehicle orientation relative to the UTM frame is being accounted for wrong.

Here is more information about our configuration.

Robot background: I have a Husky UGV with a Duro GPS running the latest Piksi ROS driver with an RTK basestation, and a LORD microstrain 3DM-GX5-25 IMU running the latest version of the ros-mscl node written by LORD.

Current Sensor Transforms: With respect to sensor transforms, there are no sensor transforms specified for the Duro as the latitude and longitude are indifferent. The IMU is mounted on top of the robot such that IMU frame on the robot is x=forward, y=right, z=down. This IMU also makes all measurements with respect to NED, so I have written a republishing node to take in the NED IMU data and rotate the data such that the republished IMU data is with respect to ENU as robot_localization expects data in this format. Since the IMU data is transformed into an ENU format, I don't think I need to apply any rotational static transforms to the sensor data since the orientation is in the right frame and the yaw rate has been corrected to increase when the robot turns CCW. Is this assumption valid?

Current Navigation setup: I am running navigation suite as suggested by the dual_ekf_navsat example provided by the robot_localization package. In the first instance of robot_localization for continuous data, I am using the EKF node to fuse linear velocities from the /husky_velocity_controller/odom topic and the gyro rates from the IMU on the /gx5/imu/data topic.

The second instance is the EKF node fusing the same linear velocities from the wheel odometry and rates from the IMU gyro, but now including the odometry/gps output from navsat_transform, which is just the (x,y) coordinates.

As specified navsat_transform_node is subscribed to the /gps/fix topic from the Duro, /gx5/imu/data_ENU topic from the republished ENU IMU data and the output of the second EKF instance (/odometry/filtered_map). Since the IMU data has been transformed into an ENU frame, I have set the yaw_offset=0.0 and set the magnetic declination according to my geographic location.

As an example, we drove our Husky robot straight forward 4 m (with no turning) and the odom frame estimate agrees with the approximate distance travelled reporting minimal lateral translation in y, but the map frame estimate reports that the vehicle is quickly drifting laterally where over the course of a 4 m straight line, the map frame estimate will be reporting a position of (x,y)=(3.36,2.39). When comparing the vector magnitude between the final odom frame and map frame estimate, we see that they are almost the same length of vector, leading me to think that navsat_transform is incorrectly identifying the transform between the UTM frame and the initial vehicle heading. It was validated by looking at the output of navsat_transform (odometry/gps) and the estimated position is looking like the frame is not inline with the initial orientation of the robot. Alternatively, if we start the robot facing in other directions, the estimated position in x,y provided by navsat_transform_node don't align with the map or odom frame defined by the starting position of the robot but also do not show the same lateral deviation mentioned in the example above. From this, we are suspicious of the IMU and the orientation provided it as this is the factor that will decide the translation between UTM and the vehicle frame but its difficult to say. Or perhaps something is going on with how navsat_transform is identifying the transform to UTM.

Is there something I have overlooked in the implementation of the navsat_transform? Has anyone seen this behaviour before with navsat_transform? Any help would be much appreciated. I am happy to provide more information.

Here is a folder containing the launch files, robot URDF file with the static transforms, navigation config files, the NED to ENU republisher used on this setup and a few bag files for context.

---

Originally posted by mfader18 on ROS Answers with karma: 31 on 2020-08-26

Post score: 2

Answer 1

Update: Through more testing, it was found that the absolute orientation measurement from the IMU we were using was not the direction the robot was actually facing (by a handful of degrees). This resulted in the map frame of the vehicle being oriented in a slightly different direction from where the robot was facing due to the poor initial orientation measurement provided to navsat_transform_node. The observed pose difference between the map and odom frame when driving a straight line makes sense given the map frame is rotated differently from the odom frame. Our solution was to calibrate the IMU and try to move away from any sources of magnetic disturbances and this showed to fix the issue to be somewhat repeatable and make the orientation data useable. The IMU orientation data was not always right but in general the odom and map frame wouldn't be rotated significantly from each other.

We used this navigation setup for autonomous path following where all control actions were based on the pose estimate from the map frame EKF and the desired path also defined in the map frame. Occasionally if the initial pose estimate from the IMU was significantly different from the actual heading of the robot, we would see the vehicle steer aggressively right away to get onto the path that is oriented within the map frame, which is some rotation off from the robots actual initial heading. To fix this problem, restarting or recalibrating the IMU would provide more accurate heading measurements.

---

Originally posted by mfader18 with karma: 31 on 2020-09-08

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by JackB on 2020-10-01:
In my personal experience, the IMUs are so noisy that they actually make the estimation worse almost all the time. What I do is define a static datum for navsat_transform_node and then reference all map_frame points in that coordinate system. When I run the filter it might take a few filter cycles to integrate enough gps measurements to get the heading correct, but after it has it, it has it, and wont need imu data to maintain it. Does that make sense?

*And thanks for following up and answering your own question, your response is very descriptive and well posed (and btw you can "accept" you answer as correct by clicking the check mark below the up vote and down vote arrows I think)

Comment by JackB on 2020-10-01:
I should note that my above answer is dependent on having quality corrected GPS like RTK_fixed or RTK_float, or low quality GPS like SBAS and with a large covariance

Comment by mfader18 on 2020-10-05:
Thanks for the comments JackB. The IMU orientation was not the most reliable for absolute orientations but of course, the pose estimate would converge after the robot would begin to move. Cheers!

Comment by Sid05 on 2022-04-21:
Hi,

I am facing the same issue, can you please elaborate how you calibrated the IMU ? Please guide me , I have been searching through ros answers and trying many different things and your post describes the problem that I too face I would really appreciate an answer as I face the same issue for my project and the deadline is close

Thanks

Comment by mfader18 on 2022-04-25:
Hi there, It has been some time since I was working with this hardware and I don't recall if we used a setting in the launch file to calibrate the sensor but I believe we found that disconnecting and reconnecting the IMU before starting another field trial seemed to recorrect any kind of drift in the compass. Apologies for the lack of detail, I no longer have access to the hardware I was using.

Looks like others are having similar problems with drifting compass measurements. Keep an eye on this post to see if there is any resolution. https://github.com/LORD-MicroStrain/microstrain_inertial/issues/142

Comment by Sid05 on 2022-04-26:
Hi,

Thank you for getting back. I could experiment with disconnecting and connecting the IMU cable, will see how it goes I am using a Xsens IMU (https://www.mouser.de/ProductDetail/Xsens/MTi-30-2A8G4?qs=1mbolxNpo8cPcuq05Ezcgg%3D%3D)