Position Estimation of Remotely Operated Underwater Vehicle

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Position Estimation of Remotely Operated

Underwater Vehicle

Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping

av Kenny Jönsson LiTH-ISY-EX--10/4361--SE

Linköping 2010

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Position Estimation of Remotely Operated

Underwater Vehicle

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Kenny Jönsson LiTH-ISY-EX--10/4361--SE

Handledare: Martin Skoglund

isy, Linköpings universitet

Linus Aldebjer

SAAB Underwater Systems

Examinator: Fredrik Gustafsson

isy, Linköpings universitet

(4)

(5)

Avdelning, Institution Division, Department

Division of Automatic Control Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

Datum Date 2010-06-22 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-57518 ISBN — ISRN LiTH-ISY-EX--10/4361--SE Serietitel och serienummer Title of series, numbering

ISSN —

Titel Title

Positionsestimering av undervattensfarkost

Position Estimation of Remotely Operated Underwater Vehicle

Författare Author

Kenny Jönsson

Sammanfattning Abstract

This thesis aims the problem of underwater vehicle positioning. The vehicle used was a Saab Seaeye Falcon which was equipped with a Doppler Velocity Log(DVL) manufactured by RD Instruments and an inertial measurement unit (IMU) from Xsense. During the work several different Extended Kalman Filter (EKF) have been tested both with a hydrodynamic model of the vehicle and a model with constant acceleration and constant angular velocity. The filters were tested with data from test runs in lake Vättern. The EKF with constant acceleration and constant angular velocity appeared to be the better one. The misalignment of the sensors were also tried to be estimated but with poor result.

Nyckelord

(6)

(7)

Abstract

This thesis aims the problem of underwater vehicle positioning. The vehicle used was a Saab Seaeye Falcon which was equipped with a Doppler Velocity Log(DVL) manufactured by RD Instruments and an inertial measurement unit (IMU) from Xsense. During the work several different Extended Kalman Filter (EKF) have been tested both with a hydrodynamic model of the vehicle and a model with constant acceleration and constant angular velocity. The filters were tested with data from test runs in lake Vättern. The EKF with constant acceleration and constant angular velocity appeared to be the better one. The misalignment of the sensors were also tried to be estimated but with poor result.

(8)

(9)

Acknowledgments

I would like the thank all who, in someway, have contributed to the work in this report. This thesis could not have been done without the help and advices from my supervisors both at Saab Underwater System and the university. All employees at Saab should have an eulogy for creating a nice and productive work environment. I would also like to thank my family for the support throughout this spring and specially my fiancée who have supported me heavily.

(10)

(11)

Introduction

1.1 Background

Unmanned Underwater Vehicles (UUV) are widely used around the world, both in military applications such as mine hunting and mine disposal and civilian applica-tions such as surveillance of divers or hull inspecapplica-tions. There exist different kinds of UUV and they are mainly divided into Remotely Operated Vehicles (ROV) and Autonomous Underwater Vehicles (AUV). The main difference between those two are that the ROV is tethered either to a submarine, a surface vessel or a harbour and is thereby controlled by a pilot. The AUV is instead autonomous and operates without supervision. This thesis will mainly address the positioning of a ROV, since the ROV often can move in all directions but the AUV often just moves forward and steer the heading and depth with rudders like a torpedo. The ROV instead have thrusters in all directions as seen in Figure 1.2. Figure 1.1 shows an example of an AUV. The vehicle is Saab Sapphires and it looks very much like torpedo.

When the ROV is below the surface it is hard to tell where it is positioned. GPS transmitters do not work under the surface and can therefore not be used.

Figure 1.1: Saab Sapphires, an example of an AUV. The vehicle looks like a torpedo as seen.

(14)

2 Introduction

Figure 1.2: The Falcon vehicle which was used during the study.

Today one uses different kinds of Inertial Navigation Systems (INS), sometimes supported with velocity measurements to estimate the position. Some of those systems have quite accurate position estimation but they are also very expensive.

1.2 Aim and problem description

SAAB Underwater System is interested in using less expensive sensors and instead using powerful software and model the dynamic properties of the vehicle. It is important that the algorithm manages to estimate the position and orientation even if the vehicle’s properties change due to the vehicle picks something up from the seabed for instance. The aim of this thesis is to investigate if this can be solved with an algorithm that is independent of the vehicle’s properties or if a property dependent vehicle model has to be used. The thesis should answer the following questions:

1. Will the position estimation be better if a hydrodynamic model is used in-stead of a model which assumes constant acceleration and constant angular velocity?

2. Is it possible to estimate the hydrodynamic coefficients online?

1.3 Vehicle

The vehicle which will be used in this study is a SAAB Seaeye Falcon system. The vehicle can be seen in Figure 1.2. The vehicle can be controlled in four degrees of freedom, x, y, z,-directions and heading. The vehicle has three sensors which two of them will be used in the study.

(15)

1.4 Previous work 3 • Xsens MTi sensor. This is an IMU that measures the specific force and

angular velocity of the vehicle and also the magnetic field around the vehicle. The sensor has an internal sensor fusion algorithm that can give the attitude of the vehicle. This feature will only be used to see if the created algorithms give reasonable results.

• RD Instruments Workhorse Navigator. This sensor is a Doppler Velocity Log(DVL) which measures the velocity of the vehicle relative the seabed. The output can be in either the earth fixed or a sensor fixed reference frame and therefore the DVL also has an internal compass. In this thesis, the output will be in the sensor-frame since then the accuracy of the velocity output is independent of the compass accuracy.

• The third sensor is a depth sensor. The sensor measures the pressure around the vehicle and converts it to corresponding depth. This sensor will not be used in this thesis since this study is aiming at estimating the horizontal position.

The sensors will be presented further in Chapter 3.

1.4 Previous work

Earlier master thesis at Saab Underwater Systems have resulted in a model which can be used for modelling the ROV[1, 2]. This model is a rigid-body model which simulates the hydrodynamic forces acting on the vehicle. The forces are repre-sented in a body fixed coordinate system. The model can be used in the develop-ment of the navigation algorithm, see further Section 2.3.2.

There has been fairly much research in related fields of research. There are several article published about using observers for estimation of hydrodynamic coefficients in underwater vehicles. In one of the articles they have studied how the hydrodynamic coefficients can be estimated in a vehicle that is shaped like a torpedo[3]. The other ones have studied a vehicle that moves in six degrees of freedom like the Falcon ROV[4, 5]. Those studies have been mostly theoretical studies where they have used theory and simulations to test their hypotheses. There has also been research about how a linearised model can be determined at a certain operating point[6].

1.5 Outline of the work

This report have been divided into five chapters. The first chapter is introduction and the second explains the theory that have been used. In the second chapter different reference frames are explained. The reference frames are very important since different sensors present its data in different reference frames. One can read how to represent the same vector in different reference frames. The chapter also explains how to keep track of the rotation of the vehicle.

(16)

4 Introduction

Figure 1.3: The Double Eagle Sarov which was used to measure the reference trajectory.

The third chapter presents the sensors and the sensor models. It also presents how the sensors have been calibrated and how they are used.

