Line-of-sight path-following control utilizing an extended Kalman filter for estimation of speed and course over ground from GNSS positions

Path-following control systems for ships can be designed using both heading and course angle autopilots in conjunction with a proportional line-of-sight (LOS) guidance law. Ships are usually equipped with a gyrocompass from a safety perspective since magnetic compasses are susceptible to magnetic disturbances. Unfortunately, the gyrocompass is an expensive device, and smaller vessels and boats cannot afford to use this as the primary device for steering. An alternative solution is to compute the course over ground (COG) and speed over ground (SOG) from global navigation satellite systems (GNSS) and use these signals for feedback control. This article presents course autopilot design for path following and a five-state extended Kalman filter (EKF) to estimate the COG and SOG efficiently. Even though many algorithms exist for computation of the COG and SOG, it is advantageous to design an EKF since a state estimator can be extended to include other sensory systems such as Doppler Velocity Log (DVL), radar, attitude rate sensors, computer vision systems, etc. This is in contrast to proprietary systems that do not allow the user to modify the software. The convergence and accuracy of the EKF are significantly improved by using target-tracking models in combination with kinematic equations. A high-fidelity model of a MARINER class cargo ship is used in the path-following case study. From the simulation study, it can be concluded that the EKF successfully estimates the COG and the SOG from GNSS measurements during path following. The solution is remarkably robust and accurate, and when combined with a course autopilot, the need for a compass is eliminated during path following.


Introduction
Ships are usually equipped with a gyrocompass, which is a nonmagnetic compass based on a fast-spinning disc (Fossen [1]). A north-seeking gyro gives a highly accurate measurement of the heading (yaw) angle, and this is the preferred sensor from the safety point of view. Smaller boats cannot afford to use expensive sensors such as gyrocompasses. An alternative could be to use a magnetic compass. However, magnetic compasses are not used on-board commercial ships as navigational devices since they are susceptible to magnetic disturbances. A magnetic compass is, in fact, an extremely simple device. It consists of a small, lightweight magnet balanced on a nearly frictionless pivot point. However, the magnetic field of the Earth is not perfectly aligned along the Earth's rotational axis. It is skewed slightly offcenter. This skew or bias is called declination, and it must be compensated for. Sensitivity to magnetic variations and declination causes significant problems when designing path-following control systems.
An alternative measurement could be to use two GNSS antennas on the same receiver with a known offset vector to compute the heading angle. However, this method requires good GNSS signal reception and it can be sensitive to ionospheric disturbances, multipath, loss of signals, the number of available satellites, etc.
Suppose the heading angle is available as a measurement. In that case, it is straightforward to design a proportional-integral-derivative (PID) controller for pathfollowing control using a conventional heading autopilot (see Fossen [1]). However, the objective of this article is to discuss an alternative low-cost path-following controller for ships using course autopilots. The motivation for this is to avoid using an expensive gyrocompass, an unreliable magnetic compass, or two GNSS antennas (Farrell [2]) when designing ship path-following control systems. To succeed, it is necessary to employ a robust algorithm for estimating the ship's COG, SOG, and course rate. These are the main signals needed to implement the LOS path-following guidance law of Lekkas and Fossen [3], or the vector-field guidance law of Nelson et al. [4] both utilizing the course angle for feedback.
Li and Jilkov [5] have reviewed dynamic models for target tracking in 2D and 3D. The simplest models for a targeting maneuver are the white-noise constant velocity (CV) and constant acceleration (CA) models (Bar-Shalom et al. [6]). In this work, it is assumed that the target velocity ̇x = w or acceleration ẍ = w are independent processes driven by Gaussian white noise w. The CV model works well for ship path-following control since the velocity is nearly constant even though the ship experiences small accelerations. The CA model can be used to describes more aggressive maneuvers (Li and Jilkov [5]). When a moving ship or a target's trajectory satisfies a kinematic constraint, the kinematic constraint can be utilized as additional information about the ship to improve the tracking of the time-varying velocity and bearing. Tahk and Speyer [7] proposed to incorporate the kinematic constraints as pseudomeasurements. Using the kinematic constraint, it is possible to remove some of the uncertainty of the timevarying velocities and force the velocity estimates to change consistently with the dynamics of a moving ship. A formulation of the kinematic constraint for constant speed has been presented by Alouani and Blair [8] where the filter utilizes pseudomeasurements to obtain unbiased estimates. The EKF presented in this article uses a CV model for the speed dynamics, while the course angle dynamics is modeled as a CA model to obtain estimates of both the course angle and the course rate (Fossen and Fossen [9]). The North-East velocity equations of the ship are represented by amplitude and phase to avoid that the yaw angle enters the kinematic equations of motion and at the same time preserve the kinematic constraints for a moving object in 2D. This ensures that the speed and course angle satisfy the kinematic constraints when estimated in an EKF. This is important to obtain accurate estimates of the COG and the SOG.
The remainder of this article is organized as follows: Sect. 2 presents the kinematic equations and EKF for COG, SOG, and course rate estimation. Section 3 presents the LOS path-following controller utilizing a course autopilot system. Section 4 contains a simulation study of a MARINER class cargo ship in a path-following scenario. The concluding remarks are drawn in Sect. 5.

