Simulation and Localization of Autonomous Underwater Vehicles Leveraging Lie Group Structure