The fourth chapter contains information about what filters that have been tested and what motion models have been used. Some of the filters have been tested with data collected in a water tank and others have been tested with data collected in Lake Vättern, depending on the purpose. What kind of data that have been used is explicitly presented for every filter.

The last chapter summarises the conclusions that have been drawn during the work. After the conclusion follows references and appendices. The appendices contain information about the numerical values of parameters that have been used.

1.6 Data collection

Mainly three types of data have been collected. The first type is data collected under stationary conditions. In this case stationary means that the vehicle is at rest. This data have been used for determine covariance matrices for the measure-ment noise of the sensors. The data has both been collected in the water tank at Saab underwater systems and on land.

Data have also been collected when the vehicle have been moving inside the water tank. Since no reference data have been available during these tests, the tests have mainly consist of simple motion patterns like driving in circles or forward and backwards. They have always started and stopped at the same position. This data has been used for test velocity estimations among other things.

The third kind of data is data collected in lake Vättern. This data have been collected to test the position estimation of different filters. During these data collections a reference path was followed. The path was marked by cable on the seabed. Along this cable different landmarks were located. A lattice acted as the starting point and crayfish cage was the end point of the trail. The lattice can be seen in Figure 1.4a and the cage in Figure 1.4b. The trajectory can be seen in Figure 1.5. The reference trajectory was measured with a Double Eagle Sarov vehicle. Sarov is a vehicle that can be used both as a ROV and UAV. Sarov has a

(17)

1.6 Data collection 5

(a) The lattice (b) The crayfish cage

Figure 1.4: The lattice marks the start of the trajectory and the crayfish cage marks the end.

quite sophisticated navigation system that can be considered to measure the truth trajectory for this short path. The strange appearance of the trajectory is due to limited resolution in the data logging function. Sarov can be seen in Figure 1.3.

(18)

6 Introduction −40 −30 −20 −10 0 0 10 20 30 40 East [m] North [m] Reference trajectory

Figure 1.5: The measured reference trajectory. The trajectory starts in origin and ends in [n, e] = [39.9, −33.7].

(19)

Chapter 2

Theoretical Background

2.1 Reference Frames

The ROV is a rigid body and is moving in 6 degrees of freedom. The degrees of freedom are [u, v, w, p, q, r]T where u, v, w are the velocity in surge, sway and heave and p, q, r are the angular velocities in roll, pitch and yaw[7]. To describe these movements, several coordinate frames will be used.

• NED: The North-East-Down frame is defined as the tangential plane to earth’s surface in the point in which the ROV is positioned. Since the ROV is moving in a local area at moderate speeds, the earth can be considered to be flat and this coordinate system is assumed to be inertial. This is the coordinate system that will be used in the navigation algorithm for keeping track of the position. This frame is referred to as n-frame.

• NWU : This frame (North-West-Up) is the same as the n-frame but rotated π radians around the north axis. This frame is referred to as the w-frame. • BODY : This frame is fixed in the vehicle with the the xb-axis pointing

fore-words in the vehicle’s direction, yb-axis to the port and the zb-axis upwards.

The origin of the frame will be approximately the centre of mass of the vehicle. This frame is referred to as b-frame.

• SENSOR: This frame represents those frames that the sensors presents its data in. The frame is fixed in the b-frame but it is not sure they will coincide. The reference of this frame is either MTi-frame or DVL-frame depending on which sensor frame that is considered. It can also be referred to as the s-frame if it is an arbitrary sensor that it could refer to.

2.1.1 Transformation between reference frames

A vector that is described in a certain reference frame can also be presented using another reference frame[7]. The transformation between two frames can be done

(20)

8 Theoretical Background

by multiplying the vector by a rotation matrix that fulfills RRT =RTR = I3,

det(R) =1

where I3is the 3 by 3 identity matrix. The rotation matrix can be buildt with Rλ,β=I3+ sin(β)S(λ) + (1 − cos(β))S2(λ) (2.1) where λ is a unit vector which means |λ| = 1 and λ is also parallel with the rotation axis, β the rotation angle and S(λ) is the skew-symmetric matrix

S(λ) =   0 −λ3 λ2 λ3 0 −λ1 −λ2 λ1 0   (2.2) λ =λ1 λ2 λ3

2.2 Attitude of the vehicle

The attitude of a vehicle can be interpreted as three rotations around three different axes, called the Euler angles. These rotations is often represented by Θ = [φ, θ, ψ]. In this thesis, the unit quaternion will be used instead. This due to the lack of singularities which exist when Euler angles are used and the pitch angle θ approaches 90◦. Euler angles are used for describing the rotation of sensors relative the b-frame. This because the Euler angles are easy to understand and the singularities wont be any problem since the sensors will not have a pitch of more then a few degrees relative the b-frame. Further description of the Euler angles can be seen in many books[1, 7, 8, 9].

2.2.1 Unit quaternion

The unit quaternion can be interpreted as an expansion of the complex numbers into three dimensions represented by a vector[7]

q =     q0 q1 q2 q3     (2.3)

To represent a rotation in three dimensions they must fulfil qT_{q = 1. The real part}

η = q0can be defined as η = cosβ₂ and the imaginary parts = [q1, q2, q3] = λ sinβ₂ where λ is the unit vector in (2.1). With this definition the rotation matrix in (2.1) can be expressed as

R_bw= Rη,= I3+ 2ηS() + 2S2() (2.4)

(21)

2.3 ROV dynamics 9

The notation Rw_b means that the vector vb which is represented in the b-frame can be represented in the w-frame by

vw= Rw_bvb

In this thesis the quaternion will describe the rotation of the b-frame relative the w-frame, which means that the b-frame will coincide with the w-frame when the Euler angles roll, pitch and yaw are equal to zero. Therefore, the complete rotation from the b-frame to the n-frame is described by

Rn_b(q) =   1 0 0 0 −1 0 0 0 −1  R w b(q) (2.5)

With this representation of the attitude the kinematics of the orientation will be described by[7] ˙ q = 1 2T (q) · ω (2.6) where T (q) =     −1 −2 −3 η −3 2 3 η −1 −2 1 η     η =q0  =[q1q2q3]T

2.2.2 Euler angles from unit quaternion

One way to get the Euler angles from unit quaternions is to require the rotations matrices to be equal[7]. This requirement yields

φ = arctan(Rw_b,(3,2)(q), Rw_b,(3,3)(q))

θ = arcsin(Rw_b,(3,1)(q)) (2.7)

ψ = arctan(Rw_b,(2,1)(q), Rw_b,(1,1)(q))

where the function arctan is the inverse function of tangens defined in all four quadrants.

2.3 ROV dynamics

The models described in this section describe the behaviour of the ROV. The mod-els will be used in different Extended Kalman Filters. Two different modmod-els will be used: one rigid-body model which assumes constant acceleration and constant angular velocity and one with a hydrodynamic model for the acceleration and an-gular velocity. Numerical values of the hydrodynamic coefficients is presented in Appendix B.

(22)

2.3.1 Rigid-Body model with constant acceleration and

con-stant angular velocity

