Comparison of Dynamic and Kinematic ModelDriven Extended Kalman Filters (EKF) for the

Localization of Autonomous Underwater Vehicles

Sharan Balasubramanian1∗, Ayush Rajput1∗ †, Rodra W. Hascaryo2,Chirag Rastogi1, and William R. Norris1

1 Department of Industrial and Enterprise Systems Engineering2 Department of Aerospace Engineering

University of Illinois at Urbana-ChampaignUrbana, Illinois 61801

Email: [sharanb2, arajput3, hascary2, chiragr2, wrnorris]@illinois.edu

Autonomous Underwater Vehicles (AUVs) and RemotelyOperated Vehicles (ROVs) are used for a wide varietyof missions related to exploration and scientific research.Successful navigation by these systems requires a goodlocalization system. Kalman filter based localizationtechniques have been prevalent since the early 1960s andextensive research has been carried out using them, bothin development and in design. It has been found that theuse of a dynamic model (instead of a kinematic model) inthe Kalman filter can lead to more accurate predictions,as the dynamic model takes the forces acting on the AUVinto account. Presented in this paper is a motion-predictiveextended Kalman filter (EKF) for AUVs using a simplifieddynamic model. The dynamic model is derived first andthen it was simplified for a RexROV, a type of submarinevehicle used in simple underwater exploration, inspectionof subsea structures, pipelines and shipwrecks. The filterwas implemented with a simulated vehicle in an open-sourcemarine vehicle simulator called UUV Simulator and theresults were compared with the ground truth. The resultsshow good prediction accuracy for the dynamic filter, thoughimprovements are needed before the EKF can be used onreal time. Some perspective and discussion on practicalimplementation is presented to show the next steps neededfor this concept.

1 IntroductionLocalization is one of the key pillars of autonomous

navigation. A major component of localization is the abilityof the system to estimate the state of the vehicle in orderto help locate it. Bayesian estimators are most commonlyused for this, the most common strategy using Bayesianestimators being the Kalman filter (KF). The KF assumes

∗These authors contributed equally to this work†Corresponding author

the states to be Gaussian random variables and uses leastsquares minimization to estimate final state. For under-waterlocalization, the Extended Kalman filter (EKF) has beenwidely used [1–3]. An EKF linearizes a non-linear dynamicmodel to find a local approximation.

Sampling-based strategies are also popular choices forstate estimation. The Unscented Kalman filter (UKF) andparticle filter (PF) are two common choices. The UKFis an improvement over the EKF for some applications,using sigma points to estimate the state parameters. It usesa deterministic sampling strategy but assumes underlyingthe state to be a Gaussian random variable. Comparingthe performance of EKF with UKF, [4] observed UKF tobe better while [5] has observed EKF to be better, so theappropriate one to use depends on the inputs and application.When underlying state estimation is not a Gaussian randomvariable, particle filter (PF) is a good choice. The particlefilter represents a non-Gaussian state distribution usingweighted samples, and uses a random sampling strategy.PF has been successfully implemented to estimate states forunderwater vehicles in several studies, including [6–8]. Incomparison to a Kalman filter, localization is observed to bemore robust when PF is used under sensor noise, as observedby [9].

In state estimation, the plant (hardware) is mostcommonly represented by a purely kinematic model (i.e.,model describing vehicle motion without considering forceand moments [5, 10, 11]). However kinematic models failto capture highly non-linear behaviour often observed inunderwater systems. Approaches based on dynamic modelshave been implemented successfully [4, 12, 13, 13] to getmore accurate state estimates. The study described inRef. [13] implemented a 3 DoF dynamic model along withINS and DVL sensor data to estimate state of vehicle.

To explore this further, this article derives a rigorousdynamic model of an underwater vehicle (UWV) to be used

arX

iv:2

105.

1230

9v1

[cs

.RO

] 2

6 M

ay 2

021

with an extended Kalman filter. This model considers severalforces acting on the UWV and is simplified to be ableto work on a RexROV vehicle within the UUVSim [14]simulation environment. This simplification was done toreduce the number of terms in the dynamic model, sincesome of the vehicle degrees-of-freedom (DoF) are not used.Removing these terms reduces the computational cost whileonly eliminating terms which had little to no effect on thevehicle behavior. The RexROV vehicle, as shown in Fig. 2,has 4 lateral thrusters and 4 longitudinal thrusters to governits motion. The RexROV model in UUVSim consists of thedynamic model with GPS, Doppler Velocity Logs (DVL) andIMU sensors. The block diagram in Fig. 1 summarizes theproposed digital experiments. In this simulation, a vehiclewith 6 DoF is set for waypoint navigation. It is localizedusing a 4 DoF dynamic model with an EKV. A pure pursuitcontroller is used to generate control commands.

Fig. 1: Block Diagram

The article is divided into several sections, beginningwith a detailed description of the methodology (Section2), the definition of the coordinate system and conventions(Section 3), the derivation (Section 4) and simplification(Section 5) of the dynamic model, a discussion of the IMUacceleration (Section 6), the EKF algorithm (Section 7), adescription of the controller (Section 8), some simulatedexperiments to test the performance (Section 9), a discussionof practical implementation, leading to physical real-worldexperiments (Section 10), and finally some conclusions andfuture work directions (Section 11).

2 MethodologyBeginning with the seminal work by Fossen [15],

