Development of and preliminary results with an extended Kalman filter for the estimation of the translational and angular velocity of underwater vehicles

This paper reports a novel application of the extended Kalman filter (EKF) for the estimation of the full state of rigid-body underwater vehicles (UVs) without requiring access to velocity measurements. The theory is presented along with a preliminary experimental evaluation of the performance of the algorithm. The reported EKF employs measurements of vehicle position and orientation, the control input of vehicle actuator forces and moments, and a second-order, nonlinear dynamical plant model for the UV. The standard approach for estimating the full state of a rigid-body UV (translational position, angular position, translational velocity, and angular velocity) is to employ an extended Kalman filter of a kinematic plant model; in contrast, this paper employs an EKF for the full second-order plant dynamics. The standard approach, employing a kinematic plant model, typically requires instrumentation of the full vehicle state, including the translational and angular velocities. The algorithm reported herein is novel because it provides estimates of the translational and angular velocity of the vehicle without requiring those states to be instrumented. To evaluate the performance of this approach in the presence of real-world sensor noise and limitations in modeling of vehicle plant and actuator dynamics, a preliminary experimental evaluation is reported. This experimental evaluation compares the mean of the EKF state estimates to actual instrumented states of an UV in fully-coupled 6-degree-of-freedom (DOF) motion.