This model is very simple and is easy to understand[8]. The model can be used when the the knowledge of the system is limited since it only assumes a rigid-body and moderate changes in acceleration and angular velocity. In state-space form the model can be represented as

      ˙an ˙vn ˙ pn ˙ ω ˙ q       =       0 0 0 0 0 I 0 0 0 0 0 I 0 0 0 0 0 0 0 0 0 0 0 1 2T (q) 0             an vn pn ω q       (2.8)

where an_{is the acceleration, v}n_{the velocity, p}n _{the position in the centre of mass}

of the vehicle and ω is the angular velocity and q the attitude of the vehicle. The acceleration, velocity and position are all represented in an inertial reference frame. In one implementation the acceleration and velocity will be represented in the b-frame and therefore we have

        dab dt b dvb dt b ˙ pn ˙ ω ˙ q         =       −ω × ab ab_{− ω × v}b Rn b(q)v b 0 1 2T (q) · ω       (2.9)

since for the derivative of vector V in a rotating reference frame β can be repre-sented as[9]  dV dt = dV dt β + ωβ× V (2.10)

2.3.2 Hydrodynamic model

The hydrodynamic model used is mainly the same as the one used at Saab Un-derwater Systems and presented in earlier thesis at Saab[1, 2]. Numerical values used for the hydrodynamic coefficients is presented in Appendix B. To determine the motion of the vehicle, Euler’s first and second axiom can be used. The axioms tells us how a rigid body will move when affected by forces and moments. The axioms can be stated as

F = dmv dt inertial (2.11) τ = dIω dt inertial (2.12) where v represent the velocity of the body’s centre of mass and ω is the angular velocity of the body in a inertial frame. F is the total amount of force affecting

(23)

2.3 ROV dynamics 11

the body, m the mass of the vehicle, τ the total amount of moments and I is the inertia matrix. These axioms can be rearranged using (2.10) and results in

 dmv dt b + ω × mv =F (2.13)  dIω dt b + ω × Iω =τ (2.14)

Since m and I are constant in the body frame, this expression can be rearranged to  dv dt b =1 m(F − (ω × mv))  dω dt b =I−1(τ − ω × Iω) (2.15)

The vehicle is affected by a number of forces all of them presented in the following sections. All forces and moment are represented in the b-frame.

Added Mass and Inertia

The loss of acceleration in water can be modelled as an added mass[1, 7]. This apply both for the translation and rotation of the vehicle. To capture the difference of the vehicle in different direction m is replaced with I3m + Ma and I with I + Ia

in (2.15). Both Ma and Ia are three dimensional diagonal matrices. This yields

 dv dt b =(I3m + Ma)−1(F − (ω × (I3m + Ma)v)) (2.16)  dω dt b =(I + Ia)−1(τ − ω × (I + Ia)ω) (2.17) Gravity

The gravity is affecting the vehicle and can be modelled as

Fgb=Rbn(q)   0 0 mg   (2.18) τg=0

where g ≈ 9.81 in Sweden. The force of gravity is affecting the vehicle in the centre of mass and therefore the moment from the gravity force is zero.

Buoyancy

The buoyancy force is a force that acts on the vehicle and prevents it from sinking. Buoyancy arise when a body moves the fluid surrounding it self and take its place.

(24)

Theoretically, buoyancy forces and moments can be described by

F_bb=Rbn(q)   0 0 −gρV   (2.19) τb=rcb× Fbb

where the vector rcbis pointing from the centre of mass to the centre of buoyancy.

In most underwater vehicle the centre of buoyancy is above the centre of gravity since there will be a restoring moment on the vehicle and the vehicle will be stable. In most underwater applications the vehicle has almost the same buoyancy and gravity magnitude since it otherwise would sink to the bottom or reach the surface. Therefore the buoyancy force is often set to

F_bb=Rb_n(q)   0 0 −mg   (2.20)

since the vehicle then is neutral in the water. Force from thrusters

Earlier master thesis at Saab underwater systems have suggested a model for the thrusters on this vehicle[2]. This is a dynamic model with a rise time of 120 ms. This dynamics is fast compared to the rest of the system and will therefore be neglected. The force from the thrusters will depend on the input signals and is described by

F_t,ib = 2.98 · 10−4u3_i − 0.016|ui|ui+ 0.32ui Vi (2.21)

where Ft,ib is the force from thruster i and ui is the input signal for thruster i. Vi

is a unit vector pointing in the direction as the thruster exerts its force. This force also create a moment on the vehicle which can be calculated by

τ_t,ib =rb_t,i× Fb

t,i (2.22)

where rb_t,i is the vector pointing from the origin to the location of the thruster.

Drag force

A good model for the drag force on the Falcon vehicle has also be presented in earlier thesis. The common practise is to model the drag force as quadratic but in Millert et al. a linear term is added[2].

Fdb=   (CD,x,lin+ CD,x,quad· |vx|) vx (CD,y,lin+ CD,y,quad· |vy|) vy (CD,z,lin+ CD,z,quad· |vz|) vz   (2.23)

(25)

2.4 Discretisation 13

As seen in (2.23) the drag force is assumed to affect the system independently in the three axis. CD,i,j represents the drag coefficients which can be seen in

Appendix B.

The drag force is assumed to affect the vehicle in the centre of mass and therefore the drag force will not produce any moment. However, the rotational drag will produce a moment according to

τ_db =   (CD,x,anglin+ CD,x,angquad· |ωx|) ωx (CD,y,anglin+ CD,y,angquad· |ωy|) ωy (CD,z,anglin+ CD,z,angquad· |ωz|) ωz   (2.24)

2.4 Discretisation

In an EKF the model has to be discrete and therefore discretisation of the rigid-body model and the sensor models are done by using a first order Taylor approx-imation.

d dty(t) ≈

y(t + ∆t) − y(t)

∆t (2.25)

If ∆t is chosen to be the sample frequency Ts then Equation (2.25) can be

rear-ranged to

yk+1≈ yk+ Ts

dy

dt(t) (2.26)

where yk+1= y(t + ∆t) and yk= y(t).

A stable and well behaving system does not have to be stable when the system is discretised. If the sample time Ts is set to a to large value the system can be

unstable. This system is tested with a sample time Ts = 0.01 s, which gives a

stable system.

When noise has to be discrete, this can be done in several ways. The easiest way, which will be used in this thesis is

(26)

(27)

Chapter 3

Sensors and Sensor Models

In the navigation system two different sensors will be used. One of them are the MTi sensor delivered by Xsens that measures the specific force and angular velocity of the vehicle. It also measures earth’s magnetic field which can be used to determine the attitude of the vehicle. The vehicle also has a doppler velocity log (DVL) from RD Instruments which measures the velocity and heading of the vehicle.

3.1 Xsens MTi

This sensor measures the specific force, angular velocity of the body and also the magnetic field around the body. The acceleration are not the same everywhere in-side the body and therefore it is important to know where the sensors are mounted in the vehicle. rM T i will denote the vector pointing towards the sensor with the

origin at the vehicle’s centre of mass. Now the acceleration measured by the sensor can be expressed as

