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

Abstract

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.

1. Introduction

Commercial unmanned surface vehicle (USV) systems are used in many operations such as harbor inspection, surveillance, mapping, data acquisition, oceanography, etc. (see Figure 1 and Figure 2). This creates a need for low-cost sensor systems to operate a USV safely with satisfactory performance. The autopilot system is a critical component that is used for turning and path following. Both heading and course autopilots can be used for this purpose. Course autopilots are the preferred solution during path following. However, for stationkeeping it is necessary to control the heading angle. The reason for this is that the course angle is not defined at zero speed.

Ships are usually equipped with a gyrocompass, which is a nonmagnetic compass able to find true North using a fast-spinning gyroscope (Fossen [1]). The gyrocompass gives a highly accurate measurement of the heading (yaw) angle, but it is too expensive to be used in small USV systems. Hence, it is tempting to use a low-cost magnetic compass for navigation and maneuvering. This is not straightforward since a magnetic compass is susceptible to magnetic disturbances produced by electromagnetic devices such as propellers and thrusters. In addition, the magnetic field of the Earth is not perfectly aligned but skewed along the Earth’s rotational axis. The skew or bias is called declination, and it must be compensated for when navigating.

An alternative measurement to the compass could be to use two GNSS antennas on the same receiver with a known offset vector to compute the heading angle. The accuracy is further improved by using real-time kinematic (RTK) GNSS positioning (Farrell [2]). This is the preferred solution for small USVs since the offset vector can be small. It is well known that the RTK GNSS receivers are sensitive to ionospheric disturbances, multipath, loss of signals, the number of available satellites, etc., so the reliability can, in many cases, be unsatisfactory.

The scope of this article is to derive a robust state estimator for SOG, COG, and course rate such that these signals can be used to design a USV course autopilot for path following. The state estimator should only use the North–East positions, alternatively the latitude and longitude, of a GNSS receiver. If the course angle is available as a direct measurement or an estimate, it is straightforward to design a proportional-integral-derivative (PID) controller for course control; see Fossen [1].

The dynamics of a USV moving along a path or a trajectory can be modeled by using 2D target-tracking models (Li and Jilkov [4]). The simplest models for a target-tracking maneuver are the white-noise constant velocity (CV) and constant acceleration (CA) models (Bar-Shalom et al. [5]). These models are based on the assumption that the target speed or acceleration are independent processes driven by Gaussian white noise. The Otter and Mariner USVs shown in Figure 1 and Figure 2, respectively, are not able to produce aggressive linear accelerations. Hence, the CV model is well suited to describe the vehicle’s speed U during path-following control, since U is nearly constant even though the USV experiences small linear accelerations. Analysis of automatic identification system (AIS) data confirms this (Siegert et al. [6]). However, both USVs have propellers that can turn the vehicles quite fast. This suggests that the course angle $\chi$ should be modeled by a CA model. Consequently, the target-tracking models are chosen as a combination of the CV and CA models according to

$$\begin{array}{cc}\hfill \dot{U}& =w_1\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{\chi }& ={\omega }_{\chi }\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill \dot{{\omega }_{\chi }}& =w_2\hfill \end{array}$$

(3)

where $w_1$ and $w_2$ are Gaussian white-noise processes. The models (1)–(3) will be used as a basis for the EKF presented in Section 3.

The main result of this article is a five-state EKF (Appendix A) aided by GNSS positions or latitude–longitude measurements, which is intended for course and path-following control. The EKF can also be used to process AIS measurements (Fossen and Fossen [7]) with the purpose of ship motion prediction. The algorithm is computationally efficient and easy to implement. The main advantage of the EKF is that vendors do not need to use COG and SOG measurements from GNSS receivers, which have varying quality. In addition, the algorithm produces an accurate estimate of the course rate, which is an important signal when implementing PID course autopilot systems.

The remainder of this article is organized as follows: Section 2 presents the kinematic equations of a surface vehicle, while Section 3 contains the EKF for COG, SOG, and course rate estimation. Section 4 describes a method for USV course autopilot design. Section 5 includes a simulation study of a small USV under course autopilot control, while Section 6 and Section 7 present the experimental results using two commercial USV systems. The discussion and concluding remarks are drawn in Section 8 and Section 9.