kinematic and dynamic models, which have been observedto be the de facto standard used in marine vehicle modeling,are laid out in Refs. [14, 16–18] and then simplified for aparticular vehicle before being applied to an EKF and runin a computer simulation. The work cited above use a 6DoF dynamic model for the application, although Berg [17]noted that the ROV effectively has 4 DoF [17]. This researchaims to investigate the feasibility of a 4 DoF model formotion prediction by incorporating it into an EKF. Two DoFwere eliminated due to the inherent stability of the vehiclewhich helped in reducing the 6x6 state matrix to a pseudo

4x4 state matrix thereby reducing computational cost.The 4DOF model is proposed in Ref. [19] and was successfullyused by Ref. [20] in control development. Since motionpredictions are being augmented with sensor readings, it isexpected from this approach to work effectively with theproblem proposed in this paper. A Robotic Operating System(ROS) Gazebo-based open-source marine vehicle simulatorwas used in this research [14]. The simulator incorporatesthe dynamic model by Fossen [14] with a vehicle modelbased on parameters derived from the works by Berg [17].

Fig. 2: BODY axis convention

3 Coordinate Systems and Convention

Before derivation of the dynamic model, it is importantto define and describe the coordinate system that should beused for it. The simulation consists of two bodies - theworld or environment and the underwater vehicle, each withits own coordinate system. The world uses a fixed-frameNorth-East-Down (NED) and the vehicle uses a fixed-frameBODY [15–17] system. The BODY frame is based onthe Society of Naval Architects and Marine Engineers(SNAME) 1950 convention [15]; the BODY axis conventionfor RexROV is as shown in Fig. 2. The NED frame is usedfor the vehicle’s position, pose and the environment relateddynamic effects such as buoyancy and weight [16], whilethe BODY frame is used to describe the vehicle’s linear andangular velocities [17]. Table 1 shows the naming conventionfollowed.Clear transformation between the two coordinate systemsis crucial to the dynamic model. Considering (5), thetransformation from BODY to NED coordinates is:

ηnb/n = Jn

b (Θ)vbb/n (1)

where subscript b/n means frame b in motion with respect toframe n. The Superscript denotes the frame in which matrix

Table 1: 1950 SNAME convention for marine vehicles

Term Pose Velocity Acceleration Force

Surge x u u X

Sway y v v Y

Heave z w w Z

Roll φ p p K

Pitch θ q q M

Yaw ψ r r N

is defined. Matrix J is a 6×6 transformation matrix given:

Jnb (Θ) =

[Rn

b(Θ) 03x303x3 T n

b (Θ)

](2)

where Rnb(Θ) and T n

b (Θ) are the following 3× 3 rotationmatrices for linear and angular velocities with a conditioncosA 6= 0 thus A 6= 90◦or 270◦ [17].

Rnb(Θ) =

cψcθ sφsθcψ− sψcφ sψsφ+ cψcφsθ

sψcθ cψcφ+ sφsθsψ sθsψcφ− sφcψ

−sθ sφcθ cθcψ

(3)

T nb (Θ) =

1 sφtθ c(φ)tθ0 cφ −sφ

0 sφcθ cφcθ

(4)

For simplicity, cosine, sine, and tangent of angle A has beenshown as cA, sA, and tA respectively above.

4 Dynamic Model derivationThe forces acting on an underwater vehicle can be

divided into five categories: Kinetic forces, hydrodynamics,hydrostatics, actuator forces and disturbances. The 6 DoFdynamics of an underwater vehicle can be represented withthe following two vector equations [21]:

η = J(η)v (5)

Mv+C(v)v+D(v)v+g(η)+g0 = τ+ τwind + τwave (6)

The terms used in the equation are as listed below:

The dynamic model consists of the terms and coefficientslisted in the table 2.

η - Vehicle position and pose

v - Vehicle velocity (in BODY frame)

J - Transformation matrix

M - inertial matrix

C(v) - Coriolis forces

D(v) - Damping matrix

g(η) - Buoyancy force

τ - Actuator force

τwind - Effect of wind

τwave - Effect of water waves

Table 2: List of Parameter and Coefficient Symbols

Coefficient Explanation

m Mass of the vehicle

W Weight of the vehicle

B Buoyancy force

g Acceleration due to gravity

ρ Density of water

I Moment of inertia

Ix, Iy, IzMoment arm of thrusters fromcenter of gravity

Xu,Yv,Zw Mass increase due to translation

Kp,Mq,Nr Inertia increase due to rotation

Xu,Yv,ZwLinear damping coefficients fortranslation

Xu|u|,Yv|v|,Zw|w|Quadratic damping coefficients fortranslation

Kp,Mq,NrLinear damping coefficients forrotation

Kp|p|,Mq|q|,Nr|r|Quadratic damping coefficients forrotation

The forces acting on an underwater vehicle are modeledseparately at first and then substituted in equation (6) toderive the dynamic model. The vehicle is considered as arigid body since it’s not expected to undergo any deformationat any operating depth. Thus the rigid body kinetics (τRB)can be expressed as a sum of the inertial (MRBv) and Corioliseffects (CRB(v)v) as shown in equation (7).

τRB = MRBv+CRB(v)v (7)

For a 6 DoF model, where the vehicle center of origin andcenter of gravity coincide, MRB can be represented as in (8).

MRB =

m 0 0 0 0 00 m 0 0 0 00 0 m 0 0 00 0 0 Ixx 0 00 0 0 0 Iyy 00 0 0 0 0 Izz

(8)

The Coriolis effect(CRB(v)) is due to the rotation of theenvironment in which the AUV is operating and hence it canbe represented as in (9).

CRB(v) =[

mS(v2) −mS(v2)S(rbg)

mS(rbg)S(v2) −S(Ibv2)

](9)