ameasured=abvehicle+ ω × (ω × rM T i) + α × rM T i+ Rbn(q) · g

n _(3.1)

where ω and α represent the angular velocity and angular acceleration of the vehicle respectively. gn represents the gravity vector which also will be measured and is assumed to point downwards in the n-frame. α is unknown and therefore the third term will be neglected and the measurement noise increased instead.

3.1.1 Sensor calibration

Magnetic fields will be influenced by magnetic materials in the vehicle and the environment. This will introduce an error in the heading estimation, but the error deriving from magnetic materials in the vehicle can be compensated for[10]. When one rotates the vehicle at a certain position the magnetic field is constant in an earth fixed reference frame. If the vehicle rotates around a certain axis the measured magnetic field will be measured as a circle with the centre at origin.

(28)

16 Sensors and Sensor Models −0.5 0 0.5 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4

The measured magnetic field

y_mag,x

y mag,y

Figure 3.1: The measured magnetic field when the vehicle is rotated. Unfortu-nately, these measurements were subject to severe disturbances.

When the magnetometers are influenced by magnetic materials the plot will be an ellipses and may also be biased. In Figure 3.1 you can see that the magnetometers need to be calibrated. Unfortunately, the measurements probably were subject to severe disturbances which can affect the calibration in the x, y-plane. Since we not are interested in the magnitude of the magnetic field, the compensation can be done according to Hb=γymag+ κ (3.2) γ =   γx 0 0 0 γy 0 0 0 γz   (3.3)

where γ represents the scale error and κ the known bias. Since it is preferable to estimate the bias for all measurements from the MTi sensor, κ will not be used other then for initialising the estimation of the bias vector. Determination γ and κ can be done in several, more or less, advanced ways[11]. In this thesis γ and κ were determined in two steps.

(29)

3.1 Xsens MTi 17 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3

The magnetic field after calibration

Hy

H_x

Figure 3.2: The measured magnetic field when the measurement is compensated according to equation (3.2). The vehicle was rotated around its z-axis.

that γxand γy were set to

γx= max 1, Ymax− Ymin Xmax− Xmin (3.4) γy= max 1,Xmax− Xmin Ymax− Ymin (3.5)

where Xmax and Xmin is the largest and smallest measured value of the

magnetic field in x-axis. The same goes with Ymaxand Ymin. κxand κywas

then determined by κx=  Xmax− Xmin 2 − Xmax · γx (3.6) κy =  Ymax− Ymin 2 − Ymax · γy (3.7)

2. The vehicle was rotated around its x-axis and data was collected. Since the magnetometers know are calibrated in the x, y-plane, measurements in z-axis

(30)

18 Sensors and Sensor Models −1 −0.5 0 0.5 1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8

The magnetic field after calibration

H Z

H_y

Figure 3.3: The measured magnetic field when the measurement is compensated according to equation (3.2). The vehicle was rotated around its x-axis.

are corrected to fit a sphere. γz and κz were then determined by

γz= Ymax− Ymin Zmax− Zmin (3.8) κz=  Zmax− Zmin 2 − Zmax · γz (3.9)

In Figures 3.2 and 3.3 the result of the calibration can be seen.

The vector Hb _{will be compared to a vector with the same direction as the}

magnetic field. This vector, Hn

ref, will be normalised to the length of the mean

of the compensated calibration data used in Figure 3.2. The relation between the vectors is

Hb =R(q)b_nH_refn (3.10)

where the matrix R(q)b

n is the rotation matrix from the n-frame to the b-frame.

Combining (3.10) with (3.2) gives

ymag=γ−1 R(q)bnH n ref − κ = γ−1R(q) b nH n ref− γ−1κ (3.11)

Using this model with a constant reference magnetic vector, Hn

ref, created

(31)

3.1 Xsens MTi 19

different magnitude in the lake compared to the magnetic field in the water tank. This was solved by normalising H_refn to the mean length of the measured magnetic field.

3.1.2 Sensor misalignment

It is hard to mount a sensor so that the sensor’s coordinate system aligns perfectly with the vehicle’s coordinate system. This can be solved in different ways. One way is to use a rotation matrix which rotates the vectors from the sensor frame to the body frame. For the MTi sensor this gives

yM T i =   RM T i b (ΘM T i) abvehicle+ ω × (ω × rM T i) + Rbn(q) · gn RM T i b (ΘM T i)ω RM T i b (ΘM T i)γ−1Rbn(q)Hrefn   + bM T i,k+ vM T i,m (3.12)

where ΘM T i is the orientation of the MTi sensor relative the vehicle’s coordinate

system and Rs

b(ΘM T i,k) is the rotation matrix from the body fixed system to the

sensor fixed. bM T i,k is the estimated bias and vM T i,m is the measurement noise.

For this system ΘM T i ≈−3.4◦ −1.2◦ 0◦

T .

3.1.3 MTi model

The complete model for this sensor will then be

bM T i,k+1=bM T i,k+ vM T i,b (3.13)

yM T i =   RM T i_b (ΘM T i,k) abvehicle+ ω × (ω × rM T i) + Rbn(q) · gn RM T i_b (ΘM T i,k)ω R_bM T i(ΘM T i,k)γ−1Rbn(q)Hrefn   + bM T i,k+ vM T i,m (3.14)

where H is the magnetic field, bM T i,k is the bias for the sensor at time instance k

and vM T i,bare assumed to be a vector with white noise. vmis described in section

3.1.4. The covariance matrix for the noise is determined by analysing measurement and the covariance matrix for vM T i,b are assumed to be diagonal and the variances

are set according to the sensor manual. The covariance matrices are presented in Appendix A. The bias vector is initialised to

bM T i,0=   0 0 −γ−1_κ   (3.15)

3.1.4 Measurement noise

Figure 3.4 and 3.5 show the estimated Power Spectral Density (PSD) for the noise in the x-direction for the accelerometer in the MTi sensor. Figure 3.4 shows

(32)

20 Sensors and Sensor Models 0 0.5 1 1.5 2 2.5 3 0 0.5 1 1.5x 10 −4 _PSD

Normalised Frequency [rad/s]

Figure 3.4: Power spectral density for the measurement noise in accelerometer’s x-direction when measured with vehicle on land.

that the noise has a relative large power around 1.45 rad. This phenomena is not present in Figure 3.5 at the same frequency. When the vehicle is located on land this is probably due to vibrations from a ventilating fan located beneath the floor were the measurements were undertake. In Figure 3.5 the behavior of the PSD probably is due to small waves in the water. When looking at the z- and y-directions the result is almost the same. From this, the conclusion that the noise can be modelled as white noise is drawn. The power decreases with the frequency but not that much that it will significantly change the result. When looking at the gyros and magnetometers, the PSDs have approximately the same appearance and the same conclusions were drawn. The covariance matrix for the measurement noise is decided by analysing the measurement when the vehicle is at rest. The covariance matrix is presented in Appendix A.

(33)

3.1 Xsens MTi 21 0 0.5 1 1.5 2 2.5 3 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1x 10 −4 _PSD

