src/Navigation/AUV/SMO: Added the calculation of vx,vy and vz.