robot_localization adjusting launch file

edit

(@mister_kay was kind enough to provide me with a bag file, so some of my response will relate to that data.)

There are some changes you need to make to your launch files and also some changes you need to make to your sensor data.

First, just so we're on the same page, it appears from plotting your GPS data in Google Earth that you started facing more or less directly north, then drove forward a bit, made a left turn, and then carried on going straight and turning left until you sort of completed a square. Please let me know if this is incorrect.

Assuming I'm correct, though:

• First, your wheel odometry data. Do you have access to the node that generates it? I would change its frame_id to "odom," fix its velocities so they're in base_link (or in your case, mustang_base), and then make sure to fill out the child_frame_id with that value. More importantly, however, you need to fix your orientation data. If you watch the output of your wheel odometry message, you will see that your yaw angle (once converted from a quaternion) is turning right after each leg, i.e., the value decreases. However, we know from the GPS data that the robot actually turned left. See REP-103. When you turn left, your yaw angle should increase, and turning right should result in a decrease.

  The Z, roll, pitch, and their respective velocities will remain zero, as this information is probably coming just from wheel encoders, which simply measure wheel rotations and therefore cannot measure 3D pose and velocity variables.

  As for the covariances, you should give them non-zero values. robot_localization can handle zero covariances, but it does so by changing them to a small positive value. This means that the measurements from any data source with zero covariances will be "trusted" too much by the filter.

  If for whatever reason you are unable to modify the velocity data, I would configure your odometry data this way (after you fix the sign of your yaw data, of course):

  <param name="odom0" value="wheel_odometry"/>

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

  <param name="odom0_differential" value="true"/>

• The same goes for your visual odometry data. Its yaw velocity angle seems to be negated. Also, for some reason, its data is reported in the UTM frame. There are other issues with your UTM frame handling that are causing some grief, but for now, I would report this data in the frame_id = odom frame, with its child_frame_id set to mustang_base. It's possible that the reason your state estimate followed the visual odometry so closely is that you wheel odometry couldn't be transformed into the correct frame, and was being ignored.

  <param name="odom1" value="wheel_odometry"/>

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

  <param name="odom1_differential" value="false"/>

• Your GPS data looks to be very close to ...