where rbg is the position of the Center of Gravity (CG)

matrix. The vector v2 (velocity vector) is the velocity vectorand Ib is the moment of inertia matrix and S representsthe skew-symmetric matrix. Fossen [15, 21] showed thatmultiplying the rigid body Coriolis matrix above with thevelocity vector yield the Coriolis effect as shown in (10).

CRB(v)v =

m(qw− rv)m(ru− pw)m(pv−qu)qr(Izz− Iyy)rp(Ixx− Izz)qp(Iyy− Ixx)

(10)

The hydrodynamic force (τhydrodynamics) includes thehydrodynamic drag (D(v)v), the Coriolis-centripetal effects(CA(v)v) and the inertial effects (MAv) as shown in (11) [15].

τhydrodynamics =−D(v)v−CA(v)v−MAv (11)

The individual components of τττhydrodynamics are simplifiedfrom Fossen equations using the low speed assumption [15]:

MA =−

Xu 0 0 0 0 00 Yv 0 0 0 00 0 Zu 0 0 00 0 0 Kp 0 00 0 0 0 Mq 00 0 0 0 0 Nr

(12)

CA(v) =

0 0 0 0 −Zww Yvv0 0 0 Zww 0 −Xuu0 0 0 −Yvv Xuu 00 −Zww Yvv 0 −Nrr Mqq

Zww 0 −Xuu Nrr 0 −Kp p−Yvv Xuu 0 −Mqq Kp p 0

(13)

Multiplying (13) with the velocity vector:

CA(v)v =

Yvvr−ZwwqZwwp−XuurXuuq−Yvvp

(Yv−Zw)vw+(Mq−Nr)qr(Zw−Xu)uw+(Nr−Kp)pr(Xu−Yv)uv+(Kp−Mq)pq

(14)

The drag force consists of two components namely linear andquadratic drag forces as shown in equation (15) [15].

D(v) = Dlinear +Dquadratic (15)

where Dlinear is a diagonal matrix with all the linear dragcomponents and Dquadratic is a diagonal matrix with allthe quadratic components. Thus summing the linear andquadratic drag terms and multiplying them with the velocitycomponents D(v)v is found as shown in (16).

D(v) =−

(Xu +Xu|u||u|)u(Yv +Yv|v||v|)v

(Zw +Zw|w||w|)w(Kp +Kp|p||p|)p(Mq +Mq|q||q|)q(Nr +Nr|r||r|)r

(16)

The hydrostatic forces consist of two forces: buoyancyand gravitational force(weight). It is assumed that the Centerof Buoyancy (CB) and Center of Gravity (CG) are on thez-axis and zB is the distance of the CB to the CG forsimplicity as the assumption holds for most of the cases.Buoyancy is the upward force caused when immersing abody into a liquid and is a factor of the volume of the liquiddisplaced by the body. In the case of a fully submergedvehicle (AUV), the volume of liquid displaced is equal tothe total volume of the vehicle. Hence buoyancy (B) is asshown in (17). The weight (W) of the vehicle is equal to themass times acceleration due to gravity as shown in (18).

B = ρgV (17)

W = mg (18)

In the case of an AUV, ρ is the density of water and V is thevolume of the vehicle. The sum of the two forces gives thehydrostatic force which in vector form, is written as (19).

g(η) =

(W −B)sinθ

−(W −B)cosθsinφ

−(W −B)cosθcosφ

−zBBcosθsinφ

−zBBsinθ

0

(19)

The final component of the dynamic model involvescalculating the actuator forces. Deep underwater, the effectof wind and ocean currents are minimal and so τwind andτwave are equivalent to zero. The thruster forces are vehiclespecific since different vehicles have different specificationsand orientations for their thrusters. In general, the forceand the torque contribution of each thruster in the 6 DoF isrepresented as shown in equation (20).

Ti =

TxTyTzTφ

Tθ

Tψ

(20)

There are both empirical and theoretical methodsto measure these thruster forces [16–18]. Finallysubstituting the above derived formulas into equation (6) anddecomposing them into respective components would yieldthe dynamic model as shown in equations (21) to (26).

u =

(1

m−Xu

)((Xu +Xu|u||u|)u+(B−W )sinθ

∩+m(rv−qw)−Yvrv+Zwqw+Tx) (21)

v =(

1m−Yv

)((Yv +Yv|v||v|)v− (B−W )sinφcosθ

∩+m(pw− ru)+Xuru−Zw pw+Ty) (22)

w =

(1

m−Z w

)((Zw +Zw|w||w|)w− (B−W )cosθcosφ

∩+m(qu− pv)−Xuqu+Yv pv+Tz)

(23)

p =

(1

Ix−Kp

)((Kp +Kp|p||p|)p− (Mq−Nr)qr

∩+(Iyy− Izz)qr+(Zw−Yv)vw

∩+BzB cosθsinφ+Tθ

) (24)

q =

(1

Iy−Mq

)((Mq +Mq|q||q|)q− (Nr−Kp)pr

∩+(Izz− Ixx)pr+(Xu−Zw)uw

∩+BzB sinθ+Tφ

) (25)

r =(

1Iz−Nr

)((Nr +Nr|r||r|)r− (Kp−Mq)pq

∩+(Ixx− Iyy)pq+(Yv−Xu)uv+Tψ

) (26)

5 Reduced Dynamic Model for RexROVThe RexROV submarine is the AUV used in this

research and it relies on its thrusters to orient and move itself.The position and orientation of the thrusters are as shown inFig. 3.

Fig. 3: RexROV’s thrusters orientation from UUVSim[22]

