Working on fixing vel_model... Quaternion issue is still real.  Added Pose2DWithCovariance.msg as a msg of algp1_msgs