The development of Autonomous Underwater Vehicles (AUVs) has offered numerous advantages, but has also presented new challenges. Free from the burden of an umbilical cable connecting them to the support ship, they are a quantum leap from widely used Remotely Operated Vehicles (ROVs) providing a stable platform for the new high resolution scientific sensors and reducing the operational costs. One of the most significant challenges is the problem of underwater navigation or, in other words, how to determine the vehicle's position within the environment, at least as accurate as needed, according to the standards of each scientific survey. Our research objective is to accurate localize the AUV in the three dimensional environment (3D) using its own sensors without any external infrastructure under the concept of Simultaneous Localization and Mapping (SLAM). The environment will be sensed by state of the art multibeam echosounders that they are becoming broadly available, providing high precision of the surrounding bathymetry and they are not disturbed by turbidity or lighting conditions. Moreover, attitude sensors that are commonly found in AUVs will aid on the results precision. Under probabilistic 3D raw data registration and SLAM framework, most of the AUVs navigation constraints will be removed and will allow safe and precise surveys in the challenging underwater environment.