Estimation of COG, SOG and course rate
This section contains the kinematic preliminaries and EKF for estimation of COG, SOG, and course rate.

Kinematic preliminaries
The relationship between the angular variables course, heading and sideslip is important for ship maneuvering. The terms course and heading are used interchangeably in much of the literature on guidance, navigation and control of marine craft, and this leads to confusion. Let the BODY and North-East-Down (NED) references frame in Fig. 1 be denoted {b} = (x b , y b , z b ) and {n} = (x n , y n , z n ) , respectively. Then, the following definitions can be made: Definition 1 (Yaw or heading angle ) The angle from the x n axis (true North) to the x b axis of the ship, positive rotation about the z n axis by the right-hand screw convention.
The heading angle is usually measured using a magnetic compass, gyrocompass, or two GNSS receivers; see Gade [10] for a discussion on 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. For small vehicles and boats, the gyrocompass is expensive, while it is difficult to use a magnetic compass since it is prone to electromagnetic disturbances. This makes course autopilots for path-following control a preferred solution, provided that the course angle can be accurately estimated.
Definition 2 (Course angle ) The angle from the x n axis (true North) to the velocity vector of the ship, 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 ship can be described by (see Fossen [1]) where (u, v) are the surge and sway velocities, respectively. Equations A ship is exposed to ocean currents. Let (u c , v c ) denote the velocity components of an irrotational current. Hence, the ocean current speed is (see Fig. 1), and the relative speed Figure 1 shows the sideslip angle for a ship as defined below.
Definition 4 (Sideslip angle ) The angle from the x b axis to the relative velocity vector of the ship, positive rotation about the z b axis by the right-hand screw convention.

Five-state extended Kalman filter
The primary objective of the EKF is to compute accurate estimates of the COG and SOG of the ship when moving along the path (Fossen and Fossen [9]). In addition, it is desirable to calculate an estimate of the course rate. It is essential that the estimates are properly filtered and that old data points (memory effects) are included in the filter. This is the primary motivation for using the Kalman filter. Since the path is not parametrized, the only information during path following is the ship's North and East positions, which are assumed measured by a GNSS receiver. The North-East positions (x n , y n ) of a ship are given by the kinematic equations (3) and (4), while the speed U can be modeled by a CV model driven by white noise. The course angle is modeled as a CA model to obtain estimates of the course rate ̇= . This corresponds to using white-noise-driven double and triple integrators in the targettracking literature, see Li and Jilkov [5].
Simulation studies revealed that the CV and CA models were most accurate for ship path following since the speed and course rate are nearly constant most of the time. The only observed changes of speed and course rate are when the ship turns in the vicinity of a waypoint. The EKF is implemented using Cartesian coordinates (x n , y n ) under a flat-Earth assumption. For global navigation applications the longitude-latitude representation should be used as described by Fossen and Fossen [9].
State-space model The resulting state-space model expressed in Cartesian coordinates is where w 1 and w 2 are Gaussian white-noise processes, and where two small constants 1 > 0 and 2 > 0 have been added to the model to ensure that U and converge to zero during dead reckoning and stationkeeping. Equations (13) and (15) are referred to as Singer models [11] in the targettracking community. The corresponding North-East GNSS position measurements are where 1 and 2 are Gaussian white-noise processes. The state and noise vectors corresponding to (11)- (15) and (16)-(17) are Consequently, the discrete-time state-space model can be expressed as where (16) The model is discretized using Euler's method and h denotes the sampling time. The resulting EKF algorithm for (22) and (23)

LOS path following using course control
The control system is implemented as two successive loops as shown in Fig. 2. The guidance law and course autopilot are presented below.

Course autopilot
The course angle dynamics is approximated by a first-order model (Nomoto et al. [13]) where is the rudder angle and d is a constant disturbance due to unmodeled dynamics and environmental disturbances. In practice, d will be a nearly constant drift term but an integral controller will be able to compensate for drift. The Nomoto gain and time constant K and T, respectively can be determined by a maneuvering test. e.g. a turning circle or a zigzag test or system identification (Fossen [1]). Note that the yaw rate r of the ship relates to the course rate by   [14]).
The reference feedforward term is chosen as where d and ̇d are the desired angular velocity and acceleration, respectively.
The resulting closed-loop system is The controller gains can be determined by pole placement with n and as design parameters. This gives The integrator time constant is chosen such that 1∕T i = n ∕10 . In other words, This guarantees that the tracking errors ̃ and ̃ converge exponentially to zero under the assumption of ḋ = 0.