Figure 3.5: Power spectral density for the measurement noise in accelerometer’s x-direction when measured with vehicle in water.

(34)

22 Sensors and Sensor Models

3.2 Doppler Velocity Log (DVL)

The DVL measures the velocity of the vehicle relative the seabed. The sensor is a acoustic device that sends sound pings through the water and measures the reflections at the seabed[12]. In this reflections there will be a doppler shift which corresponds to the velocity of the vehicle. The output from the sensor is rep-resented in a sensor fixed coordinate system, DVL-frame. Neither this sensor is mounted in the vehicle’s centre of mass and therefore a expression is needed for the velocity at the point in which the sensor is mounted. The distance from the vehicle’s centre of mass and the sensor is denoted rDV L and the expression is

vmeasured= vvehicleb + ω × rDV L

The DVL also has an internal compass which measures the heading of the vehicle. The heading is positive when the vehicle is rotated clockwise around the positive z-axis(from above) which gives the model[7]

yDV L= vb_vehicle+ ω × rDV L − arctan(Rw b,(2,1)(q), R w b,(1,1)(q)) − arctan(H n ref,n, H n ref,e) + vDV L,m (3.16) where vDV L,m is the measurement noise described in Section 3.2.2.

− arctan(Rw

b,(2,1)(q), R w

b,(1,1)(q)) gives the heading angle, see Equation (2.7).

− arctan(Hn ref,n, H

n

ref,e) is the magnetic declination i.e. the angle between

mag-netic north and true north. Hn

ref,n is the north component of the magnetic

refer-ence vector and Hn

ref,e is the east component.

3.2.1 Sensor misalignment

For the DVL sensor the misalignment gives the model

yDV L= _RDV L b (ΘDV L) vvehicleb + ω × rDV L − arctan(Rw b,(2,1)(q), R w b,(1,1)(q)) − arctan(H n ref,n, H n ref,e) + ψDV L + vDV L,m (3.17)

ψDV L is the rotation of the DVL around the z-axis and ΘDV L is the orientation

of the DVL sensor relative the vehicle’s coordinate system and Rs

b(ΘDV L,k) is

the rotation matrix from the b-frame to DVL-frame. For this system ΘDV L ≈

0◦ ₀◦ ₋₄₅◦T .

The sensor misalignment can in some of the filters be estimated and will then be modelled as a constant. The equation for this will then be

(35)

3.2 Doppler Velocity Log (DVL) 23 0 0.5 1 1.5 2 2.5 3 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6x 10 −4 _PSD

Figure 3.6: Power spectral density for the DVL’s measurement noise in z-axis. The sample frequency for this measurement was 5.59 Hz.

(36)

24 Sensors and Sensor Models

3.2.2 Measurement noise

The covariance matrix for vDV L,mis a 4×4-matrix which is assumed to be diagonal

and the variances in the different senors are determined by experiment. This sensor does not have a fixed sample time. The sample time depends on how long time elapses before the ping reaches the seabed and bounces back. In figure 3.6 the PSD for the measurement noise is plotted. The sample time in this plot is almost the same during the whole measurement since the vehicle is at rest. The sample frequency in this measurement was 5.59 Hz. The peak around frequency 0.4 corresponds to 0.34 Hz, which is the same as in the peak in figure 3.5. Therefore this peak is also probably due to small waves in the water. The conclusion is that also this measurement noise can be well approximated with Gaussian noise. The covariance matrix for this sensor is decided by the covariance in the measurement and can be viewed in Appendix A.

(37)

Chapter 4

Motion Models and Filters

During the work have several different navigation filters been developed and tested. In this chapter those filters that have contributed to the result of the work in some way will be presented. During the development of the filters many tests were undertaken in a water tank and therefore some of the filters are only tested with data from this water tank. Other filters have also been tested with data from series of data collection in lake Vättern.

All filters are Extended Kalman Filters[13, 8]. The algorithm is implemented with first order Taylor transformation and a normalisation of the unit quater-nions after each measurement update. The normalisation is implemented to make sure |q| = 1. The filters differs in the motion models and what states they are estimating.

4.1 Position estimation with known orientation

In this filter the orientation is assumed to be the same as the orientation estimated by the MTi-sensor. This filter is probably the easiest way to estimate the position of the vehicle. In this filter a model with constant acceleration was used and the estimated acceleration was compared with the one estimated by the filter that uses the hydrodynamic vehicle model. The model is described in (4.1) To see how the accelerometers in the MTi sensor is affecting the estimation this filter have been tested both with and without the use of acceleration data. Test data have been collected in the water tank when the vehicle has been driven one turn in the tank.

ab_k+1=ab_k− Ts(ω × abk) + va vbk+1=vkb+ Ts(abk− ω × vbk) pn_k+1=pk+ TsRbn(q)v b k (4.1) 25

(38)

26 Motion Models and Filters 0 50 100 150 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Estimated velocity Time [s] [m/s] v x v y v z

Figure 4.1: The estimated velocity. Here the accelerometers in the MTi sensor and the DVL are used.

yM T i,k=RM T ib (ΘM T i,k)  a b k+ ωk× (ωk× rM T i) + Rbn(qk)   0 0 9.81    + bM T i,k yDV L,k=RDV Lb (ΘDV L) vbk+ ω × rDV L

where va is the system disturbances affecting the acceleration. When the

ac-celerometer data was not used the measurement equation is just yDV L,k=RDV Lb (ΘDV L) vbk+ ω × rDV L

4.1.1 Performance

Since there dos not exist any reference data for the velocity only subjective evalu-ation of Figures 4.1, 4.2, 4.3 and 4.4 is underlying reason for conclusions. Figure 4.1 and 4.2 shows the estimated velocity. Its hard to notice any big different and it is better to confront 4.3 and 4.4. Figures 4.3 and 4.4 show the acceleration estimations. As seen the estimation behaves better when the acceleration mea-surements are used. The acceleration estimations become more noisy with the accelerometers.

(39)

4.1 Position estimation with known orientation 27 0 50 100 150 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Estimated velocity Time [s] [m/s] v x v y v z

Figure 4.2: The estimated velocity. Here is only the DVL used and not the accelerometers in the MTi sensor.

30 35 40 45 −0.2 −0.1 0 0.1 0.2 Estimated acceleration Time [s] [m/s 2 ] x y z

Figure 4.3: The estimated acceleration. Here the accelerometers in the MTi sensor and the DVL is used.

(40)

28 Motion Models and Filters 30 35 40 45 −0.2 −0.1 0 0.1 0.2 Estimated acceleration Time [s] [m/s 2 ] x y z

Figure 4.4: The estimated acceleration. Here is only the DVL used.

4.2 Position estimation with known orientation

and with use of vehicle model

This estimator was used to see if the velocity estimation can be improved by using a hydrodynamic model of the vehicle. The model that is used is the one presented in Section 2.3.2. The coefficients for this vehicle have been determined in an earlier thesis work at Saab Underwater Systems [2]. In (4.2) the equation for the complete model is presented. In this filter only the DVL sensor and vehicle model is used for velocity estimation. The attitude and rotational velocities are taken from the sensor fusion in the MTi sensor.

