Five-State Extended Kalman Filter for Estimation of Speed over Ground (SOG), Course over Ground (COG) and Course Rate of Unmanned Surface Vehicles (USVs): Experimental Results

Small USVs are usually equipped with a low-cost navigation sensor suite consisting of a global navigation satellite system (GNSS) receiver and a magnetic compass. Unfortunately, the magnetic compass is highly susceptible to electromagnetic disturbances. Hence, it should not be used in safety-critical autopilot systems. A gyrocompass, however, is highly reliable, but it is too expensive for most USV systems. It is tempting to compute the heading angle by using two GNSS antennas on the same receiver. Unfortunately, for small USV systems, the distance between the antennas is very small, requiring that an RTK GNSS receiver is used. The drawback of the RTK solution is that it suffers from dropouts due to ionospheric disturbances, multipath, interference, etc. For safety-critical applications, a more robust approach is to estimate the course angle to avoid using the heading angle during path following. The main result of this article is a five-state extended Kalman filter (EKF) aided by GNSS latitude-longitude measurements for estimation of the course over ground (COG), speed over ground (SOG), and course rate. These are the primary signals needed to implement a course autopilot system onboard a USV. The proposed algorithm is computationally efficient and easy to implement since only four EKF covariance parameters must be specified. The parameters need to be calibrated for different GNSS receivers and vehicle types, but they are not sensitive to the working conditions. Another advantage of the EKF is that the autopilot does not need to use the COG and SOG measurements from the GNSS receiver, which have varying quality and reliability. It is also straightforward to add complementary sensors such as a Doppler Velocity Log (DVL) to the EKF to improve the performance further. Finally, the performance of the five-state EKF is demonstrated by experimental testing of two commercial USV systems.