This paper describes an algorithmic solution for simple and efficient underwater orientation and depth control. Maintaining a position with an underwater robot is a difficult task. In the case of an Autonomous Underwater Vehicles (AUVs), not only the underwater conditions, but also the environmental effects off the surface need to be considered. There is a large number of algorithms have been designed by researchers based on computer vision, sensor fusion, etc. to estimate the location precisely, yet most of them are specific for the given hardware. Our solution employs a multi-sensor fusion based algorithm, where the data is taken from magnetic and pressure sensors. A PID controller was designed and implemented to ensure proper orientation keeping and depth control in rippling water. The solution has been tested in various environments, and successfully used during the marine challenges of the euRathlon 2015 competition.