Experimental Evaluation of A New Developed Algorithm for An Autonomous Surface Vehicle and Comparison with Simulink Results

Performing repeatable duties automatically was the dreams of human being for centuries. Although full autonomy has long been dreamed of by visionaries, many researches have been performed for surface vehicles automation since the last century to get close to this dream stepwise. To increase daily working hours and accuracy and reduce cost, operations such as hydrography are susceptible for autonomy. Beside platform topology, installed sensors and energy resources, the core elements of any autonomous surface vehicle are navigation, guidance and control systems. To perform bathymetry operation in autonomy manner, a reliable and robust navigation algorithm is designed and embedded in an autonomous surface vehicle titled Morvarid. Morvarid is a plug-in hybrid solar powered catamaran boat. The developed algorithm is a combination of extended Kalman filter, search ball and potential field approaches. Many experimental field tests are carried out after simulation in Simulink environment. Test results illustrated the algorithm and improved the path followed by reducing SD and RMSE and there is a good correlation between simulation run and experimental tests.


Introduction
In the last century many researches have been performed about development, importance and applications of unmanned marine vehicles (UMVs) that most of them are reviewed by . According to Naeem et al. (2016), UMVs are generally classified into underwater and Surface Vehicles (SVs). Surface vehicles have gained a widespread interest for applications ranging from oceanography, remote sensing, weapons delivery, force multipliers, environmental monitoring, surveying and mapping, and providing navigation and communication support to underwater vehicles. Mousazadeh et al. (2017) categorized SVs, as an Autonomous Surface Vehicle (ASV) that is different from an Unmanned Surface Vehicle (USV), which relies on a user to remotely operate the vehicle. An ASV could operate without men supervision. Tang et al. (2015) explained that ASVs are robots for operation at lakes, canals, harbors and even open seas that are characterized by small size, good hiding capability, high mobility, and low price. One of the well-known applications for USVs would be bathymetry (Campbell et al., 2012). According to Simetti et al. (2012), the need for innovative and reliable technologies and using of USVs for reducing the harbour vulnerability was the subject of some researches. USVs can perform patrolling surveys of the more crucial waterways, providing a remote operator with acoustic or optic images acquired through the on-board sensory equipment.
From the view of reaction forces applied to SVs, marine crafts can be classified according to their maximum operating speed (U) and length of the submerged portion (L) using the Froude number: Fr=U/(Lg) 0.5 , where g is the acceleration due to gravity. The weight of a vessel operating with a Froude number smaller than about 0.4 to 0.5 is almost completely supported by the hydrostatic force of buoyancy that is proportional to the volume of fluid displaced (displacement mode). As the Froude numbers are larger than about 1.0 to 1.2, the weight is almost completely supported by a hydrodynamic force that is roughly proportional to the square of the speed (planning mode). For intermediate values of the Froude number, the vessel's weight is supported by both the hydrostatic and hydrodynamic force (semi-displacement vessel) (Fossen, 2011).
Control design for ASVs is a subject of great interest since these vehicles are strongly non-linear and show complex hydrodynamics effects. Qin et al. (2017) explained that two significant trends have recently become noticeable in USV motion control studies. One trend is to imbue the control system with intelligence, with the goal of achieving autonomous control of USVs. The other trend is to extend autonomous control from single vessels to multiple vessels. In spite of strong demand for the development of innovative ASVs, only semi-autonomous USVs have normally been used rather than fully-autonomous ASVs (mainly due to challenges in automated and reliable guidance, navigation and control (GNC)) .
Recently, computers and sensors have reached the degree of maturity necessary to provide mobile platforms with a certain degree of autonomy. Giordano et al. (2015) equipped a bathymetry USV with a differential Global Position System (GPS) system and single beam echo sounder; inertial platform for attitude control; ultrasound obstacle-detection system with temperature control system; emerged and submerged video acquisition system. Their experiments results showed that integrating several existing technologies improved the final performance and quality of the acquired data. Caccia et al. (2008) illustrated that in spite of high accuracy and expensive positioning systems (e.g. RTK-GPS), some experiments demonstrated the effectiveness of extended Kalman filter (EKF) to perform basic control tasks such as auto-heading, auto-speed and straight line following a USV equipped only with GPS and compass.
Generally speaking, the problems related to the motion control and guidance of ASVs are classified in the literature into three basic groups (Chaos et al., 2009;Bibuli et al., 2009): point stabilization, path following, and path-tracking (trajectory tracking). To efficiently and effectively navigate the ASV formation, path planning algorithms are required to generate optimal trajectories and provide practical collision avoidance manoeuvres. As the ASVs are underactuated and restricted by various motion constraints, in a research of Liu and Bucknall (2016) presented a new algorithm named the "angle-guidance fastmarching square" (AFMS) to make the generated path compliant with vehicle's dynamics and orientation restrictions. In another research of Liu and Bucknall (2015) introduced a path planning algorithm for the USV formation navigation based on the fast marching (FM) method, which has features of fast computation speed and low computation complexity. A novel model based backstepping controller is designed by Dong et al. (2015) for trajectory tracking control of underactuated ASV in the horizontal plane. The well-known Persistent Exciting (PE) conditions of yaw velocity are completely relaxed in this study. After simulation, it is shown that the designed controller could drive ASV tracking in an arbitrary trajectory. Peng et al. (2017) described the design and realization of an intelligent mobility platform for hydrographic surveying and charting. The USV has two kinds of control modes, remote manual control and beyond visual range wireless remote control and autonomous tracking and collision avoidance algorithm. Experimental tests in a rectangular test-bed illustrated that the tracking error was in the range of ±3 m and the yaw changes were in the range of ±2° with economic speed of 7.5 kn. And finally, the potential field is an algorithm to manage multiple behaviors. The basic idea of potential fields is from the behavior exhibited by the charged particle navigating through a magnetic field. Unlike magnetic fields where the topology is externally specified by environmental conditions, the topology of the potential fields that a robot experiences is determined by the designer. More specifically, the designer (a) creates multiple behaviors, each assigned a particular task or function, (b) represents each of these behaviors as a potential field, and (c) combines all of the behaviors to produce the robot's motion by combining the potential fields (Goodrich, 2002).
With reviewed texts, designing a reliable and robust algorithm for navigation of ASVs is nowadays the subject of dispute. So, the main objectives of this research would be categorized as: (1) determining the dynamic model of designed ASV, (2) introducing a novel algorithm for trajectory tracking (path planning) and experimental evaluation of ASV autonomously in real situations, and (3) modeling the developed algorithm by block diagrams and comparison of the field data with simulation results.

