6 Degrees of Freedom State Estimation Underwater