Through the inherent configuration of the RexROV, thesubmarine is stable with no pitch and roll due to its structure.From an operations perspective, there is also no need forthe RexROV or similar vehicles to possess pitch and rollcapabilities. Thus 2 DoF can be eliminated and the 6 DoFmodel can be reduced to a pseudo 4 DoF model. This meansthat φ and θ are always 0. Likewise, their correspondingvelocities, p and q, and accelerations, p and q, componentsare always 0. Canceling those terms for the other four linearand angular accelerations, the dynamic model 1 becomes:

u =

(1

m−Xu

)((Xu +Xu|u||u|)u

∩+mrv−Yvrv+Tx) (27)

1It means equation continues ∩∩∩

v =(

1m−Yv

)((Yv +Yv|v||v|)v

∩−mru+Xuru+Ty) (28)

w =

(1

m−Z w

)((Zw +Zw|w||w|)w

∩− (B−W )+Tz) (29)

r =(

1Iz−Nr

)((Nr +Nr|r||r|)r

∩+(Yv−Xu)uv+Tψ

) (30)

A Creo based CAD model was developed based on theinformation from the UUVSim website [14]. The tables 3and 4 show the Thruster ID and their respective location fromthe CG with their orientations.

Table 3: RexROV Thruster Positions

Thruster Location w.r.t. CG(m)

ID lx ly lz

0 -0.890895 0.334385 -0.528822

1 -0.890895 -0.334385 -0.528822

2 0.890895 0.334385 -0.528822

3 0.890895 -0.334385 -0.528822

4 -0.412125 0.505415 -0.129

5 -0.412125 -0.505415 -0.129

6 0.412125 0.505415 -0.129

7 0.412125 -0.505415 -0.129

Based on Fig. 3 and tables 3 and 4, the individualthruster forces Tx, Ty, Tz and Tψ can be derived by resolvingthem based on their respective orientations as shown in (31)to (34).

Tx =3

∑i=0

Ti cosθi cosψi +7

∑j=4

Tj cosψ j (31)

Ty =3

∑i=0

Ti cosθi cosψi +7

∑j=4

Tj sinψ j (32)

Table 4: RexROV Thruster Orientation

Thruster Orientation(deg)

ID φ θ ψ

0 0 74.53 -53.21

1 0 74.53 53.21

2 0 105.47 53.21

3 0 105.47 -53.21

4 0 0 45

5 0 0 45

6 0 0 135

7 0 0 -135

Tz =3

∑i=1

Ti sinψi (33)

Tψ =7

∑i=0

Ti(lxi sinψi− lyi cosψi) (34)

The angle terms θA and ψA used in the above fourequation are the orientation of thruster A with respectto the xy-plane and z-axis respectively. They are takenfrom Tables 3 and 4. Tables 5 and 6 provide theRexROV parameters and coefficients that are used in theequations (27) to (30). These coefficients are specificto RexROV as shown in 2 and are derived from Berg’smodel [17] and UUVSim [14].

Table 5: RexROV Physical Parameters

Parameter Value

Length(x-axis) 2.6 m

Width(y-axis) 1.5 m

Height(z-axis) 1.6 m

Mass(m) 1.863 kg

Volume(V) 1.838 m3

B 18393.9972 N

g 9.81 m/s

ρ 1000 kg/m3

Izz 691.23 kgm2

Table 6: RexROV Coefficients

Coefficient Value

Xu 779.79

Yv 1222

Zw 3659.9

Kp 534.9

Mq 842.69

Nr 224.32

Xu -74.82

Yv -69.48

Zw -782.4

Kp -268.8

Mq -309.77

Nr -105

Xu|u| -748.22

Yv|v| -992.53

Zw|w| -1821.01

Kp|p| -672

Mq|q| -774.44

Nr|r| -523.27

6 IMU accelerationAs seen in equations (27) to (30), the dynamic model for

RexROV involves calculating the acceleration values. Theaccuracy of the modeling can be verified by comparing itto the acceleration values generated by the IMU sensor onUUVSim [14]. The IMU sensor takes into account driftingbias by following a random walk with zero-mean whiteGaussian noise as the rate. Its model includes zero-meanGaussian noise as well.

The following Fig. 8 shows the acceleration valuesgenerated by the model and the acceleration values given bythe IMU sensor in the simulation in the x-axis for behaviorevaluation 3 test course [23]. It can be seen that our modelperforms similarly to the IMU sensor. It is also importantto note that the IMU sensor modeled in the simulation hasa very high level of accuracy with a variance of 0.004 m/s2.However, in practice, the IMU sensors have a higher variancethan the one in simulation. In general, IMUs are prone todrift, which results in error accumulation over time. Thisresults in divergence with actual values in practice whilecalculating position.

As seen in Fig. 4, the dynamic model provides a changein the value where there is a change in the trajectory. Thepositive acceleration values calculated by the model areconsistent with the IMU sensor values. Further research will

Fig. 4: Acceleration values predicted by model and IMU in x-axis

be conducted to understand the inconsistency on the negativeacceleration values calculated by the model. The dynamicmodel has an edge over the IMU sensor in the fact that ituses velocity and thrust forces for calculation. The thrustforces can be reliably received from the generated look-uptable based on the thruster RPM. The velocity, being the firstderivative, gives a highly reliable estimate for the positioncalculation as opposed to acceleration, which is the secondderivative.

7 Extended Kalman Filter AlgorithmThe Kalman Filter [24] is an iterative mathematical tool