2. Kinematics

The relationship between the angular variables course, heading, and crab angle is important for maneuvering of a USV in the horizontal plane. The terms course and heading are used interchangeably in much of the literature on marine systems, and this leads to confusion. Let the BODY and North-East-Down (NED) reference frames in Figure 3 be denoted by $\left\{b\right\}=(x_b,y_b,z_b)$ and $\left\{n\right\}=(x_n,y_n,z_n)$, respectively.

Definition 1.

(Yaw or heading angle $\psi$) The angle ψ from the $x_n$ axis (true North) to the $x_b$ axis of the USV, positive rotation about the $z_n$ axis by the right-hand screw convention.

The heading angle is usually measured by using a magnetic compass, gyrocompass or two GNSS receivers; see Gade [8] for a discussion of methods. The heading angle is well defined for zero speed such that it is possible to design a heading autopilot to maintain constant heading during stationkeeping and transit. However, during transit it is common to use a course autopilot for path following.

Definition 2.

(Course angle $\chi$) The angle χ from the $x_n$ axis (true North) to the velocity vector of the USV, positive rotation about the $z_n$ axis by the right-hand screw convention.

Note that the course angle is only defined for positive speed. The North–East positions $(x^n,y^n)$ of a USV can be described by (Fossen [1]),

$$\begin{array}{cc}\hfill {\dot{x}}^n& =ucos\left(\psi \right)-vsin\left(\psi \right)\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill {\dot{y}}^n& =usin\left(\psi \right)+vcos\left(\psi \right)\hfill \end{array}$$

(5)

where $(u,v)$ are the surge and sway velocities, respectively. These equations can be expressed in amplitude-phase form by

$$\begin{array}{cc}\hfill {\dot{x}}^n& =Ucos(\psi +{\beta }_c):=Ucos\left(\chi \right)\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill {\dot{y}}^n& =Usin(\psi +{\beta }_c):=Usin\left(\chi \right)\hfill \end{array}$$

(7)

where the course angle is defined as

$$\chi :=\psi +{\beta }_c$$

(8)

Furthermore, the amplitude U and phase variable ${\beta }_c$ are

$$\begin{array}{cc}\hfill U& =\sqrt{u^2+v^2}\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\beta }_c& =\mathrm{atan}\left(\frac{v}{u}\right)={sin}^{-1}\left(\frac{v}{U}\right)\hfill \end{array}$$

(10)

Note that U is the speed in the horizontal plane, and ${\beta }_c$ is the crab angle.

Definition 3.

(Crab angle ${\beta }_c$) The angle ${\beta }_c$ from the $x_b$ axis to the velocity vector of the USV, positive rotation about the $z_b$ axis by the right-hand screw convention.

3. EKF for SOG, COG and Course Rate

The primary objective of the EKF is to compute accurate estimates of the SOG, COG, and course rate of the USV when moving along a path. The dynamic model of the EKF ensures that old GNSS position measurements are used to compute the estimates. Since the path is not parametrized and the heading angle is unknown, the only information during path following is the measured GNSS position, alternatively the latitude-longitude pair.

3.1. Five-State EKF: North-East Position Measurements

The North–East positions $(x^n,y^n)$ of the USV are given by (6) and (7), while the speed and the course angle are modeled by the CV and CA models (1)–(3). The resulting state–space model expressed in North–East coordinates is

$$\begin{array}{cc}\hfill {\dot{x}}^n& =Ucos\left(\chi \right)\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {\dot{y}}^n& =Usin\left(\chi \right)\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill \dot{U}& =-{\alpha }_{1}U+w_1\hfill \end{array}$$

(13)

$$\mathrm{Subjected} \mathrm{to}\left(2\right)$$

$$\begin{array}{cc}\hfill \dot{{\omega }_{\chi }}& =-{\alpha }_2{\omega }_{\chi }+w_2\hfill \end{array}$$

(14)