Materials and methods
An ASV titled 'Morvarid 1 ' is designed and developed for bathymetry in shallow waters. The platform is shaped as catamaran with net curb weight of approximately 700 kg with about 5.5 kn forward speed (more information about Morvarid is provided in Mousazadeh et al. (2018)). Morvarid is equipped with some local perception and global monitoring sensors such as the range finder Lidar, a set of ultrasonic sensors, three depth-scanner sonar, a high resolution GNSS (~1 m accuracy), one real time infrared camera, a stereoscopic range finder system, an onboard industrial computer (IPC), a long-range (~10 km) wireless connection system to another computer on land, an auto-pilot system, and an IMU. Communication between Morvarid and the bureau is based on an Ethernet network capable of data sending to the maximum 300 Mbps in distances of 10 km. Schematic overview of the Morvarid robot boat and some communication protocols is illustrated in Fig. 1.
Morvarid ASV could navigate in four navigation modes: (1) The first priority is the manual driving mode. In this mode until a person sits on the boat to control the joystick on-board, the last three navigation modes would be inactive; (2) The second priority is allocated to remote control sys- Hossein MOUSAZADEH, Ali KIAPEY China Ocean Eng., 2019, Vol. 33, No. 3, P. 268-278 269 tem from offshore; (3) The third priority is the control from office using a GUI and finally; (4) Fully autonomous trajectory tracking or path planning that is the subject of this research. All data received in the boat communicated to office by Wi-Fi network as on-line. The Morvarid office software is equipped with a GUI that is developed using Visual C#, and bathymetry map is sketched in real-time. The software allows a user to import a rectified aerial picture of the region of operation either from any source that provides linearly scaled maps such as Google Earth. After the picture has been loaded, the software allows the user to calibrate the map using a two-point calibration. Once a calibrated map has been loaded, the user can connect the boat and view the boat on the display overlaid on the map in the form of a red arrow. To navigate autonomously by trajectory tracking (path planning), user should click on four points at the corners of desired area to define four pair of position data (longitude and latitude). When these four pairs of data communicated from office to IPC, meshing of surface area is start. The area is divided into trapezoidal form by lines of user defined distance (e.g. 20 m) and tracking points on the line (e.g. 4 m). However, the algorithm is designed in such a way that can track both curved trajectory and straight line trajectory with high accuracy. After design and simulation, some experiments will be carried out to demonstrate the effectiveness of the designed controller.

