Framework for Robust Localization of UUVs and Mapping of Net Pens