used to estimate the state of a system based on a system ofequations and measurements. The Kalman filter has threesteps, namely, the Initialization, Prediction and Update. Fora Kalman filter to work, there are two major conditions.The Kalman filter works with the Gaussian distribution withlinear prediction and update equations. These two conditionsare interrelated. A linear system fed to a Gaussian producesa Gaussian output, while a nonlinear system produces anon-Gaussian output. Hence Kalman filters work well withlinear systems of equations. However, most of the real-timesystems that are encountered are nonlinear.

One way to apply Kalman filters to these nonlinearsystems is to approximate them as linear systems byusing Taylor series [25] around the mean of the nonlinearGaussian. This version of the Kalman filter is referred to asthe Extended Kalman Filter (EKF). The EKF algorithm is asshown in Algorithm 1. The terms used in the algorithm 1 arelisted below.

Applying the above mentioned Extended Kalman Filter

x - State Matrix

x - State Prediction

f : X×U → R2 - Function to predict the states

F - State Transition Matrix

P - Predicted Covariance

P - Process Noise Covariance

Q - Measurement Noise Covariance

z - Measurement from Sensors

h(x) - Prediction to Measurement coordinate

y - Difference between Predictionand Measurement

H - Jacobian Matrix

R - Measurement Noise Covariance

K - Kalman Gain

I - Identity Matrix

k - Represents the current time step

k-1 - Represents the previous time step

Result: State Matrix : xInitialization: Initialize the state of the filter(Q,P)and the belief in the state(K)

while New Datapoint doPredict:

xk = f (x,u)Fk =

∂ f (x,u)∂x |x,u

Pk=FkPk−1FTk +Q

Update:yk = zk−h(xk)

Hk =∂h(x)

∂x |xSk = HkPkHT

k +RKk = PkHT

k S−1k

xk = xk +KkykPk = (I−KkHk)Pk

endAlgorithm 1: Extended Kalman Filter

Algorithm 1 to the RexROV submarine, the state matrix for4 DoF shown in (35) can be found.

xxx ===

xyzψ

(35)

The states can be predicted using Newton’s equationsof motion[15] as shown in (36). Here, v is the velocityvector (37) and v is the acceleration vector (38).

f (x,u) = xk−1 + vk∆t +vk∆t2

2(36)

v =

uvwr

(37)

v =

uvwr

(38)

The state transition matrix F in the expanded form isshown in (39).

F =

∂ f (x)

∂x∂ f (x)

∂y∂ f (x)

∂z∂ f (x)

∂ψ

∂ f (y)∂x

∂ f (y)∂y

∂ f (y)∂z

∂ f (y)∂ψ

∂ f (z)∂x

∂ f (z)∂y

∂ f (z)∂z

∂ f (z)∂ψ

∂ f (ψ)∂x

∂ f (ψ)∂y

∂ f (ψ)∂z

∂ f (ψ)∂ψ

(39)

Using the previously discussed stability assumptions,the F-matrix reduces to (40).

F =

∂ f (x)

∂x 0 0 00 ∂ f (y)

∂y 0 0

0 0 ∂ f (z)∂z 0

0 0 0 ∂ f (ψ)∂ψ

(40)

The terms in the F-matrix are derived as follows,

∂ f (x)∂x

= 1+∂u∂x

∆t +∂u∂x

∆t2

2(41)

∂ f (x)∂x

= 1+∂u∂t

∂t∂x

∆t +∂u∂t

∂t∂x

∆t2

2(42)

For small changes in u with respect to time t, ∂u∂t can be

taken as ∆u∆t and (42) changes to,

∂ f (x)∂x

= 1+ u1u

∆t +∆u∆t

1u

∆t2

2(43)

∂ f (x)∂x

= 1+uu

∆t +∆u∆t

2u(44)

Similarly the other three terms in the F-matrix areobtained as shown in (45) to (47).

∂ f (y)∂y

= 1+vv

∆t +∆v∆t

2v(45)

∂ f (z)∂z

= 1+ww

∆t +∆w∆t

2w(46)

∂ f (ψ)∂ψ

= 1+rr

∆t +∆r∆t

2r(47)

In equations (44) to (47), the linear velocity values areobtained from the DVL sensor and the angular velocity isobtained from the IMU sensor. The acceleration values areobtained from the dynamic equations (27) to (30). Thisconcludes the predict step in the EKF Algorithm 1. Themeasurement matrix z is represented as shown in (48).

z =

xmymzmψm

(48)

Here, xm,ym comes from the GPS sensor, zm comesfrom the pressure sensor and ψm comes from the IMU. Alsothe prediction values are already in the same coordinates asthe measurement values and hence h(x) is the same as x.This results in the Jacobian matrix (H) becoming an Identitymatrix (I4) as shown in (49).

H =∂h(x)

∂x=

∂x∂x

= I4 (49)

The other update equations are reduced to the following:

Sk = HkPk +R (50)

Kk = PkS−1k (51)

xk = xk +Kkyk (52)

Pk = (I−Kk)Pk (53)

Thus, the EKF Algorithm for RexROV becomes asshown in Algorithm (2). The algorithm has been appliedto several test courses from [23] in the next section and theirresults are discussed. The source code for implementation ofthe EKF algorithm shown below is available in https://gitlab.engr.illinois.edu/auvsl/submarine

Result: State Matrix : xInitialization: Initialize the state of the filter(Q,P)

and the belief in the state(K)while New Datapoint do

Predict:xk = f (x,u)

Fk =∂ f (x,u)

∂x |x,uPk = FkPk−1FT

k +QUpdate:yk = zk− xk