where $w_1$ and $w_2$ are Gaussian white-noise process noise. Two small constants ${\alpha }_1\ge 0$ and ${\alpha }_2\ge 0$ have been added to the model to ensure that U and ${\omega }_{\chi }$ converge to zero during stationkeeping. Equations (13) and (14) are referred to as Singer models [9] in the target-tracking community (Li and Jilkov [4]).

The GNSS measurement equations associated with (11)–(14) are

$$\begin{array}{cc}\hfill y_1& =x^n+{\epsilon }_1\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill y_2& =y^n+{\epsilon }_2\hfill \end{array}$$

(16)

where ${\epsilon }_1$ and ${\epsilon }_2$ are Gaussian white-noise measurement noise. For the speed Equation (13), simulation studies revealed that the CV model with the Singer modification ${\alpha }_1=0.01$ was most accurate for USVs since the speed is nearly-constant most of the time, i.e., small linear accelerations. Turning was accurately described by using the CA model with ${\alpha }_2=0.1$. This gave satisfactory course rate estimates.

The discrete-time representation of (11)–(14) is obtained by Euler’s method

$$\begin{array}{cc}\hfill x^n[k+1]& =x^n\left[k\right]+hU\left[k\right]cos\left(\chi \left[k\right]\right)\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill y^n[k+1]& =y^n\left[k\right]+hU\left[k\right]sin\left(\chi \left[k\right]\right)\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill U[k+1]& =(1-h{\alpha }_1)U\left[k\right]+h{w}_1\left[k\right]\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill \chi [k+1]& =\chi \left[k\right]+h{\omega }_{\chi }\left[k\right]\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\omega }_{\chi }[k+1]& =(1-h{\alpha }_2){\omega }_{\chi }\left[k\right]+h{w}_2\left[k\right]\hfill \end{array}$$

(21)

where h is used to denote the sampling time. The discrete-time measurement equations are

$$\begin{array}{cc}\hfill y_1\left[k\right]& =x^n\left[k\right]+{\epsilon }_1\left[k\right]\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill y_2\left[k\right]& =y^n\left[k\right]+{\epsilon }_2\left[k\right]\hfill \end{array}$$

(23)

Consequently, the discrete-time state–space model becomes

$$\begin{array}{cc}\hfill \mathit{x}[k+1]& ={\mathit{A}}_d\mathit{x}\left[k\right]+{\mathit{E}}_d\mathit{w}\left[k\right]\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill \mathit{y}\left[k\right]& ={\mathit{C}}_d\mathit{x}\left[k\right]+\mathit{\epsilon }\left[k\right]\hfill \end{array}$$

(25)

where

$$\begin{array}{cc}\hfill \mathit{x}& ={\left[x^n,y^n,U,\chi ,{\omega }_{\chi }\right]}^{\top }\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill \mathit{y}& ={\left[x^n,y^n\right]}^{\top }\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill \mathit{w}& ={[w_1,w_2]}^{\top }\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill \mathit{\epsilon }& ={[{\epsilon }_1,{\epsilon }_2]}^{\top }\hfill \end{array}$$

(29)

The Jacobians are

$${\mathit{A}}_d=\left[\begin{array}{ccccc}\hfill 1& \hfill 0& \hfill hcos\left({\widehat{x}}_4\left[k\right]\right)& -h{\widehat{x}}_3\left[k\right]sin\left({\widehat{x}}_4\left[k\right]\right)& 0\\ \hfill 0& \hfill 1& \hfill hsin\left({\widehat{x}}_4\left[k\right]\right)& h{\widehat{x}}_3\left[k\right]cos\left({\widehat{x}}_4\left[k\right]\right)& 0\\ \hfill 0& \hfill 0& \hfill 1-h{\alpha }_1& 0& 0\\ \hfill 0& \hfill 0& \hfill 0& 1& h\\ \hfill 0& \hfill 0& \hfill 0& 0& 1-h{\alpha }_2\end{array}\right]$$

(30)

$${\mathit{E}}_d=\left[\begin{array}{cc}\hfill 0& \hfill 0\\ \hfill 0& \hfill 0\\ \hfill h& \hfill 0\\ \hfill 0& \hfill 0\\ \hfill 0& \hfill h\end{array}\right],{\mathit{C}}_d=\left[\begin{array}{ccccc}\hfill 1& \hfill 0& \hfill 0& 0& 0\\ \hfill 0& \hfill 1& \hfill 0& 0& 0\end{array}\right]$$

