Fusing 2 sonars / pingers as continous sensors or 'GPS'

edit

This is an issue that I have been thinking about for navsat_transform_node. In your case, one option would be to define a transform from odom to sonar that is actually the offset from base_link, but rotated by the robot's current heading. This means the transform could not come from a static transform, but would have to originate from some other node that manages the rotation of the offset and the transform broadcast. Then you can simply report each sonar position measurement in the sonar frame, and ekf_localization_node will transform it before fusing it.

EDIT in response to comments: ekf_localization_node will try to transform all absolute pose data into the frame specified by world_frame parameter. If you make the sonar transform relative to base_link, it will transform the position from sonar to base_link, and then apply the transform to the world_frame. For example, if your robot is at (10, 5) and your sonar is mounted at (1, 1) with respect to the base_link frame, and you get a sonar-based position estimate of (12, 7), then your measurement might get transformed to, say (11, 6) (after transforming to base_link), and then to (21, 11) when the transform to the world_frame is applied. '

EDIT in response to question edit: interesting. I've not seen slowdown occur before. Do you mean the message broadcast rate slows down (rostopic hz /odometry/filtered), or that it only produces an updated state at a slower rate? When I get a chance, I'll check out the bag file. My first guess is that a required transform is failing most of the time, so ekf_localization_node is ignoring the measurement.

EDIT 2: That is strange. I'll check it out and let you know what I find.

EDIT 3: Figured out the issue. Your pose2 topic has an incorrect covariance. You're measuring Y, so the covariance value should be non-zero in the (Y, Y) position, which, in a pose message, is position 7 in the array, not position 2 (position 2 is the covariance between X and Y). As your pose2 data is being reported in a different frame, when the covariance gets rotated into the odom frame, the final measurement covariance matrix that gets fed into the filter is broken. This eventually breaks the covariance for the entire state, and the filter locks up, presumably when trying to invert a singular matrix.

EDIT 4: Yeah, it's a covariance issue. Your yaw variance is ~9 degrees. That won't play well with the differential setting, at least as it is now. It's partially a bug, and partially an issue with unnecessary use of the differential setting. If you have multiple measurement sources for one variable, it's usually best to let one of them be integrated normally (i.e., with differential set to false), and let the others be handled differentially. This is more important for orientation data than for pose data. As of the next release, all differential measurements will be converted to velocities ... (more)