I am just catching up on this entire project (which I must say is really interesting!), and see that you need to fuse IMU and GPS data. If you do want to use
robot_localization, note that you don’t necessarily need wheel odometry, but rather a nav_msgs/Odometry message, which is generated by an instance of
ekf_localization_node. You could do something like the following:
nav_msgs/Odometry message containing the GPS pose (as produced by
navsat_transform_node), and possibly GPS velocity (more on that in a moment)
nav_msgs/Odometry message with your tractor’s state
Inputs: IMU, GPS, and
nav_msgs/Odometry message from EKF
nav_msgs/Odometry message containing the GPS pose, transformed into the EKF’s world frame, (optionally) a filtered GPS fix
You’ll note that there’s a circular dependency here, which is that the EKF has to produce an odometry message for
navsat_transform_node, which is then spitting out its own odometry message that gets fed back to the EKF. The reasons for it are probably a bit much to go into here, but it will work: the EKF will get its first IMU measurement and immediately produce a state estimate, which will get fed to
navsat_transform_node along with the IMU and GPS data, and
navsat_transform_node will spit out a pose that is consistent with your EKF’s coordinate frame, and can be fused back into it. This lets you control your tractor based on its world frame pose (the EKF output).
There are two things that I’d want to know more about, however:
- If you want to use the GPS velocity data and fuse that into the EKF (which I would recommend), you’d first want to make sure that the data is in the body frame of the tractor. A cursory glance at the
nmea_savsat_driversource seems to suggest that they are converting the velocity into a world frame (apologies if I am misreading that), but if you want to fuse it with the EKF, you need the body-frame velocity (i.e., on the tractor, your tractor’s linear velocity would always be +/- X).
- I’d need to know your IMU’s coordinate frame. The data sheet seems to suggest that it already assumes that the signs of the data would be
robot_localizationfriendly (e.g., a counter-clockwise turn should correspond to an increase in yaw), but I’m assuming that your IMU reads 0 when facing magnetic north, correct? I only ask because we assume a heading of 0 for east, not north. There’s a parameter in
navsat_transform_nodefor specifying that offset, however, along with the magnetic declination for your location.