UKF replacement for robot_pose_efk (still in development)