Sk = HkPk +RKk = PkS−1

kxk = xk +KkykPk = (I−Kk)Pk

endAlgorithm 2: Extended Kalman Filter for RexROV

In the dynamic Kalman filter, the velocity values aretaken from the Doppler Velocity Logs (DVL) and thrustvalues are obtained from the pre-generated look-up table.Using these, the xk values were calculated using the dynamicequations mentioned previously. For the kinematic filter,velocity values from DVL and acceleration values from theIMU are used in Newton’s equations of motion to calculatexk. The update step remains the same for both the filters, withthe GPS providing x and y co-ordinates, the pressure sensorproviding the z co-ordinate and the compass providing theheading ψ for the jacobian matrix.

8 Pure Pursuit ControllerFor a given set of waypoints, a pure pursuit

algorithm approaches the target waypoint ignoring thedynamics/kinematics of the environment and the vehicle(except vehicle length). Once in the vicinity (user defined)of the target waypoint, the algorithm generates the steeringcommand for the next waypoint until it reaches the lastwaypoint [26]. For the purposes of this work, the steeringcommand is generated while the linear velocity is constant.

Considering RexROV as a point mass [27], Pure Pursuitaims to align the Center of Mass (in our case) along thereference trajectory through generating steering commands

Fig. 5: Pure Pursuit Model

using the following formulations.

K =2∗ sin(α)

Ld(54)

δ = tan−1(K ∗L) (55)

The vehicle length (L) and the look ahead distance (Ld)are taken as unity as well. α is the difference between vehiclepose and the slope of the line connecting the target point andthe last waypoint. xk in the EKF algorithm 2 gives the currentpose of the RexROV. Both slope and pose are convertedwithin a 0 to 2π range. Output δ is published to the cmdvelRostopic with a gain of 0.3.

9 Experiments and ResultsExperiments were conducted on open source simulation

software Gazebo 7. For underwater scenario and sensorssimulation, UUVSIM [14] is used in parallel with ROSKinetic.

The vehicle, RexROV, used is 2.6× 1.5× 1.6 m3 indimensions and 1863 kg in weight. Max speed of vehicleis 0.3 m/s and only steering is controlled using pure pursuitcontroller. Vehicle is running under a depth of 20 m from sealevel. Wind and current disturbance is considered zero forthe purpose of demonstration i.e. no external disturbance.

In this section, the performance of the kinematic and thedynamic model driven pure pursuit controllers are comparedon various test courses adopted from [23].For comparison,the ground truth localization is obtained by subscribing to

topic /rexrov/pose gt which is published by default whenvehicle is launched with required inertial navigation sensors.However, it is not mentioned [14] how these sensor readingsare processed to obtain ground truth value.

The figure shows the actual path followed by the vehicleas opposed to the output of the dynamic model and kinematicmodel. The results from the test courses are summarized asa table. Total is the mean of the Cartesian distance betweenthe reference path and the path taken by the RexROV whenusing the controllers. X-Kalman (along x axis) gives the rootmean squared error (RMSE) between the value predicted bythe Kalman filter and the actual path traced by the RexROV.Likewise, Y-Kalman (along y axis) gives MSE in the y-axis.

9.1 Behavior Evaluation 1

Fig. 6: Behavior Evaluation 1

The first test course involves moving along a straightline followed by a fixed turning radius and a sharp turn. Asseen in Fig. 6, the kinematic filter follows the reference pathmuch closer than the dynamic filter. After the simple turn,both the filters converge at the same rate.

Table 7: Results of Behavior Evaluation 1

Error Dynamic Filter Kinematic Filter

Total 0.1166m 0.09880m

X-Kalman 0.0825m 0.3432m

Y-Kalman 0.2665m 1.024m

From Table 7, the total error of the kinematic filter isless than the dynamic filter. However, this is attributed to thecontroller and not the filter itself as it can be inferred from theother two entries in the table. To make further assessment ofthe filters, they were tested on the behavior evaluation 2 testcourse.

9.2 Behavior Evaluation 2The second test course is a sinusoidal path which

uses curves of constant radius with both clockwise andcounter-clockwise directions. This test course helps indetermining if there are handling and navigation issues.

Fig. 7: Behavior Evaluation 2

As seen in Fig. 7,the dynamic filter based localizationestimates are much closer to the reference trajectory asopposed to kinematic approach based. Dynamic filter looksstable while kinematic filter looks diverging first and thenconverging again. This behaviour can be attributed to theacceleration values obtained from the IMU. In Fig. 4, IMUacceleration is leading or lagging through out with respect toDynamic acceleration, hence such fluctuations in kinematicfilter is observed.

Table 8: Results of Behavior Evaluation 2

Error Dynamic Filter Kinematic Filter

Total 0.1659m 0.2482m

X-Kalman 0.1213m 0.39145m

Y-Kalman 0.3682m 4.336m

From table 8, the mean error of the dynamic filter isless than the kinematic filter. The dynamic filter is able tohandle the simultaneous changes in the x-axis and y-axismuch better than the kinematic filter.

9.3 Behavior Evaluation 3The third test course is a U-shaped path with clockwise

and counter-clockwise curves and straightaways.

Fig. 8: Behavior Evaluation 3

Table 9: Results of Behavior Evaluation 3

Error Dynamic Filter Kinematic Filter

Total 0.0738m 0.1965m

X-Kalman 0.2462 0.4028

Y-Kalman 0.4003m 4.0014m