Ship dynamics
This section discusses deriving the dynamic model of the marine vehicle in six degrees of freedom (DOF) since six independent coordinates are necessary to determine the position and orientation of a rigid body. According to Fossen (1994), it is assumed two coordinate frames: the moving coordinate frame that is fixed to the vehicle and is called body-fixed reference and an earth fixed reference frame. As shown in Fig. 1, the origin of the body-fixed frame is usually chosen to coincide with the center of gravity (CG) and its x-axis aligned with the vehicle's forward velocity vector. The kinematic equation for transforming body-fixed reference to earth-fixed one can be expressed as (Fossen, 1994): where η=[x, y, z, φ, θ, ψ] T denotes the position and orientation vector with coordinates in the earth-fixed frame, ν= [u, v, w, p, q, r] T is the generalized linear and angular velocity vector in the body-frame coordinate, and J (η) is the transformation matrix that for details the reader can refer to Fossen (1994). Here x, y and z are the vehicle position and φ, θ and ψ are the orientation in earth-fixed frame. Also u, v and w are the velocities respectively in the surge, sway and heave and p, q, and r are the angular velocities respectively around the roll, pitch and yaw ( Fig. 1). For surface vehicles the depth z and pitch θ variables are not applicable. Also the roll φ variation is found to be negligible during experimental evaluations . For simplicity ignoring the swing (z≈cons. and w≈0), pitch (θ≈0 and q≈0) and roll (φ≈0 and p≈0), the kinematic model in Eq. (1), which describes the geometrical relationship between the earth-fixed and vehicle fixed motion is given as: According to Newtonian and Lagrangian mechanics, nonlinear dynamic equation of motion can be conventionally expressed as (Fossen, 1994): where M RB denotes the rigid-body inertia matrix (including the added mass) and M RB = >0, and C RB ( ) denotes the matrix of Coriolis and centripetal terms (including added mass) and C RB ( ) = -C RB ( ) T , D RB ( ) is the damping matrix, g(η) is the vector of the gravitational forces and moments and τ=[X, Y, Z, K, M, N] T is the vector of con- trol input including the force and moments in three directions. In this case since the inputs are only from two port and starboard motors the ASV is the deal as underactuated surface ship (fewer numbers of actuators than the to-be-controlled degrees of freedom). However, the use of underactuated vehicles is very important for different reasons like simplicity, cost, efficiency, etc. (Chaos et al., 2009).
By considering T port as the portside thrusting force and T stbd as the starboard thrusting force each in surge (positive in the forward and negative in the backward direction), the control input vector becomes: where B is the distance between two thrusting power sources (1.2 m in this case). Differential steering imposes a moment about the vehicle that causes it to turn. There are distinct advantages to this type of systems: primarily, the turning radius for the vehicle is substantially reduced, as well as the additional capability to steer without the requirement of a forward speed (Bertaska et al., 2015). The mathematical model of an underactuated ship moving in the surge, sway and yaw is obtained from the motion equation of the ship moving in six degrees of freedom under disturbances induced by wave, wind and ocean current. Furthermore, it is assumed that the inertia, added mass and damping matrices are diagonal. This assumption holds when the vessels have three planes of symmetry, for which the axes of the body-fixed reference frame are chosen to be parallel to the principal axis of the displaced fluid, which are equal to the principal axis of the vessel. Most ships have the port/starboard symmetry. Moreover, the bottom/top symmetry is not required for the horizontal motion. Ship fore/aft nonsymmetry implies that the off-diagonal terms of the inertia and damping matrices are nonzero. However, these terms are small compared with the main diagonal terms. So the underactuated ship moving in the surge, sway and yaw can be described as (Doa et al., 2004): The positive constant terms m jj , where 1≤j≤3 denotes the ship inertia including the added mass and is derived according to Eq. (6). The positive constant terms d u , d v , d r , d ui , d vi , and d ri , where i=2, 3 represent the hydrodynamic damping in the surge, sway and yaw. For simplicity, the higher nonlinear damping terms are ignored (Doa et al., 2004 andReyhanoglu, 1996). The time-varying terms, τ wu (t), τ wv (t) and τ wr (t), are the environmental disturbances induced by wave, wind and ocean-current with |τ wu (t)|≤τ wumax <∞, |τ wv (t)|≤τ wvmax <∞ and |τ wr (t)|≤τ wrmax <∞. Finally, the available controls are the surge force τ u and yaw moment τ r that are calculated as Eq. (4).
The parameters added mass and damping coefficients are the unknown variables to be derived. Mass parameters include the added mass contributions that represent hydraulic pressure forces and torque due to the forced harmonic motion of the vessel which are proportional to acceleration. With the estimate of the added mass terms presented by Fossen (1994) for the experimental model boat, it would be derived as (Muske et al., 2008): where m=700 kg is the actual mass of the USV, L=3 m is the effective length, W=0.8 m is the width, D=0.5 m is the mean submerged depth, and ρ is the density of water. About the damping coefficients, according to (Ma, 2015): For defining d u , d v and d r , the combination of experimental results, simulation and conformance with some references are used. As the damping parameters are dependent on the boat shape and dimension, main related parameters of some USVs as well as Morvarid are given in Table 1.
According to Table 1, the ratios of m 11 /d u , m 22 /d v and m 33 /d r for all given USVs are in specific range. Inserting maximum values of these ratios in the simulation model and tuning their values till the surge and yaw speeds for the predefined thrusting force and moments become stable, is continued. The predefined thrusting force is achieved from experimental results and it is the minimum thrust force that could move the boat in the steady state (for this case is 0.2 of the maximum thrust). Finally, these ratios for Morvarid are defined as m 11 /d u =10, m 22 /d v =5 and m 33 /d r =2.

