Accurate EKF-Based Underwater Navigation System with RSSI Validation and Delay-Compensated USBL Measurements

Accurate underwater positioning is a critical requirement for a wide range of marine applications, including underwater vehicle navigation, seafloor mapping, offshore infrastructure inspection, and environmental monitoring. Unlike surface or aerial platforms, underwater vehicles cannot rely on Global Navigation Satellite System (GNSS) signals, as electromagnetic waves are rapidly attenuated beyond the first few meters of the water column. This fundamental limitation makes underwater navigation inherently more challenging for vehicles such as Autonomous Underwater Vehicles (AUVs) and Remotely Operated Vehicles (ROVs), driving the need for alternative positioning technologies and robust sensor fusion strategies. Current underwater navigation solutions typically combine Inertial Measurement Units (IMU) with Doppler Velocity Logs (DVL) to provide dead-reckoning estimates of position and velocity. However, inertial-based solutions suffer from unbounded error growth over time due to sensor drift, making them unreliable for long-duration missions without periodic external corrections. Furthermore, both IMU and DVL sensors require careful calibration procedures to guarantee high positioning accuracy and overall navigation performance. This paper presents an Extended Kalman Filter (EKF)-based navigation framework that addresses these limitations through the fusion of IMU, DVL, depth, and Ultra Short Baseline (USBL) measurements. The filter propagates a nine-dimensional state vector comprising position, velocity, and acceleration in the North-East-Down (NED) reference frame, using a constant-acceleration kinematic model to perform high-rate dead-reckoning between lower-rate absolute corrections. A key contribution is a principled treatment of the delayed measurement problem inherent in acoustic USBL systems: since acoustic navigation messages carry timestamps that typically lag the current filter epoch by several seconds, naively fusing a USBL position fix at the current time introduces systematic positioning errors proportional to the vehicle displacement during the delay interval. To address this, we propose a state back-propagation strategy that reconstructs the vehicle position at the exact USBL measurement time using the current velocity and acceleration estimates, and derive a full linearized Jacobian that correctly propagates delay-induced sensitivity across all three kinematic sub-states. Prior to fusion, USBL range measurements are validated through Received Signal Strength Indicator (RSSI) analysis to reject outliers caused by multipath propagation and acoustic anomalies, with all required sensor parameters sourced directly from commercial datasheets. The complete framework preserves the standard EKF predict-update structure, requires no filter rewind, and is computationally lightweight for real-time onboard deployment on commercial underwater platforms.