(31)

The resulting EKF algorithm (see Brown and Hwang [10]) is summarized in

Table 1. Discrete-time EKF for SOG, COG, and course rate estimation.

Initial values	${\widehat{\mathit{x}}}^{-}\left[0\right]={\mathit{x}}_0$
	${\widehat{\mathit{P}}}^{-}\left[0\right]=\mathrm{E}\left[(\mathit{x}\left[0\right]-{\widehat{\mathit{x}}}^{-}\left[0\right]){(\mathit{x}\left[0\right]-{\widehat{\mathit{x}}}^{-}\left[0\right])}^{\top }\right]={\mathit{P}}_0$
Kalman filter gain matrix	$\mathit{K}\left[k\right]={\widehat{\mathit{P}}}^{-}\left[k\right]{\mathit{C}}_d^{\top }\left[k\right]{\left({\mathit{C}}_d\left[k\right]{\widehat{\mathit{P}}}^{-}\left[k\right]{\mathit{C}}_d^{\top }\left[k\right]+{\mathit{R}}_d\left[k\right]\right)}^{-1}$
State vector corrector	$\widehat{\mathit{x}}\left[k\right]={\widehat{\mathit{x}}}^{-}\left[k\right]+\mathit{K}\left[k\right]\left(\mathit{y}\left[k\right]-\mathit{h}\left({\widehat{\mathit{x}}}^{-}\left[k\right]\right)\right)$
Covariance matrix corrector	$\widehat{\mathit{P}}\left[k\right]=\left(\mathit{I}-\mathit{K}\left[k\right]{\mathit{C}}_d\left[k\right]\right){\widehat{\mathit{P}}}^{-}\left[k\right]{\left(\mathit{I}-\mathit{K}\left[k\right]{\mathit{C}}_d\left[k\right]\right)}^{\top }+\mathit{K}\left[k\right]{\mathit{R}}_d\left[k\right]{\mathit{K}}^{\top }\left[k\right]$
State vector predictor	${\widehat{\mathit{x}}}^{-}[k+1]={\mathit{A}}_d\widehat{\mathit{x}}\left[k\right]+{\mathit{B}}_d\mathit{u}\left[k\right]$
Covariance matrix predictor	${\widehat{\mathit{P}}}^{-}[k+1]={\mathit{A}}_d\widehat{\mathit{P}}\left[k\right]{\mathit{A}}_d^{\top }+{\mathit{E}}_d{\mathit{Q}}_d\left[k\right]{\mathit{E}}_d^{\top }$

where $\mathit{h}\left({\widehat{\mathit{x}}}^{-}\left[k\right]\right)={\mathit{C}}_d\left[k\right]{\widehat{\mathit{x}}}^{-}\left[k\right]$, and ${\mathit{Q}}_d\left[k\right]$ and ${\mathit{R}}_d\left[k\right]$ are the covariance matrices for the process and measurement noises. The a priori state and covariance matrix estimates (before update) are denoted $({\widehat{\mathit{x}}}^{-}\left[k\right],{\widehat{\mathit{P}}}^{-}\left[k\right])$, while the a posteriori state and covariance matrix estimates (after update) are denoted by $(\widehat{\mathit{x}}\left[k\right],\widehat{\mathit{P}}\left[k\right])$.

3.2. Five-State EKF: Latitude and Longitude Measurements

GNSS receivers output latitude, $\mu$, longitude, l, and elevation, h, using the World Geodetic System (WGS-84) ellipsoid as reference system [11]. For vehicles operating on the sea surface, we chose the height above the reference geoid as $h=0$. The  coordinate origin is conveniently fixed at a point on Earth’s surface, specified by its latitude and longitude pair $({\mu }_0,l_0)$. The Earth radius of curvature in the prime vertical, $R_N$, and the radius of curvature in the meridian, $R_M$, are (Farrell [2]),

