In this paper, submerged body dynamics model in vertical plane which can include the effect of free surface and wave is suggested to simulate the motions of submerged body moving beneath free surface precisely. A controller is designed, which can maintain a constant depth below the mean sea level and minimize the pitch angle. Numerical simulations show that the designed controller is effective on depth keeping and minimizing pitch angle in regular waves and irregular waves.