EKF estimator
To best estimate the statues of the vehicle, application of an estimator filter is common. In estimation theory, the extended Kalman filter (EKF) is the nonlinear version of the Kalman filter which linearizes an estimate of the current mean and covariance. As it is the case in this study, the state transition and observation models do not need to be linear functions of the state, but may instead be differentiable functions. The EKF can be formulated as the flowchart as shown in Fig. 2. Since the three-degree-of-freedom (DOF) motion composed of surge, sway, and yaw was considered for the filter dynamics, the USV's motion was estimated using the position and attitude measurements provided by the GPS, compass and IMU.

uν
Here, x k = [x, y, ψ, u, ν, r] T is the state vector in the k-th step, z k =[x, y, ψ, , , r] T is the measurement vector that the first and second parameters are measured by the GPS, the third parameter is measured by compass and the last three are measured by IMU. The state estimation function, f(x k-1 , u k ) is a function of the state parameters and control signal, u k , and is given according to Eq. (5). T is the time step, and H is the jacobian of h(x k ) that here would be a function of T. H=diagonal ([1 1 1 1/T 1/T 1]), also F k-1 is the jacobian of f(x k-1 , u k ). The jacobian matrix, J for an arbitrary function, f is a m×n matrix that is given as Eq. (8).
w k and v k in Fig. (2) are white noise for process and measurement respectively which are the characterization of the system modeling errors and sensors' uncertainty. Furthermore, the following conditions are simultaneously satisfied (Rodríguez and Gómez, 2009) where is the Kronecker delta function, Q k is the covariance matrix of the system noise, and R k is the covariance matrix of the observation noise that all of these are positive parameters. The adjustment of Q k and R k is critical in the development of the Kalman filter. Although estimating of R k is not very sophisticated, about Q k there are some methods for these parameters definition, e.g. Monte Carlo (Gomez-Gil et al., 2013). However, in this research R k is estimated as R=diagonal ([ ]).

