Pose updates for comparison

Added code to publish high frequency pose updates for comparison purposes - enable in param with publish_basic_pose