Navigation/AUV/SMO: Code rebuilded, simpler and more organized.