Control logic
Between path following algorithms, the potential field method is one of the most well-known algorithms that operates according to the magnetic potential field (i.e. attractive or repulsive force is inversely proportional to the distance from the destination point). This algorithm has been widely and successfully employed for control and motion planning of mobile robots for decades. The potential field based algorithms are fast, reliable and easy to tune once implemented on real platforms (Pêtrès et al., 2012). In the artificial potential field method, the movements of the robot (represented as a particle) are governed by a field, which is usually composed of two components: an attractive potential drawing the robot towards the destination and a repulsive potential pushing the robot away from obstacles.
Approximately in all developed algorithms the destination point is a single point that attracts the vehicle. Therefore, the main drawback with potential field techniques is their susceptibility to local minima (Liu and Bucknall, 2018). However, here to dissolve this problem, an algorithm is developed based on the combination of potential field and search ball algorithms (Más et al., 2010). This concept is illustrated in Fig. 3 as navigation, guidance and control (NGC) algorithm.
After meshing the desired surface area that some trajectories are created with predefined distances, the algorithm, compares ASV's longitude with each meshed line longitude. So the algorithm first finds the nearest path line in meshed area, and then searches on that line to complete the search ball. For each point on the navigation course a search is conducted to find two nearest destination points (goals) on the reference path. Once determined, the distance of these points from the USV represents the greatness of the attractive force. The process continues for all of the points in the examined trajectory. The search is spatially bounded by the user-defined search ball, that here it is selected as 2L (L is boat length) (Fossen, 1994;Naeem et al., 2008). Actually, a Line-Of-Sight (LOS) maneuvering system was used to control USV position and transition between waypoints in a trajectory as in Fossen (2011). The LOS guidance is classified as a three-point guidance scheme since it involves a typically stationary reference point in addition to the interceptor and the target. The LOS denotation stems from the fact that the interceptor is supposed to achieve an intercept by constraining its motion along the LOS vector between the reference point and the target.
The radius of the search ball, B r , allows a certain level of flexibility on the user side; however, a well-adjusted magnitude makes the algorithm more computationally efficient. The search ball algorithm can be mathematically formulated as follows. Assuming points P(x j , y j ) to be the position of USV estimated by EKF, and T={1, …, n} as a set of indices for the reference path points inside the search ball. If E and N represent the east and north coordinates respectively, for each j, a new set B j ={iT|d ij <B r } can be defined, where distances d ij are calculated with Eq. (10).
The members in the set of B j are sorted from min to max, to obtain the minimum distance and its number. Actually the algorithm determines two nearest destination points to USV. It is necessary to mention that, as shown in Fig. 4, searching is done only inside a semicircle, i.e. the algorithm seeks only ahead of the boat. Also the deflection angle of USV from each point is calculated using Eq. (11).
To attract the USV toward the destinations, a linear potential, pot, is built for every destination point as follows (Pêtrès et al., 2012): where k 2 is the desired attractive gradient and d ij is the Euclidean distance between P(x j , y j ) and i-th destination. Fi-nally, the real destination point and the required orientation of USV that is shown by red vector in Fig. 4, is calculated from the result of calculated vectors using Eq. (13).
To set the motors thrusting force, the algorithm, first defines if the boat is in the right-hand or left-hand of the pass by comparing the latitude of the boat and desired path. Then it sets the desired thrusting force according to Eq. (14). Actually thrusting force in port and starboard sides are functions from three parameters; (1) offset from desired path (z 1 ), (2) heading error (δ) and (3) orientation to north (ψ).
In left : . (14) According to Fig. 4, z 1 is the minimum distance between ASV and path that is calculated using the triangulation relations. Actually increasing distance between the boat and desired path, as well as increasing δ causes more differences between two motors (more turning torque). Also according to this equation, the boat forced in a way that differences of heading and path angle are decreased. prim force is a variable, defining the thrusting force when the USV is on the noncurved path without any bias (Eq. (15)).
where k 1 is a constant number for example, 30% of motors thrust. It is assumed that the thrust of motors, T=T max × rpm/rpm max (Bertaska et al., 2015). Variables y near_goal1 and y near_goal2 are components of destination positions in y direction for two nearest destinations and difference of these two causes to reduce the speed at curved paths (e.g. at head of the path). Also drift is the cause to reduce speed.

