SLAM with SC-PHD filters: an underwater vehicle application