$$\begin{array}{cc}\hfill R_N& =\frac{r_e}{\sqrt{1-e^2{sin}^2\left({\mu }_0\right)}}\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill R_M& =R_N\frac{1-e^2}{\sqrt{1-e^2{sin}^2\left({\mu }_0\right)}}\hfill \end{array}$$

(33)

where $r_e=6,378,137\mathrm{m}$ is the semi-minor axis (equatorial radius), and $e=0.0818$ is the Earth eccentricity (WGS-84). Consequently, the latitude and longitude dynamics are

$$\begin{array}{cc}\hfill \dot{\mu }& =\frac{1}{R_M}{v}_N=\frac{1}{R_M}Ucos\left(\chi \right)\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill \dot{l}& =\frac{1}{R_{N}cos\left(\mu \right)}{v}_E=\frac{1}{R_{N}cos\left(\mu \right)}Usin\left(\chi \right)\hfill \end{array}$$

(35)

where $v_N$ and $v_E$ are the North–East velocities of the vehicle. The discrete-time EKF model for longitude and latitude are obtained by Euler’s method

$$\begin{array}{cc}\hfill \mu [k+1]& =\mu \left[k\right]+h\frac{1}{R_M}U\left[k\right]cos\left(\chi \left[k\right]\right)\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill l[k+1]& =l\left[k\right]+h\frac{1}{R_{N}cos\left(\mu \left[k\right]\right)}U\left[k\right]sin\left(\chi \left[k\right]\right)\hfill \end{array}$$

(37)

$$\mathrm{Subjected} \mathrm{to}\left(19\right)$$

$$\mathrm{Subjected} \mathrm{to}\left(20\right)$$

$$\mathrm{Subjected} \mathrm{to}\left(21\right)$$

The state–space representation is

$$\mathrm{Subjected} \mathrm{to}\left(24\right)$$

$$\mathrm{Subjected} \mathrm{to}\left(25\right)$$

where

$$\begin{array}{cc}\hfill \mathit{x}& ={\left[\mu ,l,U,\chi ,{\omega }_{\chi }\right]}^{\top }\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill \mathit{y}& ={\left[\mu ,l\right]}^{\top }\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill \mathit{w}& ={[w_1,w_2]}^{\top }\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill \mathbf{\epsilon }& ={[{\epsilon }_1,{\epsilon }_2]}^{\top }\hfill \end{array}$$

(41)

The Jacobians are

$${\mathit{A}}_d=\left[\begin{array}{ccccc}\hfill 1& \hfill 0& \hfill h\frac{cos\left({\widehat{x}}_4\left[k\right]\right)}{R_M}& -h\frac{{\widehat{x}}_3\left[k\right]sin\left({\widehat{x}}_4\left[k\right]\right)}{R_M}& 0\\ \hfill A_{21}& \hfill 1& \hfill h\frac{sin\left({\widehat{x}}_4\left[k\right]\right)}{R_{N}cos\left({\widehat{x}}_1\left[k\right]\right)}& h\frac{{\widehat{x}}_3\left[k\right]cos\left({\widehat{x}}_4\left[k\right]\right)}{R_{N}cos\left({\widehat{x}}_1\left[k\right]\right)}& 0\\ \hfill 0& \hfill 0& \hfill 1-h{\alpha }_1& 0& 0\\ \hfill 0& \hfill 0& \hfill 0& 1& h\\ \hfill 0& \hfill 0& \hfill 0& 0& 1-h{\alpha }_2\end{array}\right]$$

(42)

where

$$A_{21}=h\frac{{\widehat{x}}_3\left[k\right]sin\left({\widehat{x}}_4\left[k\right]\right)tan\left({\widehat{x}}_1\left[k\right]\right)}{R_{N}cos\left({\widehat{x}}_1\left[k\right]\right)}$$

(43)

and

$$\mathrm{Subjected} \mathrm{to}\left(31\right)$$

4. USV Course Autopilot Design

The course angle dynamics can be approximated by a first-order model (Nomoto [12])

$$\mathrm{Subjected} \mathrm{to}\left(2\right)$$

