Revision history [back]

One note: not critical, but you have two_d_mode set to true, but have 3D variables (Z, roll, and pitch) being measured. Also, I agree with @demmeln's other point: you have two sensors measuring X, Y, and Z position. The filter will automatically transform them into the frame specified by the world_frame parameter, but even so, you may find that the filter output jumps back and forth between the measurements if their covariances are overconfident. I tend to take my "best" measurement source for any given pose variable as the absolute measurement, and turn differential mode on for the others (unless they provide velocities, in which case you should just use those instead).

One note: not critical, but you have two_d_mode set to true, but have 3D variables (Z, roll, and pitch) being measured.

Also, I agree with @demmeln's other point: you have two sensors measuring X, Y, and Z position. The filter will automatically transform them into the frame specified by the world_frame parameter, but even so, you may find that the filter output jumps back and forth between the measurements if their covariances are overconfident. I tend to take my "best" measurement source for any given pose variable as the absolute measurement, and turn differential mode on for the others (unless they provide velocities, in which case you should just use those instead).