vb_k+1=v_kb+ Ts(I3m + Ma)−1(Fk− (ωk× (I3m + Ma)vk)) + vv

pn_k+1=pn_k+ Ts· Rnb(qk)vbk (4.2)

yDV L,k=Rsb(ΘDV L) vvehicleb + ω × rDV L

where va is the system disturbances affecting the velocity.

4.2.1 Performance

For testing the same data was used as when testing the filter described in Section 4.1. The performance of this filter was not as good as expected. In Figures 4.5 the estimated velocity of the vehicle can be seen. The bad velocity estimation is probably due to bad coefficients in the model or in other ways bad accuracy of the model. In Figure 4.7 the modelled velocity can be seen when the model’s input is the same as for the system. Compared to Figure 4.6, the modelled velocity is a little lower in the vehicle’s forward direction(x-axis). Note that the data in Figure

(41)

4.2 Position estimation with known orientation and with use of vehicle model 29 0 50 100 150 −0.5 0 0.5 1 Estimated velocity Time [s] [m/s] v x v y v z

Figure 4.5: The estimated velocity when the vehicle model is used in the Kalman filter. 0 100 200 300 400 500 600 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Measured velocity Sample [m/s] _v x DVL v y DVL v z DVL

Figure 4.6: The measured velocity in this data collection. Note that the DVL-frame is rotated -45◦around the vehicle’s z-axis. Data here is presented in the DVL-frame.

(42)

30 Motion Models and Filters 0 50 100 150 −1 −0.5 0 0.5 1 Estimated velocity Time [s] [m/s] v x v y v z

Figure 4.7: Velocity simulated by the model when the input is the same as for the vehicle.

4.6 is represented in a different reference frame and the forward velocity of the vehicle vx= q v2 xDV L+ v 2 yDV L.

4.3 Position estimation with body fixed

coordi-nate system

In this filter the rigid-body model with constant acceleration and angular velocity in equation (2.9) is used. The acceleration and velocity are represented in the b-frame. This filter has been tested in three different versions with small differences. The first is the one specified in equation (4.3). In the second variant of this filter, the magnetometers in the MTi sensor have been neglected and in the third the compass in the DVL has been neglected. Those difference were made to see how the different sensors behave and how good the estimation were with those.

ab_k+1=ab_k− Ts(ω × abk) + va v_k+1b =v_kb+ Ts(abk− ω × v b k) pk+1=pk+ TsRnb(q)v b k ωk+1=ωk+ vω qk+1=qk+ Ts 2 T (ωk) · qk (4.3) bM T i,k+1=bM T i,k

(43)

4.3 Position estimation with body fixed coordinate system 31 Fig Section Frame ψM T i ψDV L Mean distance

error [m] Sensor misalign-ment estimation 4.8 4.3 b X X 0.98 No 4.9 4.3 b 0 X 3.44 No 4.10 4.3 b X 0 5.91 No 4.11 4.4 n X X 0.79 No 4.12 4.4 n 0 X 1.65 No 4.13 4.4 n X 0 8.15 No 4.14 4.5 b X X 17.73 Yes

Table 4.1: Summary of the position filters described in this chapter.

yM T i,k =       RM T i b (ΘM T i,k)  abk+ ωk× (ωk× rM T i) + Rbn(qk)   0 0 9.81     RM T i_b (ΘM T i,k)ω RM T i b (ΘM T i,k)γ−1R(q)bnHrefn       + bM T i,k yDV L,k= R_bDV L(ΘDV L) vbk+ ω × rDV L − arctan(−Rn b(2, 1), R n b(1, 1)) − arctan(H n ref(2), H n ref(1)) + ψDV L

where va and vomega are the system disturbances affecting the system. For the

second variant of this filter the measurement equation instead is

yM T i,k =     RM T i b (ΘM T i,k)  abk+ ωk× (ωk× rM T i) + Rbn(qk)   0 0 9.81     RM T i_b (ΘM T i,k)ω     + bM T i,k yDV L,k= RDV L b (ΘDV L) vbk+ ω × rDV L − arctan(−Rn b(2, 1), R n b(1, 1)) − arctan(H n ref(2), H n ref(1)) + ψDV L

The third variant has the measurement equations

yM T i,k =       RM T i b (ΘM T i,k)  ab_k+ ωk× (ωk× rM T i) + Rbn(qk)   0 0 9.81     RM T i b (ΘM T i,k)ω RM T i b (ΘM T i,k)γ−1R(q)bnHrefn       + bM T i,k yDV L,k=RDV Lb (ΘDV L) vbk+ ω × rDV L

The Jacobians needed have been analytically determined except for the

∂

∂qarctan(−R n

b(2, 1), R n

b(1, 1)) which is numerically determined.

4.3.1 Performance

This filter was tested with data from data collections in lake Vättern. Figures 4.8, 4.9 and 4.10 all show the estimated position for three different sets of data. The

(44)

32 Motion Models and Filters −35 −34.5 −34 −33.5 −33 39.5 40 40.5 41

End point estimations

East [m]

North [m]

Figure 4.8: Estimated end point with acceleration and velocity represented in b-frame and both the DVL compass and MTi’s magnetometers are used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

position estimation becomes much better when both MTi’s magnetometers and the DVL compass are supporting the gyros when estimating the heading. Table 4.1 presents the performance of the filters.

(45)

4.3 Position estimation with body fixed coordinate system 33 −34 −32 −30 −28 −26 37 38 39 40 41

East [m]

North [m]

Figure 4.9: Estimated end point with acceleration and velocity represented in b-frame and only the DVL compass is used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

−40 −39 −38 −37 −36 −35 −34 36 37 38 39 40

East [m]

North [m]

Figure 4.10: Estimated end point with acceleration and velocity represented in b-frame and MTi’s magnetometers are used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

(46)

34 Motion Models and Filters

4.4 Position estimation filter with earth fixed

co-ordinate system

In this filter the rigid-body model in equation (2.8) is used. In contrast to the filter described in Section 4.3 this filter has the acceleration and velocity represented in the n-frame. Also this filter have been tested in different variants to see how the magnetic information contributes to the estimations accuracy.

ank+1=ank+ va vn_k+1=v_kn+ Ts· ank pn_k+1=pk+ Tsvkn ωk+1=ωk+ vω qk+1=qk+ Ts 2 T (ωk) · qk (4.4) bM T i,k+1=bM T i,k yM T i,k=       RM T i b (ΘM T i,k)  ωk× (ωk× rM T i) + Rbn(qk)  an_k +   0 0 9.81       RM T i b (ΘM T i,k)ω RM T i b (ΘM T i,k)γ−1R(q)bnHrefn       + bM T i,k yDV L,k= RDV L b (ΘDV L) Rbn(qk) · vnk + ω × rDV L − arctan(−Rn

b(2, 1), Rnb(1, 1)) − arctan(Hrefn (2), Hrefn (1)) + ψDV L

where va and vω are the system disturbances. For the second variant of this filter

the measurement equation instead is

