You can read through GPS Positioning in the developers section and trying a few things.
That section has settings in there for changing it to a 2D Position (and using the pressure sensor for depth).
Here is a previous discussion and results of attempting to tune the position controller: Position hold. Waterlinked
If tuning is unsuccessful, the vehicle code most likely needs to be fixed or rewritten. The code is open-source if you would like to modify it yourself until we can spend some time with it in the future.