From Fig. 8, in behavior evaluation course 3, whichincludes all possible movements of the ROV, the dynamicfilter converges to the path more quickly than its kinematiccounter part. Again the slight divergence on the right handside is observed in kinematic filter. The reason is similar tothat of given while evaluating BE2, i.e., acceleration fromthe IMU is fluctuating at these points.

10 Practical ImplementationIn order to validate the results found during the

simulated experiments, physical experiments will be needed.While beyond the scope of the present paper, this sectionprovides some guidelines for accomplishing the physicalvalidation.

1. First, the work presented in this paper is dependent onthree major assumptions:

(a) That a valid dynamic model can be created whichis accurate enough to predict the behavior of thesystem.

(b) That a valid kinematic model can be created ofthe system which can also predict the behaviorof the system and that it generally had poorerperformance with the Kalman filters than adynamic model would be.

(c) That the reduced-order dynamic model with four 4DoF is rigorous enough to capture the behavor ofthe system appropriately.

2. The first and most important thing to do is to

experimentally validate both the dynamic and kinematicmodels of the system (which could be partiallydone by reviewing the literature in depth). If thedifferences between them are inconclusive for a widerange of scenarios, the one which provides the lowestcomputational cost should be used.

3. The assumption that a six DoF model and four DoFmodel are equivalent for this type of under-water vehicleneeds experimental validation.

4. Once the assumptions are validated (or at least theranges under which they are valid are identified),detailed sensitivity functions for each (and other partsof the system) can be calculated which will help identifyareas of risk in the system.

5. From here, the validated models, localization, andcontrol systems can be used as the basis for newvehicle designs approaches, both for plant design andcontroller design (or both at the same time, as done inco-design [28, 29]).

11 Summary, Conclusions, and Future Work

In this research, the success of a reduced order dynamicmodel with an Extended Kalman Filter was demonstrated insimulation. Localization values obtained were very closeto ground truth and no large deviations were observed.Comparison with Kinematics based Kalman filter justifiedthe choice of including dynamics in the form of a reducedorder model. Dynamics-based EKF performance surpassedits counterpart in all test cases, except one, with largemargins.

From the Figs. 6 to 8 and the Tables 7 to 9, the dynamicKalman filter predictions were very close to the ground truthvalues. On comparison of the individual values predictedby the dynamic filter and the kinematic filter with the actualposition of RexROV, the dynamic filter was more stable andreliable than the kinematic filter. The results were based onmovement only in the two primary axes, x and y, similar toa ground vehicle. Further research is being conducted onthe simultaneous triaxial movement to make the controlleruniversal. It is expected that the movement on the z-axisshould not affect the performance of the dynamic Kalmanfilter. Currently the model uses GPS data to correct positionestimation. However research has indicated that the strengthof the electromagnetic waves for the GPS signal reducessignificantly underwater [30]. Methods such as stationkeeping, SONARSLAM and vision systems are currentlybeing explored as an alternative for position estimation.Alternative controller techniques are being researched toimprove the performance of the model. The predictions wereconducted at a frequency of 10Hz and the dynamic filterworked on the simulation without any performance issues.However, the filters need to be implemented in real-timehardware to assess the actual computational performance.

AcknowledgementsThis research was funded by a generous gift grant from