$$\begin{array}{cc}\hfill {\dot{\omega }}_{\chi }+\frac{1}{T}{\omega }_{\chi }& =\frac{K}{T}{\tau }_N+d_{\omega }\hfill \end{array}$$

(44)

where T is the time constant in yaw, and K is implicitly defined by the ratio

$$\frac{K}{T}=\frac{1}{I_z-N_{\dot{r}}}$$

(45)

Here, $I_z$ is the moment of inertia, $-N_{\dot{r}}$ is the hydrodynamic added moment of inertia, ${\tau }_N$ is the yaw moment, and $d_{\omega }$ is a time-varying disturbance due to unmodeled dynamics and environmental disturbances. In practice, $d_{\omega }$ will be a nearly constant drift term, which can be compensated by an integral controller.

The Nomoto gain and time constants K and T, respectively, can be determined by a maneuvering test. e.g., a turning circle or a zigzag test (Fossen [1]). The course autopilot was chosen as a PID controller with reference feedforward

$${\tau }_N={\tau }_{FF}-K_p\left(\mathrm{ssa}\left(\tilde{\chi }\right)+\frac{1}{T_i}{\int }_0^t\mathrm{ssa}\left(\tilde{\chi }\right)\mathrm{d}\tau +T_d{\tilde{\omega }}_{\chi }\right)$$

(46)

where ${\tau }_N$ is the commanded yaw moment, $K_p$ is the proportional gain, $T_d$ is the derivative time constant, and $T_i$ is the integral time constant. The course angular velocity tracking error is denoted by ${\tilde{\omega }}_{\chi }={\omega }_{\chi }-{\omega }_{{\chi }_d}$, where the subscript d denotes the desired value. The unconstrained course angle tracking error $\tilde{\chi }=\chi -{\chi }_d$ is mapped to the interval $[-\pi ,\pi )$ using the operator $\mathrm{ssa}:\mathbb{R}\to [-\pi ,\pi )$ representing the smallest-signed angle (SSA) or difference between the two angles $\chi$ and ${\chi }_d$. The Marine Systems Simulator (MSS) Matlab implementation is (Fossen and Perez [13]),

The reference feedforward signal was chosen as

$${\tau }_{FF}=\frac{T}{K}{\dot{\omega }}_{{\chi }_d}+\frac{1}{K}{\omega }_{{\chi }_d}$$

(47)

where ${\omega }_{{\chi }_d}$ and ${\dot{\omega }}_{{\chi }_d}$ are the desired angular velocity and acceleration, respectively. The resulting closed-loop system is

$$\ddot{\tilde{\chi }}+\left(\frac{1}{T}+\frac{K{K}_p{T}_d}{T}\right)\dot{\tilde{\chi }}+\frac{K{K}_p}{T}\mathrm{ssa}\left(\tilde{\chi }\right)+\frac{K{K}_p}{T{T}_i}{\int }_0^t\mathrm{ssa}\left(\tilde{\chi }\right)\mathrm{d}\tau =d_{\omega }$$

(48)

The PID controller gains can be determined by pole placement with ${\omega }_n$ and $\zeta$ as design parameters. This gives

$$\begin{array}{cc}\hfill \frac{K{K}_p}{T}:={\omega }_n^2& \Rightarrow K_p=\frac{T}{K}{\omega }_n^2\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill \frac{1}{T}+\frac{K{K}_p{T}_d}{T}:=2\zeta {\omega }_n& \Rightarrow T_d=\frac{T}{K{K}_p}\left(2\zeta {\omega }_n-\frac{1}{T}\right)\hfill \end{array}$$

(50)

The integrator time constant was chosen such that $1/T_i={\omega }_n/10$. In other words,

$$T_i=\frac{10}{{\omega }_n}$$

(51)

This guarantees that the tracking errors $\tilde{\chi }$ and ${\tilde{\omega }}_{\chi }$ converge exponentially to zero under the assumption that ${\dot{d}}_{\omega }=0$. Global exponential stability cannot be proven since the course angle error is defined on $[-\pi ,\pi )$ and not $\mathbb{R}$ as shown by Bhat and Bernstein [14].

5. Simulation Study of the Otter USV