Numerical simulation
Simulation is the process of solving the model and is performed on a computer. If the starting point is a block diagram-based model description, then in the initialization section, the equations for each of the blocks must be defined. Before the first prototype is completed, simulations simplify evaluating engineering trade-offs between different conceptual designs. So, the simulation procedure can be used as a "what if" scenario, before experimental evaluation of designed algorithm. Simulation is the process of numerical methods for solving a block diagram model on a computer. The display section of a simulation is used to present and post the output process. The block diagram of designed al-gorithm is illustrated in Fig. 5. The core element of this model is EKF block, which equation of this estimator is programmed in it. In this block diagram, position from GNSS and states from IMU are fed to algorithm and the estimated parameters are compared with inputs. Actually, these two blocks simulate the GNSS and IMU sensors on the boat.
Details of 'IMU' and 'Pos' sub-blocks are shown in Fig. 6. Using an integrator block in Basic Feedback System (BFS) form could smooth the randomized inputs. The 'gain' block values are tuned such a way to obtain variance of each sensor approximately similar to installed ones. After running the simulation, results are given in the next section.

Experimental validation of control algorithm
To evaluate the designed algorithm's accuracy, precision and robustness, experimental tests were carried out in 'Amir-Abad' port (Caspian Sea). These tests are performed by Morvarid ASV in fully autonomous mode. By using the developed GUI and clicking four points in the corners of a trapezoid on the map in bureau PC, four longitude-latitude pairs are sent to ASV's IPC. The points are selected in such a way that at least two lines (from West to East and from  East to West) are meshed with two right and left end-turning paths. Distance between lines is set to 30 m and points on the lines are meshed by 5 m. Approximately in constant environmental situations (wind, wave), tests are repeated at least three times. Fig. 7 illustrates the desired surface area with nine meshed lines, although for evaluation, data from three consequence lines would be adequate.
For assessment of designed algorithm experimentally, some parameters are extracted. RMSE (Eq. (16)), SD (standard deviation, Eq. (17)), Max and Min offset and Range (R=Max-Min) are some derived parameters. Offset of the vehicle is its lateral displacement with respect to a hypothetical line that is parallel to the center plane of the vehicle and crosses the target point. During tests, GNSS data and out-put of EKF are stored in an Excel file, while in simulation, these data are exported to workspace using 'simout' block. It is considered that each desired line is as the base line, and error points on the top of the desired path is taken positive, while below the path it is regarded as minus.
where y i is the boat offset error in i-th point, is the predicted value of offset error (zero in this case) and is the average of n offset errors.

