Navigation/AUV/SMO: Added Vx, Vy and Vz to estimted state. Simulation runned well under SMO.