TechnipFMC (https://www.technipfmc.com/en).

References[1] Y. Lee, J. Choi, and H.-T. Choi, “Experimental results

on EKF-based underwater localization algorithm usingartificial landmark and imaging sonar,” in Proceedingsof 2014 Oceans - St. John’s, 14-19 September, 2014, St.Johns, NL, Canada., pp. 1–3, IEEE, 2014.

[2] D. Park, J. Kim, and W. K. Chung, “Simulated 3Dunderwater localization based on RF sensor modelusing EKF,” in 2011 8th International Conference onUbiquitous Robots and Ambient Intelligence (URAI),23-26 November, 2011, Incheon, South Korea.,pp. 832–833, IEEE, 2011.

[3] J. Almeida, B. Matias, A. Ferreira, C. Almeida,A. Martins, and E. Silva, “Underwater localizationsystem combining iUSBL with dynamic SBL in¡VAMOS! trials,” Sensors, vol. 20, no. 17, 2020.

[4] B. Allotta, L. Chisci, R. Costanzi, F. Fanelli,C. Fantacci, E. Meli, A. Ridolfi, A. Caiti, F. Di Corato,and D. Fenucci, “A comparison between EKF-basedand UKF-based navigation algorithms for AUVslocalization,” in OCEANS 2015 - Genova, 18-21 May,2015, Genova, Italy., pp. 1–5, IEEE, 2015.

[5] M. Karimi, M. Bozorg, and A. R. Khayatian, “Acomparison of DVL/INS fusion by UKF and EKF tolocalize an autonomous underwater vehicle,” in 2013First RSI/ISM International Conference on Roboticsand Mechatronics (ICRoM), 13-15 Februrary, 2013,Tehran, Iran., pp. 62–67, IEEE, 2013.

[6] F. Maurelli, S. Krupinski, Y. Petillot, and J. Salvi,“A particle filter approach for AUV localization,” inProceedings of OCEANS 2008, 15-18 September, 2008,Quebec City, QC, Canada., pp. 1–7, IEEE, 2008.

[7] N. Y. Ko, T. G. Kim, and Y. S. Moon, “Particle filterapproach for localization of an underwater robot usingtime difference of arrival,” in Proceedings of 2012Oceans - Yeosu, 21-24 May, 2012, Yeosu, South Korea.,pp. 1–7, IEEE, 2012.

[8] Y. Xu, W. Dandan, and F. Hua, “Underwater acousticsource localization method based on TDOA withparticle filtering,” in Proceedings of the 26th ChineseControl and Decision Conference (2014 CCDC), 31May - 2 June, 2014, Changsha, China., pp. 4634–4637,IEEE, 2014.

[9] N. Y. Ko and T. G. Kim, “Comparison of Kalman filterand particle filter used for localization of an underwatervehicle,” in 2012 9th International Conference onUbiquitous Robots and Ambient Intelligence (URAI),26-28 November, 2012, Daejeon, South Korea.,pp. 350–352, IEEE, 2012.

[10] A. Gadre and D. Stilwell, “A complete solution tounderwater navigation in the presence of unknowncurrents based on range measurements from a singlelocation,” in 2005 IEEE/RSJ International Conference

on Intelligent Robots and Systems, 2-6 August, 2005,Edmonton, AB, Canada., pp. 1420–1425, IEEE, 2005.

[11] J. Jouffroy and J. Opderbecke, “Underwater vehiclenavigation using diffusion-based trajectory observers,”IEEE Journal of Oceanic Engineering, vol. 32, no. 2,pp. 313–326, 2007.

[12] D. Ribas, P. Ridao, and J. Neira, Underwater SLAMfor Structured Environments Using an Imaging Sonar.Springer Berlin Heidelberg, 2010.

[13] O. Hegrenaes and O. Hallingstad, “Model-aided inswith sea current estimation for robust underwaternavigation,” IEEE Journal of Oceanic Engineering,vol. 36, no. 2, pp. 316–337, 2011.

[14] M. M. M. Manhaes, S. A. Scherer, M. Voss, L. R.Douat, and T. Rauschenbach, “UUV simulator: Agazebo-based package for underwater intervention andmulti-robot simulation,” in Proceedings of OCEANS2016 MTS/IEEE Monterey, 19-23 September, 2016,Monterey, CA, USA., IEEE, sep 2016.

[15] T. Fossen, Handbook of Marine Craft Hydrodynamicsand Motion Control. Wiley, Chichester, UnitedKingdom, 1st ed., 2011.

[16] A. Adam and E. Erik, “Model-based design,development, and control of an underwater vehicle,”2016. Joint MS Thesis, Department of ElectricalEngineering, Linkoping University, Linkoping,Sweden.

[17] V. Berg, “Development and commissioning of aDP system for ROV SF 30k,” 2012. MS Thesis,Department of Engineering Cybernetics, NorwegianUniversity of Science and Technology, Trondheim,Norway.

[18] L. Govinda, S. Tomas, M. Bandala, L. Nava Balanzar,R. Hernandez-Alvarado, and J. Antonio, “Modelling,design and robust control of a remotely operatedunderwater vehicle,” International Journal ofAdvanced Robotic Systems, vol. 11, p. 1, 01 2014.

[19] M. Caccia and G. Veruggio, “Guidance and control of areconfigurable unmanned underwater vehicle,” ControlEngineering Practice, vol. 8, pp. 21–37, 2000.

[20] M. Kim, H. Joe, J. Kim, and S. cheol Yu, “Integralsliding mode controller for precise manoeuvring ofautonomous underwater vehicle in the presence ofunknown environmental disturbances,” InternationalJournal of Control, vol. 88, no. 10, pp. 2055–2065,2015.

[21] T. Fossen, Nonlinear Modelling and Control ofUnderwater Vehicle. PhD Thesis, Department ofEngineering Cybernetics, Norwegian University ofScience and Technology, Trondheim, Norway, 1991.

[22] R. W. Hascaryo, “Reduced order extended kalman filterincorporating dynamics of an autonomous underwatervehicle for motion prediction,” 2019. MS Thesis,Department of Aerospace Engineering, University ofIllinois at Urbana-Champaign, Illinois, USA.

[23] W. R. Norris and A. E. Patterson, “System-level testingand evaluation plan for field robots: A tutorial with testcourse layouts,” Robotics, vol. 8, no. 4, 2019.

[24] R. E. Kalman, “A new approach to linear filteringand prediction problems,” Transactions of theASME–Journal of Basic Engineering, vol. 82,no. Series D, pp. 35–45, 1960.

[25] L. Kudryavtsev, “Taylor series. encyclopediaof mathematics.,” 2020. US Governmentdocument available at http://www.encyclopediaofmath.org/index.php?title=Taylor series&oldid=15684.

[26] R. C. Coulter, “Implementation of pure pursuit pathtracking algorithm,” Techical Report: The RoboticsInstitute, Carnegie Mellon University, 1992.

[27] J. G. Agudelo, Contribution to the Model andNavigation Control of an Autonomous UnderwaterVehicle. PhD thesis, Departament d’EnginyeriaElectronica , Universitat Polit‘ecnica de Catalunya,2015.

[28] C. M. Chilan, D. R. Herber, Y. K. Nakka, S.-J.Chung, J. T. Allison, J. B. Aldrich, and O. S.Alvarez-Salazar, “Co-design of strain-actuated solararrays for spacecraft precision pointing and jitterreduction,” AIAA Journal, vol. 55, pp. 3180–3195,Sept. 2017.

[29] J. T. Allison, T. Guo, and Z. Han, “Co-design ofan active suspension using simultaneous dynamicoptimization,” Journal of Mechanical Design, vol. 136,June 2014.

[30] G. Taraldsen, T. A. Reinen, and T. Berg, “Theunderwater gps problem,” in Proceedings of OCEANS2011 - Spain, 6-9 June, 2011, Santander, Spain.,pp. 1–8, IEEE, 2011.