Results and discussions
This section first explains results of simulation of the developed algorithm versus GNSS and IMU raw data. Then similarly, experimental test results are compared for GNSS raw data and travelled path of USV due to the designed algorithm. Fig. 8 illustrates longitude (x pos ) and latitude (y pos ) positions for raw data simulated by a random data creator in Simulink (blue curves) and output of designed algorithm (red curves). In simulation, the straight path is designed from west to east as long as 2000 m and the simulation is running for 20 s. According to the embedded GNSS, variance of random input numbers is set to 3 m. According to this figure, the developed algorithm improved the position precision, what is more tangible from latitude position (bottom curve). This improvement is obvious for longitude curve as well, but small changes are impalpable in 2000 m. However, to compare accurately, results will be given quantitatively in the last.
For a good judgment about position accuracy, the latitude would be sketched versus longitude in a graph. That is shown in Fig. 9. In this figure the input raw data of GPS are shown by red and the output of designed algorithm (EKF output) are shown by blue color. According to this figure,   variance of the algorithm output would decrease substantially. Quantitative values will be given in the last. Fig. 10 illustrates the yaw angle (compass) and yaw angle rate that are provided by IMU. With the north as zero, path angle from west to east would be 90° (1.57 rad). For simulation the designed algorithm, a set of random num-bers by mean of 1.57 rad and variance of 0.1 rad is provided. Time derivative of yaw angle is fed as yaw angle rate to algorithm. These values are coincided by the IMU characteristics in real conditions. Fig. 10 shows that the designed algorithm output (red curve) could follow the input values (blue curve) accurately.
As mentioned earlier, after simulation, some experimental tests are carried out to evaluate the designed algorithm under real environmental conditions. For each experimental test, three lines are defined (in 70, 40, 10 m latitude in local coordinate) by clicking four points on the desktop map. As shown in Fig 11, the path was approximately 100 m from west to east with 30 m between lines. In this figure, latitude and longitude coordinates are converted to meter scale and given data are for GNSS raw data. The ASV station is located in the coordinate of (0, 40) and it is transferred to (65, 80) position by remote controller. Then it is switched to autonomous mode, while the first line is located on 70 m latitude. As shown in Fig. 11, the ASV travelled near the 60 m latitude from west to east. After some evaluation it was found that this is due to thruster's angle and wave direction. When thrusters have some misadjusted yaw angle, the USV travel inclined. And this inclination is compensated by distance from path, since the final equation (Eq. (14)), combination of USV offset from desired path and its angle have direct effects. However in the path from east to west, this error source decreases considerably due to wave direction (that was north-east to south-west), and ASV travels near the "40 m latitude". So evaluation of two parameters: SD and RMSE would give two different results. Fig. 12 illustrates the travelled path by the USV from GNSS data that is shown in Fig. 11. This path is the output of designed algorithm, so considerable improvements would occur in comparison with Fig. 11. To compare these two, on image analysis application is developed in C# and offset errors are derived from travelled paths. Unbalanced line distance is obvious also in this picture that was mainly due to wave direction and thrusters inclination. The developed GUI, for data communication and bathymetry drawing is shown in Fig. 12 as well. Table 2 shows some quantitative parameters for comparison. SD illustrates the drift about the average of data while RMSE is the drift from predicted value. Here the target line  is regarded as the base line, so its position is considered to be in the coordinate of each line. According to Table 2, in each case RMSE is larger than SD. About experimental tests, each datum is the average of three path lines. As mentioned earlier two lines travelled from west to east while one of them travelled from east to west. The RMSE value from west to east direction was approximately threefold of the direction from east to west. This result was obvious in all experiments. According to Table 2 in simulation and experimental tests, SD and RMSE values for GPS raw data are larger than the output of the developed algorithm. This means that the algorithm improved the positioning results. This table shows that the developed algorithm for Morvarid has good precision rather than accuracy. Accuracy is the measure of the difference between the measured value and actual value. While precision is the ability of an algorithm or system to reproduce a certain set of readings within a given accuracy. Precision is dependent on the reliability of the instrument. According toTable 2, the designed algorithm has good repeatability and precision. Repeatability is one of the important capabilities for each algorithm. Simulation has been given better results than experiment. Because it is not defected by improbable wave and thrusters inclination. However what is concluded from Table 2 is that in all cases for SD and RMSE, the algorithm output is approximately half of the raw data (i.e. algorithm increased the navigation confidence twice). About the Max and Min, in some cases the algorithm output are larger than raw data, probably it would be processing noise because usually, image processing is combined with some noises. However this result is comparable to Mousazadeh et al. (2017) that in another experimental test with Morvarid USV they concluded that SD and RMSE were 0.3 and 2.5 m, respectively. While in the mentioned test the thrusters were somewhat adjusted correctly (Mousazadeh et al., 2017). Fig. 13 illustrates the parameter SD for GNSS raw data and the developed algorithm output. According to this figure, in experiment, the maximum SD reaches 0.6 m. However it is anticipated that in long paths, this value would be improved and it could be reduced to smaller than 0.4 m.

Conclusion
Designing control systems for ASVs are subjects of great interest recently. It has proven that multiple motion sensors play a vital role in autonomous navigation. The Morvarid an autonomous ASV was equipped with multiple sensors to plot the hydrography map in shallow waters. Dynamic model of the boat was derived and some main parameters of rigid body model were defined based on experimental evaluations. For the best estimation of position and states, the EKF estimator was embedded in navigation algorithm while for navigation in path following algorithm, combination of search ball and potential field methods was applied. For the best optimization, the designed algorithm is simulated using Simulink. The required data were provided using random data creator blocks by defined mean and variance. The algorithm was evaluated in different circumstances with different values of environmental disturbances (wind, wave and current). To evaluate the designed algorithm robustness, some experimental tests were carried out in predefined paths. Test results illustrated the algorithm and improved the path by reducing SD and RMSE. However some more improvements would be attainable by adjusting thruster's inclination and installing two thrusters in such a way to get a right hand spin for one prop and a left hand spin for the other prop.