In the simulation study, a mathematical model of the Maritime Robotics Otter USV, length $L=2.0\mathrm{m}$, was used. The model is included in the Matlab MSS toolbox (Fossen and Perez [13]) as a function

xdot = otter(x,n,mp,rp,V_c,beta_c)

which returns the time derivative, xdot, of the state vector

x = [u,v,w,p,q,r,x,y,z,phi,theta,psi]’

The inputs are the left and right propeller shaft speeds n = [n1,n2]’, the mass of the payload, m_p, the location of the payload, r_p = [x_p,y_p,z_p]’, ocean current speed, V_c, and ocean current direction, beta_c. The toolbox also has a Simulink block for numerical integration of the m-file function.

Estimation of SOG, COG and Course Rate during Course Autopilot Control

In the simulation study, the five-state EKF in Section 3.1 was used to estimate the course angle and course rate, which are the feedback signals needed to implement the course autopilot (46). The filter sampling frequency was chosen as $50\mathrm{Hz}$, while the GNSS position measurements were received at $5\mathrm{Hz}$.

The MSS Otter USV model is controlled by two propellers with shaft speeds $n_1$ and $n_2$ in $\mathrm{rad}/\mathrm{s}$. The propellers produce a surge force ${\tau }_X$ and a yaw moment ${\tau }_N$ according to

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\tau }_X\\ {\tau }_N\end{array}\right]& =k\left[\begin{array}{cc}1& 1\\ -l_1& -l_2\end{array}\right]\left[\begin{array}{c}|n_1|n_1\\ |n_2|n_2\end{array}\right]\hfill \end{array}$$

(52)

where $l_1=-0.395\mathrm{m}$, $l_2=0.395\mathrm{m}$ and $k=0.0111\mathrm{kgm}$ is the propeller gain.

The operator specifies the desired force ${\tau }_X$ in the surge direction, while the course autopilot (46) computes the desired yaw moment ${\tau }_N$. The control allocation problem is solved by applying the inverse mapping

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]& =\frac{1}{k}{\left[\begin{array}{cc}1& 1\\ -l_1& -l_2\end{array}\right]}^{-1}\left[\begin{array}{c}{\tau }_X\\ {\tau }_N\end{array}\right]\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill n_1& =\mathrm{sgn}\left(u_1\right)\sqrt{|u_1|}\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill n_2& =\mathrm{sgn}\left(u_2\right)\sqrt{|u_2|}\hfill \end{array}$$

(55)

The Nomoto time and gain constants of the Otter USV were estimated to

$$\begin{array}{cc}\hfill T& =1.0\mathrm{s}\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill K& =0.0242\mathrm{kg}{\mathrm{m}}^2\mathrm{s}\hfill \end{array}$$

(57)

The course autopilot system was implemented as

$$\begin{array}{cc}\hfill {\tau }_X& =\mathrm{Pilot}\mathrm{input}\hfill \end{array}$$

(58)

$$\mathrm{Subjected} \mathrm{to}\left(46\right)$$

where the controller gains are determined by (49)–(51) by specifying $\zeta =1.0$ and ${\omega }_n=1.5\mathrm{rad}/\mathrm{s}$. This yields

$$K_p=93.15,T_d=0.89,T_i=6.67$$

(59)

Figure 4 shows the North–East positions during autopilot control. The propeller commands are shown in Figure 5 where the control allocation algorithm (53)–(55) was applied. Figure 6 clearly demonstrates that the EKF was able to estimate the unmeasured states U, $\chi$, and ${\omega }_{\chi }$ quite accurately. The zoomed windows show the 5 Hz slow update rate of the filter (GNSS measurements frequency) compared to the 50 Hz sampling frequency of the predictor.

6. Experiments with the Mariner USV

In the first experiment, the Mariner USV was used. The geographical location is shown in Figure 7. The blue line indicates the traveled path in the Trondheim fjord, Norway. The experiments were performed in sea state 2 corresponding to wave amplitudes below 0.5 m. Latitude and longitude were measured using a u-blox NEO-M8Q GNSS receiver at 5 Hz [15], while the SOG and COG measurements were complementary measurements used to benchmark the EKF. The accuracy of the NEO-MQ8 in the horizontal plane was 2.5 m when using GPS/Glonass.

