A bathymetric Simultaneous Localization and Mapping (SLAM) algorithm is presented which is capable of mapping elevation changes in undulating seabed terrain and simultaneously localizing the vehicles position relative to the constructed terrain elevation map. The algorithm enables to marine vehicles navigate and build a map autonomously in a GPS-denied environment. In the proposed formulation, the terrain elevation is represented using a number of rectangular surface panels, and the elevation vectors at four corners of every panel are introduced into map states in the SLAM filter. Experimental results are presented using a underwater surface vehicle (USV) equipped with a multibeam echosounder (MBE) to verify and demonstrate the performance of the proposed algorithm.