LOS path-following guidance law
For a path made up of straight-line segments, the desired course angle d can be computed using the proportional LOS guidance law (Lekkas and Fossen [3]), where the proportional gain K p is parametrized in terms of the look-ahead distance > 0 . Formula (35) is easily verified from Fig. 3 where it is seen that the slope of the line segment is p = d + tan −1 (y p e ∕ ) . The cross-track error y p e between two successive waypoints p n i = [x n i , y n i ] ⊤ and p n i+1 = [x n i+1 , y n i+1 ] ⊤ is computed by where (x n , y n ) is the ship's position expressed in NED.
Lyapunov stability analysis Assume that the path-tangential coordinate system {p} has its origin at the waypoint p n i = [x n i , y n i ] ⊤ . From this it follows that cross-track error dynamics under the assumption that the course controller ensures perfect tracking, that is = d , satisfies where the trigonometric identity sin(tan −1 (x)) = x∕ √ 1 + x 2 has been applied. Consider the Lyapunov function candidate V = 1∕2(y p e ) 2 . Time differentiation of V and substitution of (37) gives, Consequently, the equilibrium point y p e = 0 is uniformly semi-globally asymptotically stable (USGES) according to Theorem 1 by Fossen and Pettersen [15].
Reference feedforward The course controller (29) makes use of the feedforward term (30) which depends on d and ̇d . Time differentiation of (35) gives The desired angular acceleration ̇d can be computed by time differentiation of (39). However, for practical implementations, it is recommended to use ̇d = 0 since the angular acceleration is a very small number for most ships.

Simulation study of a MARINER class cargo ship
The hydrodynamic coefficients of a MARINER class cargo ship, L pp = 160.9 m and ∇ = 16, 622 DWT , have been determined by means of a planar motion mechanism (PMM) and compared with the results of full-scale trials by Chislett (36) y p e = − x n − x n i sin p + y n − y n i cos p , and Strøm-Tejsen [16]. The resulting model is an m-file, mariner.m, which is implemented in the Marine Systems Simulator (MSS) together with autopilots and LOS guidance laws that can be simulated in Matlab/Simulink (Fossen and Perez [14]). The toolbox also has a Simulink block for simulation of the MARINER m-file; see  where the filter cut-off frequency 1∕T lp should be 5-10 times smaller than the natural frequency n of the closed-loop course angle dynamics. The PID controller gains are chosen as K p = 1.45 , T d = 36.3 s and T i = 200 s corresponding to n = 0.05 rads −1 and = 1.0 . The look-ahead distance was chosen as = 800 m . The performance of the LOS guidance law and course autopilot is shown in Fig. 5 where the waypoints are (0, 0), (2000, 0), (5000, 5000), (3000, 8000), (6000, 12, 000) and (10000, 12, 000) are traversed. The next waypoint is activated when the remaining distance to travel is R switch = 50 m . This is mathematically equivalent to where d i+1 = ‖p n i+1 − p n i ‖ is the distance between the waypoints p n i and p n i+1 and x p e is the along-track error (see Fig. 3), Figure 6 shows the true, estimated, and desired course angle during path following while Fig. 7 contains the corresponding speeds. The simulation study of the MARINER class cargo ship confirms that the five-state EKF is capable of estimating the COG, SOG, and course rate with great precision during path following. The course autopilot produces nonaggressive rudder angles as shown in Fig. 8. Also note that the rudder rates are less than 5 deg/s during course changing. This confirms that the desired bandwidth of the controller is chosen such that saturation is avoided.
x p e = x n − x n i sin p + y n − y n i cos p .

Conclusions
A path-following control system for ships using a course autopilot in conjunction with a proportional line-of-sight (LOS) guidance law has been presented. The primary motivation for using a course autopilot was to avoid the expensive gyrocompass or the unreliable magnetic compass in the feedback loop. Accurate estimates of the ship course angle and speed were obtained by a five-state EKF, which was designed using constant velocity and constant acceleration target-tracking models. Finally, the EKF was applied to a high-fidelity model of a MARINER class cargo ship.  The case study was path following and course control using the EKF course angle and course rate estimates. From the simulation study, it can be concluded the EKF successfully estimates the COG and the SOG from GNSS measurements during path following even when using a low-cost GNSS receiver without correction signals. The solution was remarkably robust and accurate when combined with a course autopilot. Hence, the need for a compass was eliminated during path following.