Underwater Position and Attitude Estimation Using Acoustic, Inertial, and Depth Measurements

This article considers the problem of constructing an observer for estimating position, velocity, attitude, underwater wave speed, rate sensor bias, and accelerometer bias that has both proven stability and close-to-optimal performance with respect to noise. The observer takes pseudorange, pseudorange difference, depth, and inertial measurements as input, and has a cascade structure for which the equilibrium point is proven to be locally exponentially stable due to the singularities in the attitude representation. The design of the observer is based on the exogenous Kalman filter principle, in which estimators with proven stability provide a linearization point for a linearized Kalman filter, to achieve both proven stability and close-to-optimal noise properties. Experimental validation is provided, with ground truth values generated by a camera positioning system with millimeter accuracy. The observer is compared to an extended Kalman filter and to a nonimplementable linearized Kalman filter using the true state as the linearization point, and the estimation error is almost identical to the linearized Kalman filter using the true state as a linearization point.