yM T i,k=     RM T i b (ΘM T i,k)  ωk× (ωk× rM T i) + Rbn(qk)  an_k +   0 0 9.81       RM T i b (ΘM T i,k)ω     + bM T i,k yDV L,k= RDV L b (ΘDV L) Rbn(qk) · vnk + ω × rDV L − arctan(−Rn b(2, 1), R n b(1, 1)) − arctan(H n ref(2), H n ref(1)) + ψDV L

The third variant has the measurement equations

yM T i,k=       RM T i_b (ΘM T i,k)  ωk× (ωk× rM T i) + Rbn(qk)  an_k +   0 0 9.81       RM T i b (ΘM T i,k)ω RM T i_b (ΘM T i,k)γ−1R(q)bnHrefn       + bM T i,k yDV L,k=RDV Lb (ΘDV L) Rbn(qk) · vnk + ω × rDV L

The Jacobians needed has been analytically determined expect for the

∂

∂qarctan(−R n

b(2, 1), R n

(47)

4.4 Position estimation filter with earth fixed coordinate system 35 −34.538 −34 −33.5 −33 −32.5 38.5 39 39.5 40 40.5 41

End position estimation

East [m]

North [m]

Figure 4.11: Estimated end point with acceleration and velocity represented in n-frame and both the DVL compass and MTi’s magnetometers are used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

4.4.1 Performance

This filter was tested with the same data as the one in Section 4.3. Figure 4.8, 4.9 and 4.10 shows the estimated position for three different sets of data. This filter was tested with data from data collections in lake Vättern. Even with this filter, the position estimation becomes much better when both MTi’s magnetometers and the DVL compass are supporting the gyros when estimating the heading. Table 4.1 presents the performance of the filters. Note that the position estimations is not the same as for the filter presented in Figure 4.3 which have acceleration and velocity represented in the b-frame.

(48)

36 Motion Models and Filters −35 −34 −33 −32 −31 −30 39 39.5 40 40.5 41 41.5

End position estimation

East [m]

North [m]

Figure 4.12: Estimated end point with acceleration and velocity represented in n-frame and only DVL compass is used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

−45 −40 −35 −30 −25 30 35 40 45 50

End point estimation

East [m]

North [m]

Figure 4.13: Estimated end point with acceleration and velocity represented in n-frame and only the MTi’s magnetometers are used. The blue star marks the end of reference data. The green + is the estimated end point for the first set of data. For the second data the estimated end point is marked with the magenta coloured ring and the third is marked with a black ×. All estimations are plotted with ellipses that corresponds to a 99 % confidence interval.

(49)

4.5 Sensor misalignment estimation 37 −45 −40 −35 −30 −25 20 25 30 35 40

End point estimation

East [m]

North [m]

Figure 4.14: Estimated position when the DVL misalignment also is estimated. All estimations are plotted with ellipses that correspond to a 99 % confidence interval.

4.5 Sensor misalignment estimation

Since it is hard to mount the sensors aligned with respect to each other it some-times is preferable to estimate the misalignment. The filter with rigid-body model represented in the body-frame has been complemented with a estimation of the misalignment for the DVL-sensor. The MTi sensor is assumed to have a known misalignment. This because the pitch and roll of the vehicle should be zero when the vehicle is at rest. If using the hydrodynamic model this is important since the model is asymptotically stable with a stationary point when the roll and pitch is zero. The Euler angles describing the misalignment is modelled as constant but with a uncertainty in the initial state.

ΘDV L,k+1= ΘDV L,k (4.5)

(4.3) can be extended with (4.5) and then used for the estimations.

4.5.1 Performance

The position estimation becomes poor when the misalignment is estimated. The misalignment estimation is quite different for the three sets of data. Only when the first set of data is used the estimations become reasonable. For the other two data sets the misalignment estimations differs to much from Θ =0 0 −45 for being reasonable.

(50)

38 Motion Models and Filters 0 50 100 150 200 250 −60 −40 −20 0 20 Estimated DVL misalignment Time [s] Misalignment [degrees]

Figure 4.15: Estimated misalignment for the DVL sensor when the first set of data is used. 0 50 100 150 200 −80 −60 −40 −20 0 20 40 60 Estimated DVL misalignment Time [s] Misalignment [degrees]

Figure 4.16: Estimated misalignment for the DVL sensor in the second set of data.

(51)

4.5 Sensor misalignment estimation 39 0 100 200 300 400 500 −80 −60 −40 −20 0 20 Estimated DVL misalignment Time [s] Misalignment [degrees]

(52)

(53)

Chapter 5

Conclusions

The hydrodynamic vehicle model which have been tested in this thesis was, as seen in earlier chapter, not as good as we hoped. The velocity estimations were better when a model with constant acceleration was used instead. This is probably due to bad coefficients in the model. When the coefficients were determined the vehicle had not any velocity measurement and that can be a reason for why the velocity estimation have its limitations.

It might be good to estimate the hydrodynamic coefficients with a Extended Kalman Filter. That could give coefficients which gives a model that follows the measured velocity better. As read in Section 1.4 about the previous work there have been research about estimating the hydrodynamic coefficients. One of those showed that it is possible to estimate the coefficients.

There seems to be a difference when the velocity and acceleration is represented in i vehicle fixed coordinate system instead of an earth fixed. I can not see way this difference appears.

It seems to be hard to estimate the misalignment of sensors. This can be due to bad observability of the alignment error.

5.1 Continuing work

One thing to continue working with could be to try to estimate the hydrodynamic coefficients in an EKF. This might give coefficients that work better with a EKF for estimation of the position. Since it have been done theoretical studies with simulations it would be interesting to see how it works in reality.

Most of the vehicles Saab Underwater Systems is manufacturing is equipped with one or more cameras and some of them even with a sonar. It would be very interesting to see if would be possible to use these in the navigation algorithms. This could give an opportunity prevent the position to drift away since it might be possible to identify if the vehicle have been at a certain position earlier.

If the heading estimation is bad the position estimation also will be bad. One problem that arose was when the measured magnetic field change magnitude. If the algorithm will be introduced to a commercial product another magnetic model

(54)

42 Conclusions

might be good to consider. The model should estimate the magnitude and maybe also the angle between the horizontal plane and the magnetic field.

(55)

Bibliography

[1] Linus Aldebjer. Hardware in the loop simulator for remotely operated vehicle. Master’s thesis, Chalmers university of technology, December 2004.

[2] Carl Millert and Soma Tayamon. Method development for identification of coefficients in a Remotly Operated Vehicle simulator. Master’s thesis, Faculty of Science and Technology, Uppsala University, April 2009.

[3] Joonyoung Kim. Estimation of Hydrodynamic Coefficients for an AUV Using Nonlinear Observers. IEEE Journal of oceanic engineering, 27, 2002.

[4] A. Alessandri, M. Caccia, G. Indiveri, and G. veruggio. Application of LS and EKF techniques to the identification of underwater vehicles. IEEE Inter-national Conference on Control Application, September 1998.

[5] D. Smallwood. Adaptive Identification of Dynamically posistioned Underwa-ter Robotic Vehicles. IEEE Transactions on control system Technology, 11, July 2003.

[6] A. Taino, R. Sutton, A. Lozowicki, and W. Naeem. Observer kalman fil-ter identification of an autonomous underwafil-ter vehicle. Control Engineering Practice, 15, 2007.

[7] Thor I. Fossen. Marine Control Systems, Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles. Marine Cybernetics, 1 edition, 2002. ISBN 82-9235-00-2.

[8] Fredrik Gustafsson. Statistical sensor fusion. Technical report, Institute of technology, Linköping university, 2009.

[9] A. Pytel and J. Kiusalaas. Engineering Mechanics Dynamics. Thomson Learn-ing, 2 edition, 2001. ISBN 0-534-95742-0.

[10] Michael J. Caruso. Applications of Magnetoresistive Sensors in Navigation Systems. Honeywell Inc.

[11] S. Bonnet, C. Bassompierre, C. Godin, S. Lesecq, and A. Barraud. Calibration methods for inertial and magnetic sensors. Sensors and Actuators A: Physical, 156, December 2009.

(56)

44 Bibliography

[12] RD Instruments. Navigator ADCP/DVL Technical Manual.

[13] Rudolph Emil Kalman. A new approach to linear filtering and prediction problems. Transactions of the ASME–Journal of Basic Engineering, 82(Series D):35–45, 1960.

(57)

Appendix A

Used Covariance Matrices

This appendix presents the covariance matrices that have been used in the filters. The matrices for the measurement noises have been measured and are as follows.

RDV L=



  

1.400e − 5 1.233e − 6 3.223e − 7 1.372e − 4

1.233e − 6 1.247e − 5 −1.376e − 6 2.550e − 5

3.223e − 7 −1.376e − 6 2.409e − 6 −1.463e − 5

1.372e − 4 2.550e − 5 −1.463e − 5 6.182e − 2



  

(58)

46 Used Covariance Matrices RM T i =               4 .58 e − 5 − 1 .67 e − 5 4 .67 e − 6 − 4 .54 e − 8 − 1 .60 e − 8 − 9 .39 e − 8 − 2 .13 e − 9 − 1 .55 e − 7 − 6 .67 e − 7 − 1 .67 e − 5 6 .00 e − 5 2 .12 e − 5 3 .92 e − 7 − 7 .56 e − 8 3 .49 e − 7 − 6 .58 e − 8 − 1 .36 e − 8 1 .72 e − 7 4 .67 e − 6 2 .12 e − 5 7 .77 e − 5 − 3 .73 e − 7 − 8 .93 e − 7 1 .50 e − 7 − 1 .32 e − 7 9 .68 e − 8 3 .22 e − 7 − 4 .53 e − 8 3 .92 e − 7 − 3 .73 e − 7 4 .81 e − 5 1 .16 e − 7 − 4 .49 e − 7 1 .22 e − 7 1 .48 e − 8 6 .32 e − 8 − 1 .60 e − 8 − 7 .56 e − 8 − 8 .93 e − 7 1 .16 e − 7 3 .58 e − 5 − 8 .27 e − 7 − 1 .29 e − 8 6 .83 e − 8 1 .46 e − 7 − 9 .39 e − 8 3 .49 e − 7 1 .50 e − 7 − 4 .49 e − 7 − 8 .27 e − 7 3 .80 e − 5 − 2 .01 e − 7 − 2 .10 e − 8 2 .63 e − 7 − 2 .13 e − 9 − 6 .58 e − 8 − 1 .32 e − 7 1 .22 e − 7 − 1 .29 e − 8 − 2 .01 e − 7 5 .08 e − 7 − 3 .12 e − 8 − 1 .03 e − 7 − 1 .55 e − 7 − 1 .36 e − 8 9 .68 e − 8 1 .48 e − 8 6 .83 e − 8 − 2 .10 e − 8 − 3 .12 e − 8 8 .40 e − 7 5 .33 e − 7 − 6 .67 e − 7 1 .72 e − 7 3 .22 e − 7 6 .32 e − 8 1 .46 e − 7 2 .63 e − 7 − 1 .03 e − 7 5 .33 e − 7 3 .01 e − 6              

(59)

47

The system disturbances used are

Qak+1=Ts   0.025 0 0 0 0.025 0 0 0 0.025   Qωk+1=Ts   0.0025 0 0 0 0.0025 0 0 0 0.0025  

The bias estimation for the MTi sensor have had a noise as source with the covariance matrix QM T i,accelerometers=Ts   0.00025 0 0 0 0.00025 0 0 0 0.00025   QM T i,gyros=Ts   0.76e − 6 0 0 0 0.76e − 6 0 0 0 0.76e − 6   QM T i,magnetometers=Ts   0.8e − 6 0 0 0 0.8e − 6 0 0 0 0.8e − 6  

(60)

(61)

49

Appendix B

Model Coefficients

In this Appendix the hydrodynamic coefficients that have been used is presented.

m =73.3 (B.1) g =9.81 (B.2) ρV = − g (B.3) r_{M ti}b =   −0.11 −0.22 0.10   (B.4) r_{M ti}b =   0.24 0 −0.33   (B.5) rb_b=   0 0 0.2   (B.6) CD,x,lin=42 (B.7) CD,x,quad=140 (B.8) CD,y,lin=180 (B.9) CD,y,quad=55 (B.10) CD,z,lin=90 (B.11) CD,z,quad=400 (B.12) CD,x,anglin=42 (B.13) CD,x,angquad=42 (B.14) CD,y,anglin=55 (B.15) CD,y,angquad=55 (B.16) CD,z,anglin=90 (B.17) CD,z,angquad=90 (B.18) Ma=   27 0 0 0 30 0 0 0 110   (B.19) Ia=   9 0 0 0 15 0 0 0 11.47   (B.20)

(62)

50 Model Coefficients

Position Estimation of Remotely Operated Underwater Vehicle

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Position Estimation of Remotely Operated

Underwater Vehicle

Position Estimation of Remotely Operated

Underwater Vehicle

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Abstract

Acknowledgments

Contents

Chapter 1

Introduction

1.1

Background

1.2

Aim and problem description

1.3

Vehicle

1.4

Previous work

1.5

Outline of the work

1.6

Data collection

Chapter 2

Theoretical Background

2.1

Reference Frames

2.1.1

Transformation between reference frames

2.2

Attitude of the vehicle

2.2.1

Unit quaternion

2.2.2

Euler angles from unit quaternion

2.3

ROV dynamics

2.3.1

Rigid-Body model with constant acceleration and

con-stant angular velocity

2.3.2

Hydrodynamic model

2.4

Discretisation

Chapter 3

Sensors and Sensor Models

3.1

Xsens MTi

3.1.1

Sensor calibration

3.1.2

Sensor misalignment

3.1.3

MTi model

3.1.4

Measurement noise

3.2

Doppler Velocity Log (DVL)

3.2.1

Sensor misalignment

3.2.2

Measurement noise

Chapter 4

Motion Models and Filters

4.1

Position estimation with known orientation

4.1.1

Performance

4.2

Position estimation with known orientation

and with use of vehicle model

4.2.1

Performance

4.3

Position estimation with body fixed