It should be noted that the GNSS values for SOG and COG were not validated. Hence, they do not represent groundtruth. Because of this, Figure 8 only shows the difference between the two algorithms. The GNSS receiver determines the distance between two fixes, and by using the time taken to travel this distance it can deduce its speed. The COG and SOG can therefore seem erratic under certain conditions. For example, when the USV was moving slowly through rough seas, the antenna moved from side to side as well as in the direction of the vehicle. In contrast, the EKF estimates were not affected by this.

The Kalman filter covariance matrices were chosen as ${\mathit{Q}}_d=\mathrm{diag}\{1\times {10}^7,1\times {10}^3\}$ and ${\mathit{R}}_d=1\times {10}^{-8}\mathrm{diag}\{1,1\}$. Figure 8 shows the performance of the five-state EKF. The EKF succeeded in estimating both the SOG and COG with good accuracy.

7. Experiments with the Otter USV

In the second experiment, the Otter USV was used. The USV’s location is shown in Figure 9. The blue line indicates the traveled path, which is in the proximity of the Maritime Robotics main office in the Trondheim harbor, Norway. The experiments were performed in sea state 1 corresponding to wave amplitudes below 0.1 m. The Otter USV is a much smaller vehicle than the Mariner USV and it typically operates at very low speed (0–2 m/s). This is challenging for the state estimator since the course angle is not defined at zero speed. However, the EKF was stable even at zero speed thanks to the non-zero Singer constants ${\alpha }_1$ and ${\alpha }_2$ in the model. Latitude and longitude were measured using a u-blox NEO-M8Q GNSS receiver at 5 Hz [15], while the SOG and COG measurements were complementary measurements used to benchmark the EKF. The filter covariance matrices were chosen as ${\mathit{Q}}_d=5\times {10}^5\mathrm{diag}\{1,1\}$ and ${\mathit{R}}_d=1\times {10}^{-8}\mathrm{diag}\{1,1\}$.

The experiment with the Otter USV was repeated, but, the second time, the vehicle was taken out of the harbor to operate in the middle of the Trondheim fjord. However, similar performance was achieved as seen from the plots in Figure 10 and Figure 11. From this it can be concluded that the EKF works very well at forward speed while it reaches an arbitrary course angle at zero speed. This is expected since the course angle is not defined at zero speed.

8. Discussion

The experiments with the Otter and Mariner USV systems confirm that the COG, SOG, and course rate can be estimated from latitude and longitude measurements with great accuracy when the speed is above a certain threshold value (typically 0.5 m/s). The experiments also confirm that the speed estimates are less accurate close to zero speed. At zero speed the course angle is not defined. Hence, the course angle estimate will converge to an arbitrary angle in the interval $[-\pi ,\pi )$. However, the EKF is exponentially stable at zero speed thanks to the Singer constants in the filter.

9. Conclusions

The main result of the article is a five-state extended Kalman filter (EKF) aided by GNSS latitude-longitude measurements for efficient estimation of course over ground (COG), speed over ground (SOG), and course rate. This is of particular interest for unmanned surface vehicle (USV) systems equipped with low-cost navigation sensor suites. For such systems, a gyrocompass is too expensive compared to the cost of the vehicle. A magnetic compass is unreliable due to electromagnetic interference caused by propellers and thrusters. A dual-antenna GNSS system on the same receiver (RTK GNSS) is an alternative, but the operational reliability (number of dropouts) depends on the density of the reference-station network, number of available satellites, multipath, ionospheric disturbances, etc. Furthermore, it has been demonstrated that the five-state EKF can estimate the SOG, COG, and course rate of a USV quite accurately using a single low-cost u-blox GNSS receiver. It has also been shown that the state estimates can be used to implement course and path-following control systems onboard the USV. The performance of the five-state EKF has been experimentally verified by using navigational data from two commercial USV systems, the Otter and the Mariner USVs by Maritime Robotics, which were operated west of Trondheim, Norway. The results from the experiments demonstrated that the EKF could estimate all states from low-cost latitude–longitude measurements.