Toward Auto-Calibration of Navigation Sensors for Miniature Autonomous Underwater Vehicles · 2020....

Toward Auto-Calibration of Navigation Sensors for Miniature Autonomous Underwater Vehicles

Jared J. Mach

Thesis submitted to the Faculty of the Virginia Polytechnic Institute and State University in partial fulfillment of the

requirements for the degree of

Master of Science in

Electrical Engineering

Dr. Daniel Stilwell, Chair Dr. Pushkin Kachroo Dr. William Saunders

July 23rd, 2003 Blacksburg, Virginia

Copyright 2003, Jared J. Mach

Toward Auto-Calibration of Navigation Sensors for Miniature Autonomous Underwater Vehicles

Jared J. Mach

Abstract

With an increase in the use of autonomous underwater vehicles (AUV’s) comes the

desire for smaller and cheaper AUV’s. Reduction in size and cost will make AUV

technology more accessible to research and industrial communities. These constraints

also reduce the quality of components used in the vehicle. Availability of calibration

algorithms built into the sensors also diminishes. Thus, there is a need for developing

automatic sensor calibration techniques. This thesis presents the theoretical and

simulation results of calibration algorithms developed for an electronic magnetic compass

and heading rate sensor. Development of the Autonomous Systems and Controls

Laboratory’s first miniature AUV is also presented.

Acknowledgements Many people helped to bring the author to this point in higher education. Great thanks

go the author’s committee chair and advisor, Dr. Daniel Stilwell. His technical and

personal guidance and endless optimism have motivated the author when nothing seemed

to go right.

Thanks to committee members Dr. Pushkin Kachroo and Dr. William Saunders who

gave the author a chance to serve as their teaching assistant and the guidance received

during graduate school.

Thanks to the author’s lab peers Aditya Gadre, Chris Cannel, and Caleb Sylvester for

their help in building the lab’s first AUV. Their help has been a necessity and is greatly

appreciated. Thanks to the members of the ECE shop and ISE machine shop for their

help in accommodating an EE student who likes to play with lathes and mills. And

thanks to Dr. Carl Wick for his ingenuity and consultation on the construction of the

AUV.

On a more personal level, the author wishes to thank the best parents in the world, Jim

and Ann Mach. Thanks to the Virginia Tech Newman Community for reminding the

author that there are more important things in life than cool robots.

This thesis is dedicated to the author’s fiancée, Kristen Heier. She has “stood by her

man”, giving constant encouragement, love, and faith in the author’s potential. This

work would not exist without her support.

iii

Table of Contents Acknowledgements______________________________________________________ iii

Table of Contents _______________________________________________________ iv

List of Figures _________________________________________________________ vi

List of Tables _________________________________________________________ vii

List of Symbols ________________________________________________________ viii

1. Introduction _________________________________________________________ 1

2. Compass & Heading Rate Gyroscope Characteristics ________________________ 7 2.1 The Magnetic Compass ___________________________________________________ 7 2.2 Compass Error Sources__________________________________________________ 10

2.2.1. Misalignment ______________________________________________________________ 10 2.2.2. Hard Iron Distortion_________________________________________________________ 11 2.2.3. Soft Iron Distortion _________________________________________________________ 12

2.3 The Heading Rate Gyroscope _____________________________________________ 13 2.4 Rate Gyro Error Sources_________________________________________________ 15

3. Rate Gyroscope Calibration____________________________________________ 16 3.1 Least Squares Algorithm_________________________________________________ 16 3.2 Adaptive Identifier Algorithm ____________________________________________ 24

4. Compass Calibration _________________________________________________ 35 4.1 Previous Work _________________________________________________________ 35 4.2 Least Squares Algorithm_________________________________________________ 35 4.3 Adaptive Identifier Algorithm ____________________________________________ 44 4.4 Magnetometer-Based Algorithm __________________________________________ 45

5. AUV Design and Construction _________________________________________ 55 5.1 Mechanical Description __________________________________________________ 56

5.1.1. Tail Section _______________________________________________________________ 57 5.1.2. Electronics Bay and Hull _____________________________________________________ 60 5.1.3. Front Bulkhead and Nose Cone ________________________________________________ 61

5.2 Electrical Description____________________________________________________ 62 5.2.1. Control Electronics _________________________________________________________ 63 5.2.2. Sensing Electronics _________________________________________________________ 64 5.2.3. Power Electronics __________________________________________________________ 66 5.2.4. Software Development_______________________________________________________ 66

5.3 Problems Encountered___________________________________________________ 67 5.3.1. Propeller Shaft Seal Design ___________________________________________________ 67 5.3.2. Hull Material Selection ______________________________________________________ 68 5.3.3. Inertial Measurement Card Noise ______________________________________________ 68 5.3.4. Rabbit 3000 Bandwidth ______________________________________________________ 69

iv

6. Conclusions and Future Work _________________________________________ 70 6.1 Conclusions____________________________________________________________ 70 6.2 On the Horizon _________________________________________________________ 70 6.3 Future Work___________________________________________________________ 71

7. References _________________________________________________________ 72

Appendix A—Proofs ___________________________________________________ 73 Adaptive Identifier for Compass Calibration ___________________________________ 73

Vita _________________________________________________________________ 76

v

List of Figures Figure 1.1: TTFF After 5 Minutes.________________________________________________________ 3 Figure 1.2: TTFF After 10 Minutes._______________________________________________________ 3 Figure 1.3: Body-Fixed Coordinate Frame._________________________________________________ 4 Figure 1.4: Euler Angle Representation. ___________________________________________________ 5 Figure 1.5: MOD0, Fully Assembled. _____________________________________________________ 5 Figure 1.6: MOD0, Hull Removed. _______________________________________________________ 6 Figure 2.1: Relationship Between Earth's True Axis and Magnetic Axis. __________________________ 7 Figure 2.2: Traditional Baseplate Compass (courtesy of Helen Weil). ____________________________ 8 Figure 2.3: Magnetometer Output for No External Field Influences. ____________________________ 10 Figure 2.4: Magnetometer Output with Compass Misalignment (a), and Resulting Heading Error (b).__ 11 Figure 2.5: Magnetometer Output with Hard Iron Object Present (a), and Resulting Heading Error (b). 12 Figure 2.6: Magnetometer Output in the Presence of Soft Iron (a), and Corresponding Heading Error (b).___________________________________________________________________________________ 13 Figure 2.7: Rate Gryoscope. ____________________________________________________________ 14 Figure 3.1: Least Squares Gyro Calibration Simulation, Path 2. _______________________________ 19 Figure 3.2: Least Squares Gyro Calibration Simulation, Path 3. _______________________________ 20 Figure 3.3: Least Squares Gyro Calibration Simulation, Path 4. _______________________________ 21 Figure 3.4: Least Squares Gyro Calibration Simulation, Path 7. _______________________________ 22 Figure 3.5: Least Squares Gyro Calibration Simulation, Path 3 with Sensor Noise. ________________ 23 Figure 3.6: Adaptive Gyro Calibration Simulation, Path 2. ___________________________________ 28 Figure 3.7: Adaptive Gyro Calibration Simulation, Path 3. ___________________________________ 29 Figure 3.8: Adaptive Gyro Calibration Simulation, Path 4. ___________________________________ 30 Figure 3.9: Adaptive Gyro Calibration Simulation, Path 7. ___________________________________ 31 Figure 3.10: Adaptive Gyro Calibration Simulation, Path 3 with Sensor Noise.____________________ 32 Figure 3.11: Adaptive Gyro Calibration Simulation, Coefficient (top) and Heading Estimate Error Convergence.________________________________________________________________________ 33 Figure 4.1: Heading Error for A=0, B=5.0, C=-5.0, D=2.0, E=2.0 Degrees. _____________________ 38 Figure 4.2: Least Squares Compass Calibration Simulation, Path 2. ____________________________ 39 Figure 4.3: Least Squares Compass Calibration Simulation, Path 4. ____________________________ 40 Figure 4.4: Least Squares Compass Calibration Simulation, Path 7. ____________________________ 41 Figure 4.5: Least Squares Compass Calibration Simulation, Path 8. ____________________________ 42 Figure 4.6: Least Squares Compass Calibration Simulation, Path 8 with Sensor Noise. _____________ 43 Figure 4.7: Hard Iron Error Showing the Modeled Sinusoid. __________________________________ 46 Figure 4.8: Magnetometer-Based Calibration Simulation, No Errors Present. _____________________ 49 Figure 4.9: Magnetometer-Based Calibration Simulation, Hard Iron Error Present. ________________ 50 Figure 4.10: Magnetometer-Based Calibration Simulation, Soft Iron Error Present. ________________ 51 Figure 4.11: Magnetometer-Based Calibration Simulation, Hard and Soft Iron Errors Present. _______ 52 Figure 4.12: Magnetometer-Based Calibration Simulation, Hard and Soft Iron Errors Present with Magnetometer Sensor Noise Added. ______________________________________________________ 53 Figure 5.1: MOD0 Fully Assembled. _____________________________________________________ 55 Figure 5.2: MOD0 with Hull Removed. ___________________________________________________ 56 Figure 5.3: MOD0 Mechanical Sections (hull not shown). ____________________________________ 57 Figure 5.4: Cutaway Drawing of the Assembled Tail Section.__________________________________ 58 Figure 5.5: Tail Section Partially Disassembled (Fin and Propeller Shaft Housings Hidden). ________ 58 Figure 5.6: Pin Ready for Casting (left), Completed Fin (center), Rubber Mold (right). _____________ 60 Figure 5.7: Electronics Bay, Motor Installed. ______________________________________________ 60 Figure 5.8: Front Bulkhead Mounting Detail. ______________________________________________ 61 Figure 5.9: Nose Cone Ready for Installation.______________________________________________ 62 Figure 5.10: Electrical Control System Overview.___________________________________________ 63 Figure 5.11: Electronics Layout on MOD0.________________________________________________ 63 Figure 5.12: Propeller Shaft Design: Original (top) and Re-designed (bottom). ___________________ 68

vi

List of Tables Table 3.1: Least Squares Gyro Coefficient Estimation Results._________________________________ 24 Table 3.2: Adaptive Gyro Coefficient Estimation Results. _____________________________________ 34 Table 4.1: Least Squares Compass Coefficient Estimation Results. Heading Error Statistics Derived from Subplot (d) of the Simulations. __________________________________________________________ 44 Table 4.2: Hard and Soft Iron Effects Added for Magnetometer-Based Calibration. ________________ 48 Table 5.1: MOD0 General Specifications. _________________________________________________ 56 Table 5.2: Raw Materials Used in MOD0._________________________________________________ 57 Table 5.3: MOD0 Sensor Suite. _________________________________________________________ 65

vii

List of Symbols

x x-axis magnetometer reading

y y-axis magnetometer reading

z z-axis magnetometer reading

xh local level x-axis magnetometer reading

yh local level y-axis magnetometer reading

ψ true heading

ψm measured heading (uncalibrated)

X vector of x-axis magnetometer readings

Y vector of y-axis magnetometer readings

Z vector of z-axis magnetometer readings

Xh vector of local level x-axis magnetometer

readings

Yh vector of local level y-axis magnetometer

readings

θ pitch

φ roll

EH compass hard iron error

ES compass soft iron error

E compass error (includes misalignment, hard,

and soft iron errors)

RH compass hard iron error magnitude

RS compass soft iron error magnitude

δS compass hard iron error phase shift

δH compass soft iron error phase shift

A compass model misalignment coefficient

B,C compass model hard iron coefficients

D,E compass model soft iron coefficients

xhhard x-axis hard iron correction value

yhhard y-axis hard iron correction value

xhcal corrected x-axis magnetometer reading

yhcal corrected y-axis magnetometer reading

α angle from x-axis to ellipse major axis

uxx second order x-axis moment

uyy second order y-axis moment

uxy second order mixed moment

s ratio of ellipse major axis to minor axis

Xh’ vector of x-axis magnetometer readings,

rotated about the origin

Yh’ vector of y-axis magnetometer readings,

rotated about the origin

h sample period

ωx angular rate about the body fixed x-axis

ωy angular rate about the body fixed y-axis

ωz angular rate about the body fixed z-axis

ωm measured angular rate

a, a rate gyro scale factor

b,b rate gyro bias factor

r true yaw or heading rate

viii

1. Introduction Autonomous underwater vehicles (AUV’s) are unmanned, untethered vehicles that can

conduct missions with little supervision from a host. AUV’s are used in surveillance/

reconnaissance type missions including mine detection, harbor security systems, undersea

structure inspection, and water quality surveys. Military research is also underway for

developing AUV’s with strike capability [1]. Benefits of using these vehicles are human

safety, reduction in mission cost, and improved accuracy for repetitive and mundane

tasks.

Two disadvantages of the current state of commercial AUV’s are their cost and size.

Costs in the neighborhood of $100,000 are common and some of the smallest commercial

AUV’s measure 2 meters long and weigh over 80 kilograms, requiring special equipment

to deploy and recover these vehicles. Consider the advantage of using multiple, small,

low-cost AUV’s: At an estimated cost of $2000 each and a weight of 6 kilograms,

several of these small AUV’s could perform surveying tasks as a single large AUV would

normally do. In addition, the multiple small AUV’s could cover the same area faster, or

could cover the same area with theoretically less energy (hydrodynamic drag is inversely

proportional to the square of vehicle speed). Additionally, losing one of a handful of

small cheap AUV’s is easier to justify than losing one large expensive AUV. This is the

motivation for design and construction of a low-cost miniature AUV and the secondary

topic of this thesis.

Reducing the size and cost of the AUV also reduces the quality of the equipment that

can be put into it. Selection of a navigation sensor suite becomes a problem, which is

arguably the most important set of sensors for a device thrown in the water and expected

to come back on its own. One way to solve the problem of accurate navigation is by

using a global positioning system (GPS) receiver. These devices have become small and

cheap enough to be integrated into the AUV and could be used to constantly receive

1

Introduction 2

position information. Unfortunately, the GPS satellite signal attenuates quickly when it

hits the surface of the water, making GPS readings possible only when the AUV is

surfaced.

A special network of beacons has been developed to solve this problem. The beacons,

or buoys, are placed in the work area with the AUV knowing their position. An acoustic

modem in the beacon listens for a ‘ping’ from the AUV and returns the ping once

received (each beacon has a unique frequency). The AUV uses the sound wave’s time-

of-flight (time from when the ping was sent till the ping was received) to calculate the

distance from each beacon and to triangulate the AUV position. Adding these

components does not keep with the idea of developing a low-cost and simple to deploy

AUV and is not considered for the ASCL’s first AUV.

A more graceful solution is to use dead reckoning to navigate underwater. Dead

reckoning is the process of measuring vehicle attitude (roll, pitch, yaw) and velocity

(forward, lateral, and water body currents) and integrating these measurements over time

from an initial position to determine the current position. Attitude and velocity

measurement errors will cause the integrated position estimate to deviate from the true

position over time. To correct for this error, the AUV periodically swims to the surface

to obtain a GPS position fix, resetting the dead reckoned position to GPS-referenced

position. The AUV then returns underwater and continues the mission.

Two important factors must be considered for this method to be effective: time-to-first-

fix and accuracy of the dead reckoning sensors. Time-to-first-fix (TTFF) is the amount

of time taken for the GPS receiver to obtain a position fix once the GPS antenna surfaces.

With a long TTFF, the AUV spends too much time on the surface waiting for a position

update, taking time away from the mission. Initial tests were performed to get an idea for

the relationship of time between fixes and TTFF. The GPS antenna is submerged for a

period of time, brought to the surface, and TTFF is recorded. Figure 1.1 and Figure 1.2

show histograms of the results for submerged times of 5 and 10 minutes, during clear

weather conditions. While not a conclusive test, these results suggest that acquiring

Introduction 3

periodic GPS fixes in this manner will be an appropriate means for correcting dead

reckoned position for shallow-water missions.

0

1

2

3

4

5

1 2 3 4 5 6 7 8 9 10 11 12 13 14time to fix (seconds)

occu

ranc

es

Figure 1.1: TTFF After 5 Minutes.

0

1

2

3

4

5

1 3 5 7 9 11 13 15 17 19 21 23 25 27 29time to fix (seconds)

occu

ranc

es

Figure 1.2: TTFF After 10 Minutes.

The second factor, accuracy of the dead reckoning sensors, directly affects the

allowable time between GPS fixes. With less expensive/less accurate sensors, integrated

vehicle position error could grow faster and require more frequent GPS updates than

higher accuracy sensors; leading to the same problem of spending too much time

resetting the dead reckoned position. However, if automatic sensor calibration

techniques are employed, less accurate sensors could be used for extended periods of

time. Additionally, in-water sensor calibrations would allow the AUV to be recalibrated

at the start of and during a mission, making it possible to account for local environmental

factors affecting the dead reckoning sensors.

The use of commercial off-the-shelf (COTS) sensors which take care of signal

conditioning and analog to digital conversion is preferred whenever possible. In some

cases, the COTS devices contain built-in calibration routines which can be integrated into

Introduction 4

an automatic calibration system. However, a lack in built-in calibration performance and

a desire to package the sensor using custom circuitry (due to size constraints) leaves the

calibration problem unanswered. This is the motivation for development of automatic

sensor calibration algorithms and is the primary goal of this thesis. For the scope of this

thesis only calibration of heading and heading rate sensors will be addressed.

This thesis begins with an introduction into the characteristics of the magnetic compass

and gyroscopic heading rate sensor in Chapter 2. Chapters 3 and 4 show several

algorithms developed for calibration of the compass and heading rate sensors; supported

by computer simulations. Chapter 5 presents the design, construction, and problems

faced with ASCL’s first AUV, MOD0. Chapter 6 summarizes the thesis and discusses

future work to be considered. Though all variables are explained in the text, the reader

may find the List of Symbols section on page vii a handy reference.

Figure 1.3 shows a drawing of the AUV with the body-fixed coordinate frame used

throughout this thesis (referred to as the North-East-Down convention). Figure 1.4 shows

how the orientation of the vehicle (in this case a rectangular block) is represented. The

vehicle orientation ( roll, pitch, yawφ θ ψ= = = ) is measured from a local-level

coordinate frame, where +zlocal points in the same direction as Earth’s gravity vector.

y

z

x

Figure 1.3: Body-Fixed Coordinate Frame.

Introduction 5

ψ

φθ

zlocal

xlocal

ylocal

xbody

ybody

zbody

Figure 1.4: Euler Angle Representation.

Though the author was unable to test the calibration algorithms developed (due to data

collection issues discussed in Chapter 5), the thesis was successful in that the calibration

algorithms show promise for easy implementation and the construction of MOD0

resulted in a functional AUV. Figure 1.5 and Figure 1.6 show the vehicle fully

assembled and with the hull removed. MOD0, even with its limited run-time, has

provided the ASCL team with lessons learned about AUV design and deployment. These

lessons are being leveraged to develop the next ASCL vehicle; which will be smaller,

more powerful, and easier to use and modify.

Figure 1.5: MOD0, Fully Assembled.

Introduction 6

Figure 1.6: MOD0, Hull Removed.

2. Compass & Heading Rate Gyroscope Characteristics

The compass and heading rate gyroscope are very important components of a dead-

reckoning navigation system. The compass is a device that senses the direction in which

the vehicle is heading. The heading rate gyroscope is a device that senses the rate at

which the vehicle changes heading. While each device has its own inherent

vulnerabilities, techniques have been developed to correct for these problems and provide

for a stable method of navigation [2]. This chapter introduces the compass and gyro, and

the errors associated with each sensor.

2.1 The Magnetic Compass One of the older and more common methods of finding direction is based on sensing

the magnetic field of the earth. Though there are multiple magnetic fields, one field

dominates all others and can be likened to that of a dipole magnet, shown in Figure 2.1.

The magnetic North and South poles approximately lie in an axis that intersects but does

not coincide with the earth’s true North and South poles (the axis of rotation).

Figure 2.1: Relationship Between Earth's True Axis and Magnetic Axis.

7

Compass & Heading Rate Gyroscope Characteristics 8

The baseplate compass shown in Figure 2.2 is the one of the most commonly used

compasses for personal navigation. The dark (white) end of the needle is polarized to the

South (North) pole, thus making it attracted to and pointing toward earth’s magnetic

North (South) pole. As magnetic North (South) does not coincide with geographic North

(South) pole, care must be taken to account for difference in these two poles (declination)

when determining heading.

Figure 2.2: Traditional Baseplate Compass (courtesy of Helen Weil).

The electronic compass consists of inductor coils (magnetometers) that measure the

earth’s magnetic field and calculate vehicle heading from these measurements. A 2-

dimensional electronic compass senses heading via two orthogonally placed

magnetometers. The heading, ψ, can then be found

1tan yx

ψ − =

(2.1)

where x and y are the field strengths measured by the compass in the x and y axes,

respectively. With the baseplate and the 2-dimensional electronic compass, both must be

held level to the earth’s surface for an accurate measure of the magnetic field. In cases

where the compass is fixed to a pitching and rolling vehicle, it is usually mechanically


gimballed to the vehicle frame. This gimballing creates calibration problems because the

vehicle structure can move with respect to the compass needle or magnetometers. The

structure (if ferrous) causes local distortions to the earth’s magnetic field, which vary as

the vehicle pitches and rolls. The reaction to ferrous material will be discussed in greater

detail later this chapter.

The 3-dimensional electronic compass eliminates the need for mechanical gimballing

and associated calibration problems. This compass uses three orthogonally placed

magnetometers to measure the magnetic field. With the compass “strapped down”, it

requires vehicle pitch and roll measurements to compute heading. Using this

information, the 3-dimensional magnetic field vector can be translated into a 2-

dimensional local level magnetic field vector

cos sin sin cos sin

0 cos sin

xxh

yyh

z

θ φ θ φ θφ φ

− =

(2.2)

where x, y, and z are the field strengths measured by the compass in the x, y, z axes, xh

and yh are the field strengths in the local level plane, θ is the vehicle pitch, and φ is the

vehicle roll. Heading can then be determined using (2.1), replacing x and y with xh and

yh, respectively. Provided there are no measurement errors, Figure 2.3 shows a plot of

the local level magnetic field as the compass is swept through 360˚ (see Chapter 1 for

coordinate frame definitions). The compass rose indicates true heading. Triangle points

refer to the field measurement when the vehicle is pointing in the respective cardinal

direction. For Figure 2.3 these triangles line up properly, but as errors are introduced

they will shift.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

N

S

EW

Figure 2.3: Magnetometer Output for No External Field Influences.

2.2 Compass Error Sources Heading accuracy is compromised from various electrical and environmental sources:

A/D converter resolution, temperature changes, pitch/roll sensor errors, magnetic sensor

errors, magnetic field variation, compass misalignment, hard iron field distortion, and

soft iron field distortion [3]. Of these errors, the last three are addressed in this thesis and

are discussed below.

2.2.1. Misalignment Misalignment of the compass with respect to the vehicle frame simply results in a

constant error. This type of error can also be attributed to an inaccurate estimation of the

current declination [3], [4]. Figure 2.4(a) shows the magnetic field plot with this error

introduced. A plot of the heading error as a function of true heading in Figure 2.4(b)

shows the constant bias.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

N

S

EW

0 90 180 270 360-15

-10

-5

0

5

10

15

True Heading (degrees)

Hea

ding

Err

or (d

egre

es)

(a) (b)

Figure 2.4: Magnetometer Output with Compass Misalignment (a), and Resulting Heading Error (b).

2.2.2. Hard Iron Distortion The presence of a magnetized object (an object that is fixed on the vehicle frame)

creates a constant magnetic bias on the compass magnetometers. This causes the earth’s

magnetic field to become locally shifted from its true North-South orientation [3], [4].

Figure 2.5(a) shows that when the compass measures this biased field, the center point of

the circle shifts and causes the sinusoidal heading error displayed in Figure 2.5(b). A

function for the hard iron error, EH, can be estimated

( ) sin( )H m H m HE Rψ ψ δ= + (2.3)

where ψm is the measured compass heading and RH, δH are the hard iron error signal

amplitude and phase shift, respectively. After some trigonometric substitutions, (2.3) can

be rewritten in a form easier to work with

( ) sin cosH m mE B C mψ ψ ψ= + (2.4)


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

N

S

EW

0 90 180 270 360-15

-10

-5

0

5

10

15


Hea

ding

Err

or (d

egre

es)

(a) (b)

Figure 2.5: Magnetometer Output with Hard Iron Object Present (a), and Resulting Heading Error (b).

2.2.3. Soft Iron Distortion The presence of a non-magnetized ferrous object (an object that is fixed on the vehicle

frame) creates a heading-dependant magnetic bias on the compass magnetometers [3],

[4]. When the compass measures this distorted field it results in the magnetometer plot of

Figure 2.6(a), indicating a distortion of the circle as the compass sweeps through 360˚.

Figure 2.6(b) shows the corresponding heading error. A function for the soft iron, ES is

written

( ) sin(2S m S m SE R )ψ ψ δ= + (2.5)

where RS and δS are the soft iron error signal amplitude and phase shift, respectively.

Rewriting (2.5) in a similar form as (2.4)

( ) sin 2 cos 2S m m mE D Eψ ψ ψ= + (2.6)


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

N

S

EW

0 90 180 270 360-15

-10

-5

0

5

10

15


Hea

ding

Err

or (d

egre

es)

(a) (b)

Figure 2.6: Magnetometer Output in the Presence of Soft Iron (a), and Corresponding Heading Error (b).

A complete equation modeling the misalignment, hard iron, and soft iron errors can

now be written

( ) sin cos sin 2 cos 2m m m mE A B C D E mψ ψ ψ ψ= + + + + ψ

m

(2.7)

where A represents the compass misalignment error. True vehicle heading, ψ, can now

be determined

( )m Eψ ψ ψ= − (2.8)

2.3 The Heading Rate Gyroscope The rotating disk gyroscope, shown in Figure 2.7, can be used measure rate of change

of heading. Assuming that the inner gray disk with inertia I spins at a constant rate ω , an

applied angular velocity INω on the input axis causes a resulting torque T on the

output axis, satisfying the equation:

OUT

OUT INT Iω ω= × [5]. If the axis corresponding to INω

is aligned with the yaw axis of the vehicle, yaw rates can be measured by sensing T .

A similar gyroscope design is to replace the rotating disk with an oscillating mass that

moves along the same axis as the rotating disk. Again,

OUT

INω is applied and T sensed, OUT


but now will oscillate (at the same frequency as the moving mass) with the

magnitude of oscillations corresponding to the amount of yaw rate present.

OUTT

IωOUTT

INω

Figure 2.7: Rate Gryoscope.

As in the 3D electronic compass, this sensor is strapped down to the frame of the AUV.

Therefore, the angular rate measured by the rate gyro must be transformed to represent

the heading rate (which is a locally level measurement). This transformation is

represented by

sin coscos cosyr z

φ φω ωθ θ

= + (2.9)

where r is the yaw rate, ωy is the body-fixed y-axis angular rate, ωz is the body-fixed z-

axis angular rate, and φ , θ are the AUV roll, pitch, respectively [5]. When the center of

buoyancy is positioned directly above the center of gravity, the vehicle becomes

passively stable in roll and pitch. Assuming 0φ ≈ simplifies (2.9) to


coscos zr φ ω

θ= (2.10)

2.4 Rate Gyro Error Sources Major error sources of a rate gyro measurement include A/D converter resolution,

temperature changes, electromagnetic interference, scale, and bias errors. Only the scale

and bias errors are considered for the scope of this thesis. A model for the rate gyro

including these errors is

(1 )m a bω ω= + + (2.11)

where ω is the true angular rate, ωm is the measured angular rate, a is the scale factor, and

b is the angular rate bias factor. The model is rewritten to express the true angular rate

maω ω b= + (2.12)

where

1 ,1 1

ba ba a

−= =

+ +

Combining (2.12) with the heading rate translation in (2.10) results in the following

model for heading rate

coscos zmr a bφ ω

θ = + (2.13)

where r is the true heading rate and ωzm is the measured, body-fixed z-axis angular rate.

3. Rate Gyroscope Calibration This chapter presents previous work and a new algorithm to calibrate the rate gyroscope

sensor. For all work discussed in this chapter, it is assumed that the compass, pitch, and

roll sensors are providing accurate responses. The yaw rate model presented in Chapter 2

is restated

coscos zmr a bφ ω

θ = + (3.1)

where r is the true yaw rate, ωzm is the measured, body-fixed, z-axis angular rate, φ is the

AUV roll, θ is the AUV pitch, and the rate gyro scale factor a and bias factor b are

contained in

1 ,1 1

ba ba a

−= =

+ +

3.1 Least Squares Algorithm The least squares algorithm presented in [2] is introduced to find the calibration

coefficients , ba of (3.1). Consider the discrete-time state equation relating heading and

yaw rate

( )cos ( )( 1) ( ) ( ) , ( ) ,cos ( )

omz

kk k h a k b k kk

0φψ ψ ω ψθ

ψ+ = + + = ≥ (3.2)

where ψ(k+1) is the future heading, ψ(k) is the initial heading, and h is the sample

period. For any future index n k , (3.2) is rewritten 0> ≥

1 1k n k n icos ( ) cos ( )( ) ( ) ( )cos ( ) cos ( )mz

i k i k

ik n k ah i bhi i

ψ ψ ωθ θ= =

+ = + +∑ ∑+ − + −

(3.3) φ φ

Taking a number of true heading readings from the compass (the ψ(k+n) and ψ(k) terms)

and measurements from the rate gyro for increasing n and stacking the equations yields

the matrix equation:

16

Rate Gyroscope Calibration 17

z Gq= (3.4)

where

( 1) ( )( 2) ( )

( ) ( )

k kk k

z

k n k

ψ ψψ ψ

ψ ψ

+ − + − = + −

a

qb

=

[ ]1 2G G G=

and

1

1

1

cos ( ) ( )cos ( )

cos ( ) ( )cos ( )

cos ( ) ( )cos ( )

mz

k

mzi k

n k

mzi k

kh kk

ih iiG

ih ii

φ ωθφ ωθ

φ ωθ

+

=

+ −

=

=

∑

∑

1

2

1

cos ( )cos ( )

cos ( )cos ( )

cos ( )cos ( )

k

i k

n k

i k

khk

ihiG

ihi

φθφθ

φθ

+

=

+ −

=

=

∑

∑

Now a least squares estimate of the scale and bias coefficients can be found by solving

(3.4) for q


(3.5) 1( )Tq G G G−= T z

The vector q can be found as long as G is nonsingular, meaning that the AUV should

be changing heading rate during the calibration.

TG

To evaluate the performance of this algorithm, a Matlab-based unmanned underwater

vehicle (UUV) program created by Daniel H. Harris of the University of Maryland is

used to generate sample data. The program simulates vehicle dynamics in 6 degrees of

freedom using hydrodynamic information scaled for an AUV shape and size similar to

MOD0. Heading and gyro data for eight different trajectories are simulated. For all

simulations, propeller thrust is held constant and the AUV fin angles are commanded to

run a (mostly) planar path. A Matlab program is then run, which introduces scale and

bias coefficients to the z-axis rate gyro and then implements the least square algorithm to

identify the coefficients. Figure 3.1 through Figure 3.4 shows four of the eight results.

For all eight simulations, gyro scale and bias factors were set to 0.1a = − rad/sec/volt and

rad/sec, corresponding to coefficient values of 0.1b = 1.111a = , .1111

0.001

b = − . Though a

detailed analysis of the noise characteristics for the pitch, roll, gyro, and compass sensors

was not performed, noise values are included in the simulation for Figure 3.5 to show that

the algorithm converges in the presence of noise. A zero mean, normally distributed

random number set is added to each sensor to simulate noise, with σ = for the

pitch and roll sensors, 0.004σ = for the compass sensor, and 0.005σ = for the rate

gyro.


-5 0 5 100

2

4

6

8

10

12

14

16

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)

truecorruptedcalibrated

(c) Rate Gyro Response (true and calibrated responses are indistinguishable)

0 10 20 30 40 50-4

-2

0

2

4

6

8

head

ing

(rad

ians

)

time (seconds)

trueestimate

(e) True Heading and Integrated/Calibrated Gyro Heading Estimate (indistinguishable on

this plot)

0 10 20 30 40 50-4

-2

0

2

4

6

8

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw

(b) AUV Euler Angles

0 10 20 30 40 50-4

-3

-2

-1

0

1

2

3

4x 10

-3

angu

lar r

ate

erro

r (ra

dian

s/se

c)

time (seconds) (d) Rate Gyro Response Error (true minus calibrated response)

0 10 20 30 40 50-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

head

ing

erro

r (ra

dian

s)

time (seconds) (f) Heading Error Response (true minus gyro estimated)

Figure 3.1: Least Squares Gyro Calibration Simulation, Path 2.


-2 0 2 4 6 8 10 12-2

0

2

4

6

8

10

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

head

ing

(rad

ians

)

time (seconds)

trueestimate


this plot)

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-3

-2

-1

0

1

2

3

4x 10

-3

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

head

ing

erro

r (ra

dian

s)




0 5 10 15 20 25 30

-10

-5

0

5

10

X (m

eter

s)

Y (meters)

start

stopN

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

head

ing

(rad

ians

)

time (seconds)

trueestimate


this plot)

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-4

-3

-2

-1

0

1

2

3

4x 10

-3

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

head

ing

erro

r (ra

dian

s)




0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

X (m

eter

s)

Y (meters)

start stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)


(c) Rate Gyro Response.

0 10 20 30 40 500

5

10

15

20

25

30

35

40

head

ing

(rad

ians

)

time (seconds)

trueestimate


this plot)

0 10 20 30 40 50-5

0

5

10

15

20

25

30

35

40

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-0.035

-0.03

-0.025

-0.02

-0.015

-0.01

-0.005

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-10

-8

-6

-4

-2

0

2

4

6x 10

-3

head

ing

erro

r (ra

dian

s)




-2 0 2 4 6 8 10 12-2

0

2

4

6

8

10

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

head

ing

(rad

ians

)

time (seconds)

trueestimate


this plot)

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

0.05

head

ing

erro

r (ra

dian

s)


Figure 3.5: Least Squares Gyro Calibration Simulation, Path 3 with Sensor Noise.


Table 3.1 below shows the least squares gyro coefficient estimations for the eight

different trajectories. The trajectory of paths 2, 3, and 4 provided the greatest amount of

excitation to the rate gyro signal and thus provided the best estimates. Similarly, paths 1,

7, and 8 resulted in the worst coefficient estimation due to their more static heading rate.

Path

1 1.0897 -0.1017 -1.93% 8.46%2 1.1165 -0.1119 0.49% -0.72%3 1.1163 -0.1118 0.47% -0.63%4 1.1183 -0.1121 0.65% -0.90%5 1.1518 -0.1149 3.66% -3.42%6 1.1404 -0.1139 2.64% -2.52%7 1.0733 -0.0729 -3.40% 34.38%8 1.0880 -0.1060 -2.08% 4.59%

Coef. Estimate % Error from Truea b a b

Table 3.1: Least Squares Gyro Coefficient Estimation Results.

3.2 Adaptive Identifier Algorithm A Lyapunov-based adaptive identification algorithm developed in [6] to identify model

parameters of a remotely operated vehicle is modified to find the calibration coefficients

in (3.1). For the sake of clarity, we assume the AUV is level, θ = 0 and φ = 0.

Consider the state equation relating the modeled rate gyro to angular position

( ) ( ) ( )Tx t f t tβ= (3.6)

where the state x(t) is the vehicle heading and

( )

( ) , ( ) , ( ) 01t a

f t t tb

ωβ β

= =

=

ˆ( ) ( ) ( )

(3.7)

Estimate error equations are formed

x t x t x t= −

ˆ( ) ( ) ( )ˆ( ) ( ) ( )

ˆ( ) ( )

x t x t x t

t t

t t

tβ β β

β β

= −

= −

=

(3.8)


where ˆ( )x t and ˆ( )tβ are estimations of ( )x t and ( )tβ . Consider the following

Lyapunov function candidate

11 1( , ) ( ) ( ) ( ) ( )2 2

T Tm x x t x t t tβ β β−= + Λ (3.9)

where is positive definite and 2 2R ×Λ∈ ( , ) 0 ( ), ( )m x x t tβ β≥ ∀ . Showing that the time

derivative of (3.9) is negative semi-definite proves that the state estimate error and the

coefficient estimate errors ,x β are bounded. The time derivative of (3.9) is computed:

1 1

1

1

1

1 1( , ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )2 21 12 ( ) ( ) 2 ( ) ( )2 2

( ) ( ) ( ) ( )

ˆ( ) ( ) ( ) ( )

T T T T

T T

T T

T T

m x x t x t x t x t t t t t

x t x t t t

x t x t t t

x t x t t t

β β β

β β

β β

β β

− −

−

−

−

β β = + + Λ + Λ

= + Λ

= + Λ

= + Λ

(3.10)

Consider the following candidates for ˆ( )tβ and ˆ( )x t

(3.11) ˆ( ) ( ) ( )t f t xβ = −Λ t

ˆT ˆ( ) ( ) ( ) ( )x t Kx t f t tβ= + (3.12)

where . Using (3.11) and (3.12) to compute R, 0K K∈ < ( )x t

ˆ( ) ( ) ( )ˆ( ) ( ) ( ) ( ) ( )

ˆ( ) ( ) ( ) ( )

( ) ( ) ( )

T T

T

T

x t x t x t

Kx t f t t f t t

Kx t f t t t

Kx t f t t

β β

β β

β

= −

= + −

= + − = +

(3.13)

Substitution of (3.11) and (3.13) into (3.10) shows that (3.10) is negative semi-definite


1( , ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( )

T T T

T T T T

T T T T T

T

m x x t Kx t f t t t f t x t

Kx t x t x t f t t t f t x t

Kx t x t x t f t t x t f t tKx t x t

β β β

β β

β β

− = + + Λ −Λ = + −

= + −

=

(3.14)

Knowing that the time derivative of the Lyapunov function is negative semi-definite,

the following are true:

1. Given that ( )tβ is constant and ( )tβ bounded (from the Lyapunov equation), ˆ( )tβ

is also bounded.

2. From knowledge of the overall system, we can state that ( )x t (heading) and ( )tω

(heading rate) are bounded. It follows from (3.6) that ( )x t is also bounded.

3. With ( )x t bounded from the Lyapunov equation, ˆ( )x t is also bounded.

4. Because all parameters on the right hand side of (3.12) are bounded, ˆ( )x t is

bounded. It follows from 2 that ( )x t is bounded.

5. With ( )x t and ( )x t bounded, then it must follow that lim ( ) 0t

x t→∞

= .

6. From (3.11) and the previous step, lim ( ) 0t

tβ→∞

= is true.

To conclude, all components are bounded and the state identification error, ( )x t , and

time derivative of the coefficient estimation error, ( )tβ , converge asymptotically to zero.

Still, the coefficient estimation error, ( )tβ

(t

, is only shown to be bounded and not

converging to zero. Based on previous work with this adaptive identifier [6] and on the

simulations that follow, the coefficient estimation error does converge asymptotically to

zero in practice. Additional analysis involving a suitable definition of “persistence of

excitation” might help in showing that )β converges to zero but was pursued herein.

The same trajectory data from the least squares simulation is used for evaluating this

algorithm. The coefficient estimate, (3.11), and state estimate, (3.12) (with roll and pitch

included), are written in discrete form as


(3.15) ˆ ˆ ˆ( 1) ( ) ( ) ( ), ( ) ,ok k h f k x k k kβ β β β+ = − Λ = ≥ 0

cos ( ) 0ok ˆˆ ˆ ˆ( 1) ( ) ( ) ( ) ( ) , ( ) ,cos ( )

Tx k x k h Kx k f k k x k x kk

φ βθ

+ = + + = ≥ (3.16)

where h is the sample period, φ is the AUV roll, θ is the AUV pitch, and

ˆ( ) ( ) ( )ˆ ( )ˆ( ) ,ˆ ( )

( )( )

1zm

x k x k k

a k ak

bb k

kf k

ψ

β β

ω

= −

= =

=

with ( )kψ the current sample from the compass and ( )zm kω the current sample from the

z-axis rate gyro. The initial value for x is set to the initial compass heading, and initial

values for coefficients are arbitrarily set to 1a b= = . Choosing constants and K Λ in

(3.15) and (3.16) to provide the fast coefficient convergence is largely a trial and error

method [6]. It was found that setting the constants to

2 0

0.75,0 0.2

K

= − Λ =

provided good results.

Figure 3.6 through Figure 3.9 shows four of the eight simulation results. For all

simulations gyro scale and bias factors were set to 0.1a = − rad/sec/volt and

rad/sec, corresponding to coefficient values of

0.1b =

1.111a = , .1111= −b . Figure 3.10

shows the adaptive algorithm’s stability in the presence of noise (using the same noise

levels as in the least squares simulation). Finally, Figure 3.11 shows the coefficient and

heading estimate error convergence for the five simulations in Figure 3.6 through Figure

3.10.


-5 0 5 100

2

4

6

8

10

12

14

16

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



this plot)

0 10 20 30 40 50-4

-2

0

2

4

6

8

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-0.015

-0.01

-0.005

0

0.005

0.01

0.015

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.1

-0.05

0

0.05

0.1

0.15

0.2

0.25

head

ing

erro

r (ra

dian

s)


Figure 3.6: Adaptive Gyro Calibration Simulation, Path 2.


-2 0 2 4 6 8 10 12-2

0

2

4

6

8

10

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 500

2

4

head

ing

(rad

ians

)

time (seconds)

trueestimate


this plot)

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-10

-8

-6

-4

-2

0

2

4

6

8x 10

-3

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

head

ing

erro

r (ra

dian

s)




0 5 10 15 20 25 30

-10

-5

0

5

10

X (m

eter

s)

Y (meters)

start

stop

(a) AUV Trajectory

0 10 20 30 40 50-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

head

ing

(rad

ians

)

time (seconds)

trueestimate

(e) True Heading and Integrated/Calibrated Gyro Heading Estimate

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-4

-2

0

2

4

6

8

10x 10

-3

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 500

0.05

0.1

0.15

0.2

head

ing

erro

r (ra

dian

s)




0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

X (m

eter

s)

Y (meters)

start stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)


(c) Rate Gyro Response

0 10 20 30 40 500

5

10

15

20

25

30

35

40

head

ing

(rad

ians

)

time (seconds)

trueestimate

(e) True Heading and Integrated/Calibrated Gyro Heading

0 10 20 30 40 50-5

0

5

10

15

20

25

30

35

40

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-0.8

-0.7

-0.6

-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.7

-0.6

-0.5

-0.4

-0.3

-0.2

-0.1

0

head

ing

erro

r (ra

dian

s)




-2 0 2 4 6 8 10 12-2

0

2

4

6

8

10

X (m

eter

s)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

angu

lar r

ate

(rad

ians

/sec

)

time (seconds)



0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

head

ing

(rad

ians

)

time (seconds)

trueestimate

(e) True Heading and Integrated/Calibrated Gyro Heading

0 10 20 30 40 50-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

Eule

r ang

les

(rad

ians

)

time (seconds)

rollpitchyaw


0 10 20 30 40 50-0.025

-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

angu

lar r

ate

erro

r (ra

dian

s/se

c)


0 10 20 30 40 50-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

head

ing

erro

r (ra

dian

s)


Figure 3.10: Adaptive Gyro Calibration Simulation, Path 3 with Sensor Noise.


0 10 20 30 40 50-0.5

0

0.5

1

1.5

a (r

ad/s

ec/v

olt),

b (r

ad/s

ec)

abarbbar

0 10 20 30 40 50-0.5

0

0.5

1

1.5

head

ing

erro

r (ra

dian

s)

time (seconds) (a) Path 2

0 10 20 30 40 50-0.5

0

0.5

1

1.5

a (r

ad/s

ec/v

olt),

b (r

ad/s

ec)

abarbbar

0 10 20 30 40 50-0.5

0

0.5

1

1.5

head

ing

erro

r (ra

dian

s)

time (seconds) (c) Path 4

0 10 20 30 40 50-0.5

0

0.5

1

1.5

a (r

ad/s

ec/v

olt),

b (r

ad/s

ec)

abarbbar

0 10 20 30 40 50-0.5

0

0.5

1

1.5

head

ing

erro

r (ra

dian

s)

time (seconds) (e) Path 3, with Sensor Noise

0 10 20 30 40 50-0.5

0

0.5

1

1.5

a (r

ad/s

ec/v

olt),

b (r

ad/s

ec)

abarbbar

0 10 20 30 40 50-0.5

0

0.5

1

1.5

head

ing

erro

r (ra

dian

s)

time (seconds) (b) Path 3

0 10 20 30 40 50-1

-0.5

0

0.5

1

a (r

ad/s

ec/v

olt),

b (r

ad/s

ec)abarbbar

0 10 20 30 40 50-0.5

0

0.5

1

head

ing

erro

r (ra

dian

s)

time (seconds) (d) Path 7

Figure 3.11: Adaptive Gyro Calibration Simulation, Coefficient (top) and Heading Estimate Error Convergence.


Table 3.2 below shows the adaptive gyro coefficient estimations for the eight different

trajectories. As seen in the least squares simulation, the trajectories of paths 2, 3, and 4

with higher excitation of the rate gyro signal provided better coefficient estimates. Paths

1, 7, and 8 resulted in worse coefficient estimations due to their more constant heading

rates.

Path

1 -0.3332 0.4871 -129.99% 538.43%2 1.0921 -0.1090 -1.71% 1.89%3 1.0973 -0.1089 -1.24% 1.98%4 1.1003 -0.1132 -0.97% -1.89%5 1.1888 -0.1142 6.99% -2.79%6 1.1534 -0.1121 3.81% -0.90%7 -0.0583 0.7867 -105.25% 808.10%8 -0.3777 0.2193 -133.99% 297.39%

Coef. Estimate % Error from Truea b a b

Table 3.2: Adaptive Gyro Coefficient Estimation Results.

The least squares calibration algorithm has shown to be slightly better in estimating the

coefficients for the rate gyroscope. Though better values for and K Λ may exist and

outperform the least squares method, the need for tuning these constants makes the least

squares algorithm a more attractive approach. With techniques established for calibration

of the heading rate, attention is now turned toward calibrating the compass using similar

techniques.

4. Compass Calibration This chapter presents previous work on compass calibration and algorithms developed

to calibrate the sensor. For all work discussed in this chapter, it is assumed that the gyro,

pitch, and roll sensor are accurately calibrated. For ease of reference, the compass

calibration model is restated

( )m E mψ ψ ψ= − (4.1)

where

( ) sin cos sin 2 cos 2m m m mE A B C D E mψ ψ ψ ψ= + + + + ψ (4.2)

4.1 Previous Work Reference [4] presented results calibrating the TCM2 compass aboard Woods Hole

Oceanographic Institute’s REMUS AUV. Their first approach was to compute the five

coefficients in (4.2) by orienting REMUS in 8 directions (N, NE, E, SE, S, SW, W, and

NW), recording the error at each direction. Then simple averaging of the deviations

produced the coefficients. A second approach was to automate the TCM2’s built-in

calibration so the calibration could be performed in-water. Good results were reported,

but the TCM2 calibration lacked the ability to correct for soft-iron errors. A third and

final approach involved ignoring the use of (4.2) completely. Instead, paths were run

along those headings used in actual missions and the deviations from true heading

(determined using underwater acoustic nets, similar to the sonar beacons mentioned in

the introduction) were stored in memory. Then when the AUV is commanded to run

along one of those paths, it looks up the correction factor for that path. The premise for

this approach to be successful is that a mission will only need to accurately run along

specific headings.

4.2 Least Squares Algorithm A least squares algorithm is developed to find the parameters in (4.2) by modifying the

gyroscope calibration routine presented in Chapter 3. Consider the discrete-time state

equation relating heading and yaw rate

35

Compass Calibration 36

( 1) ( ) ( ), ( ) ,k k hr k k k (4.3) 0oψ ψ ψ ψ+ = + = ≥

( )

where ψ(1+k) is the integrated true heading, ψ(k) is the initial true heading, r(k) is the

true yaw rate, and h is the sample period. Substitution of (4.1) into (3.2) gives

( 1) ( 1) ( ) ( ) ( )m m m mk E k k E k hrψ ψ ψ ψ+ − + = − + ( ) k

[ ] ( ) ( )( ) ( 1) ( ) ( 1) ( )m m m mhr k k k E k E kψ ψ ψ ψ− + − = − + + (4.4)

For any future index , (4.4) is rewritten 0n k> ≥

[ ] ( ) (1

( ) ( ) ( ) ( ) ( )k n

m m m mi k

h r i k n k E k n E kψ ψ ψ ψ+ −

=

− + − = − + + ) ∑ (4.5)

Taking a number of true yaw rate readings and compass readings for increasing n and

stacking the equations yields the matrix equation

( )R Z Gq− = (4.6)

where

1

1

( )

( )

( )

k

i kk

i k

k n

i k

h r i

h r iR

h r i

=

+

=

+ −

=

=

∑

∑

∑

( 1) ( )( 2) ( )

( ) ( )

m m

m m

m m

k kk k

Z

k n k

ψ ψψ ψ

ψ ψ

+ − + − = + −


( ) ( )( ) (

( ) (

( 1) ( )( 2) ( )

( ) ( )

m m

m m

m m

E k E kE k E k

Gq

E k n E k

ψ ψψ ψ

ψ ψ

− + +

)

)

− + + = − + +

and

BC

qDE

=

1 2 3 4G G G G G= [ ]

1

sin ( 1) sin ( )sin ( 2) sin ( )

sin ( ) sin ( )

m m

m m

m m

k kk k

G

k n k

ψ ψψ ψ

ψ ψ

− + + − + + = − + +

2

cos ( 1) cos ( )cos ( 2) cos ( )

cos ( ) cos ( )

m m

m m

m m

k kk k

G

k n k

ψ ψψ ψ

ψ ψ

− + + − + + = − + +

3

sin 2 ( 1) sin 2 ( )sin 2 ( 2) sin 2 ( )

sin 2 ( ) sin 2 ( )

m m

m m

m m

k kk k

G

k n k

ψ ψ

ψ ψ

− + + = − + +

ψ ψ− + +

ψ ψ− + +

4

cos 2 ( 1) cos 2 ( )cos 2 ( 2) cos 2 ( )

cos 2 ( ) cos 2 ( )

m m

m m

m m

k kk k

G

k n k

ψ ψ

ψ ψ

− + + = − + +


Now a least squares estimation of the hard and soft iron error coefficients can be found

by solving (3.4) for q

( )1( )T Tq G G G R Z−= − (4.7)

The vector q can be found as long as G is nonsingular, meaning that the AUV must

be changing heading during the calibration. Because the misalignment coefficient A

doesn’t change with heading, it subtracts out in (4.4) and (3.4). However, this coefficient

can be found by running a straight-line path on the water surface and calculate true

heading from recorded GPS points. Compass and GPS derived headings can then be

compared and A determined.

TG

Simulation of this algorithm is performed using the same data sets used in the rate gyro

calibration. A Matlab program introduces hard and soft iron coefficients to the compass

data and then implements the least squares algorithm to identify the coefficients. Figure

4.2 through Figure 4.5 shows four of the eight results. For all eight simulations, the hard

and soft iron coefficients are set to 0, 5.0, 5.0, 2.0, 2.0A B C D E= = = − = = degrees.

Though other values for the coefficients were tried resulting in similar success, these are

selected for presentation based on experimental results in [4]. Figure 4.1 shows a plot of

this raw heading error. For a final measure of each simulation’s performance, subplot

(d) in Figure 4.2 through Figure 4.5 shows the residual heading error after calibration.

Additive sensor noise is included in the simulation for Figure 4.6 showing that the

algorithm remains stable in the presence of noise. The same noise levels from the gyro

calibration simulations are used in this simulation.

0 50 100 150 200 250 300 350 400-8

-6

-4

-2

0

2

4

6

8

10

head

ing

erro

r (de

gree

s)

heading (degrees) Figure 4.1: Heading Error for A=0, B=5.0, C=-5.0, D=2.0, E=2.0 Degrees.


-5 0 5 100

2

4

6

8

10

12

14

16X

(met

ers)

Y (meters)

start

stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 50-200

-100

0

100

200

300

400

500

head

ing

(deg

rees

)

time (seconds)

truemeasuredcalibrated

(c) True, Measured, and Calibrated Heading Response

0 10 20 30 40 50-200

-100

0

100

200

300

400

500

Eule

r ang

les

(deg

rees

)

time (seconds)

rollpitchyaw


0 50 100 150 200 250 300 350 400-1.5

-1

-0.5

0

0.5

1

1.5

head

ing

erro

r (de

gree

s)

heading (degrees) (d) Residual Heading Error after Calibration

Figure 4.2: Least Squares Compass Calibration Simulation, Path 2.


0 5 10 15 20 25 30

-10

-5

0

5

10

X (m

eter

s)

Y (meters)

start

stopN

S

EW

(a) AUV Trajectory

0 10 20 30 40 500

100

200

head

ing

(deg

rees

)

time (seconds)



0 10 20 30 40 50-50

0

50

100

150

200

Eule

r ang

les

(deg

rees

)

time (seconds)

rollpitchyaw


0 50 100 150 200 250 300 350 400-6

-5

-4

-3

-2

-1

0

1

2

3

head

ing

erro

r (de

gree

s)




0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

X (m

eter

s)

Y (meters)

start stop

N

S

EW

(a) AUV Trajectory

0 10 20 30 40 500

500

1000

1500

2000

2500

head

ing

(deg

rees

)

time (seconds)



0 10 20 30 40 50-500

0

500

1000

1500

2000

2500

Eule

r ang

les

(deg

rees

)

time (seconds)

rollpitchyaw


0 50 100 150 200 250 300 350 400-10

-5

0

5

10

15

head

ing

erro

r (de

gree

s)




0 2 4 6 8 10 12-4

-2

0

2

4

6

8X

(met

ers)

Y (meters)

start

stop

(a) AUV Trajectory

0 10 20 30 40 50-50

0

50

100

150

200

250

300

350

400

head

ing

(deg

rees

)

time (seconds)



0 10 20 30 40 50-50

0

50

100

150

200

250

300

350

400

Eule

r ang

les

(deg

rees

)

time (seconds)

rollpitchyaw


0 50 100 150 200 250 300 350 400-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

head

ing

erro

r (de

gree

s)




0 2 4 6 8 10 12-4

-2

0

2

4

6

8X

(met

ers)

Y (meters)

start

stop

(a) AUV Trajectory

0 10 20 30 40 500

50

100

150

200

250

300

350

400

head

ing

(deg

rees

)

time (seconds)



0 10 20 30 40 50-50

0

50

100

150

200

250

300

350

400

Eule

r ang

les

(deg

rees

)

time (seconds)

rollpitchyaw


0 50 100 150 200 250 300 350 400-1.5

-1

-0.5

0

0.5

1

1.5

head

ing

erro

r (de

gree

s)


Figure 4.6: Least Squares Compass Calibration Simulation, Path 8 with Sensor Noise.


Table 3.1 below shows the resulting coefficient estimations for the eight different

trajectories. The best results were obtained using paths 2 and 8. These trajectories

provide the compass with all 360 degrees of direction as well maintaining an equal

amount of samples from each direction. The poor performance in path 7 is due to an

unequal number of samples in all directions. Path 4 did not expose the compass to all

360 degrees and thus resulted in poor performance.

PathB C D E B C D E Mean Max Std. Dev.

1 5.1142 -4.3445 1.9421 2.2769 2.28% -13.11% -2.90% 13.85% 0.0689 1.8617 0.88312 4.6495 -5.4340 1.8485 2.0434 -7.01% 8.68% -7.58% 2.17% -0.0064 1.0727 0.59583 5.8101 -4.9039 2.0189 2.1420 16.20% -1.92% 0.94% 7.10% 0.0685 1.9642 0.79634 6.6407 -4.9079 1.8975 2.6514 32.81% -1.84% -5.13% 32.57% 0.1270 3.3002 1.40475 6.9860 -2.8462 0.5875 5.4550 39.72% -43.08% -70.63% 172.75% 0.3684 6.9324 3.63006 7.7090 -3.4602 3.7559 3.7009 54.18% -30.80% 87.80% 85.05% 0.2272 5.5741 3.01937 6.3478 1.1154 2.3701 7.5394 26.96% -122.31% 18.51% 276.97% 0.5424 12.2204 6.40648 5.0407 -4.9553 2.0288 1.7872 0.81% -0.89% 1.44% -10.64% 0.0187 0.8956 0.4312

Coefficient Estimate Error from TrueCoefficient Estimates (degrees) Heading Error Statistics (deg)

Table 4.1: Least Squares Compass Coefficient Estimation Results. Heading Error Statistics Derived from Subplot (d) of the Simulations.

A major problem with this method is the requirement of a pre-calibrated heading rate

gyro. The use of a bench-top rate gyro calibration would take away from the goal of a

fast on-site calibration. Additionally, the gyro’s inherent bias drift problems will require

that it be recalibrated more often than the compass. Thus, a compass calibration routine

that is not a function of rate gyro data is desired.

4.3 Adaptive Identifier Algorithm The same adaptive identifier used in the rate gyro calibration is modified for finding the

calibration coefficients in (4.2), without the use of gyro data. Consider the kinematical

relationship

( ) cos ( )( ) sin ( )

x t v ty t v t

ψψ

==

(4.8)


where v is the vehicle velocity. The Cartesian coordinates ( ), ( )x t y t are derived from

longitude and latitude GPS information. The +x axis corresponds to due north heading,

+y axis corresponds to due east heading, and a positive clockwise heading measurement

is used. Expanding (4.8), let

(4.9)

1sin ( )

( ) cos ( ) , ( ) , ( ) 0sin 2 ( )cos 2 ( )

m

m

m

m

At B

f t t t tCt Dt E

ψψ β βψψ

= =

=

then

( )( )

( ) cos ( ) ( )

( ) sin ( ) ( )m m

m m

x t v t E t

y t v t E t

ψ ψ

ψ ψ

= − = −

( ) cos ( ) ( ) ( )

( ) sin ( ) ( ) ( )

m

Tm

Tx t v t f t t

y t v t f t t

ψ β

ψ β

= − = −

(4.10)

The same approach in the adaptive algorithm for the rate gyro is used here, except that

problems arose in choosing an appropriate Lyapunov function ( , )m x β , state estimate

update function ˆ( )x t , and coefficient update function ˆ( )tβ . The major problem is due to

the coefficient terms in ( )tβ being contained in the sine and cosine functions in (4.10).

A detailed exercise of attempting to find ( , )m x β , ˆ( )tβ , and ˆ( )x t is shown in Appendix

A.

4.4 Magnetometer-Based Algorithm In the least squares and adaptive algorithms, the approach has been to compute

(uncalibrated) heading from the magnetometer information and then use (4.1) and (4.2) to

correct the heading. An approach is now considered where the raw magnetometer data is


corrected, eliminating the need for a model [3]. Figure 4.7 shows the same hard iron

error as in Figure 2.5b with the hard iron error modeled sinusoid, showing the distortion

present in the actual error signal. By correcting the magnetometer data itself, a more

accurate vehicle heading is expected.

0 90 180 270 360

-15

-10

-5

0

5

10

15


Hea

ding

Err

or (d

egre

es)

ActualModeled

Figure 4.7: Hard Iron Error Showing the Modeled Sinusoid.

Consider a local level magnetometer data set:

1 2

1 2

n

n

xh xh xhXhyh yh yhYh

=

(4.11).

A requirement for using this algorithm is that the vehicle swims one or more complete

circles, so that there is an equal representation of magnetometer readings in all directions

Hard iron error is first characterized by identifying the location of the center of the circle

(or ellipse if soft iron errors are present). This is simply done by taking the average of Xh

and Yh measurement sets

1

1

1

1

n

hard ii

n

hard ii

xh xn

yh yhn

=

=

=

=

h∑

∑ (4.12).


Now a hard-iron calibrated compass reading can be calculated

cal hard

cal hard

xh xhxhyh yhyh

= −

(4.13).

With the hard iron error subtracted out, soft errors are now characterized by the angle

of the major axis of the elliptical magnetometer plot (with respect to North) and the ratio

of the major axis to the minor axis lengths. The angle of the major axis of the ellipse, α,

corresponds to the least second moment of inertia [7]. This is found using the following

equation

1 21 tan2 2

xy

xx yy

uu u

πα −

= − −

(4.14),

where uxx, uyy, and uxy, are the second order x-axis, y-axis, and mixed moments,

respectively and α measured in a +CW manner with respect to the +xh axis. These

moments are computed

( )

( )

( )(

2

1

2

1

1

1

1

1

n

xx i hardin

yy i hardi

n

xy i hard i hardi

u xh xhn

u yh yhn

u xh xh yh yhn

=

=

=

= −

= −

= − −

∑

∑

∑ )

(4.15).

With the major axis angle known, the hard iron calibrated magnetometer data set is

rotated about the origin by the angle -α so that the major axis lies along the xh axis

' cos( ) sin( )' sin( ) cos( )

hard

hard

Xh xhXhYh yhYh

α αα α

−− − − = −− −

(4.16).

The ratio of the major to minor axis is then determined

max( ')max( ')

XhsYh

= (4.17).


With α and s known, the magnetometer reading can be calibrated by rotating the reading

by -α, scaling the yh-axis component by s, and then rotating the reading back to it’s

original orientation

(4.18) cos( ) sin( ) 1 0 cos( ) sin( )sin( ) cos( ) 0 sin( ) cos( )

cal hard

cal hard

xh xh xhyh yh yhs

α α α αα α α α

−− − − − = −− −

The simulation program used for least squares gyro/compass calibrations and for

adaptive gyro calibrations does not provide magnetometer data. The Matlab program

first creates a single circle with a nominal radius of 20µT (the approximate measurement

range of the compass used for MOD0), and center point, eccentricity, and major axis

angle specified. The program then implements the algorithm shown above to determine

these hard and soft iron effects and correct them. Table 4.2 shows the hard and soft iron

effects used in each simulation (Figure 4.8 through Figure 4.12), including simulation of

magnetometer sensor noise ( 0.1σ = µT). In the subplots (a) and (d) of the following

figures, the triangular heading markers indicate the measurement taken when the compass

is pointed in the true North, East, South, West directions.

Figure Xh Shift Yh Shift Eccen. Axis Angle(uT) (uT) (deg)

8 0.0 0.0 0.0 09 2.0 2.0 0.0 010 0.0 0.0 0.5 12011 2.0 2.0 0.5 12012* 2.0 -4.0 0.8 10

* noise added Table 4.2: Hard and Soft Iron Effects Added for Magnetometer-Based Calibration.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(a) Measured Magnetometer Plot;

-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(c) Calibrated Magnetometer Plot

0 50 100 150 200 250 300 350 400-3

-2

-1

0

1

2

3x 10

-14

head

ing

erro

r (de

gree

s)

heading (degrees) (b) Measured Heading Error (before calibration)

0 50 100 150 200 250 300 350 400-0.1

-0.05

0

0.05

0.1

head

ing

erro

r (de

gree

s)

heading (degrees) (d) Residual Heading Error (after calibration)

Figure 4.8: Magnetometer-Based Calibration Simulation, No Errors Present.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(a) Measured Magnetometer Plot (dotted circle shown to

emphasize the shift of the magnetometer data).

-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest


0 50 100 150 200 250 300 350 400-10

-5

0

5

10

head

ing

erro

r (de

gree

s)


0 50 100 150 200 250 300 350 400-0.1

-0.05

0

0.05

0.1

head

ing

erro

r (de

gree

s)


Figure 4.9: Magnetometer-Based Calibration Simulation, Hard Iron Error Present.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(a) Measured Magnetometer Plot (dotted circle shown to

emphasize the distortion of the magnetometer data)

-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest


0 50 100 150 200 250 300 350 400-5

0

5

head

ing

erro

r (de

gree

s)


0 50 100 150 200 250 300 350 400-0.1

-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

head

ing

erro

r (de

gree

s)


Figure 4.10: Magnetometer-Based Calibration Simulation, Soft Iron Error Present.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(a) Measured Magnetometer Plot (dotted circle shown to emphasize the shift and distortion of the magnetometer

data)

-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest


0 50 100 150 200 250 300 350 400-15

-10

-5

0

5

10

15

head

ing

erro

r (de

gree

s)


0 50 100 150 200 250 300 350 400-0.1

-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08he

adin

g er

ror (

degr

ees)


Figure 4.11: Magnetometer-Based Calibration Simulation, Hard and Soft Iron Errors Present.


-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest

(a) Measured Magnetometer Plot (dotted circle shown to emphasize the shift and distortion of the magnetometer

data)

-20 -10 0 10 20

-20

-10

0

10

20

Xh (u

T)

Yh (uT)

NorthSouthEastWest


0 50 100 150 200 250 300 350 400-40

-30

-20

-10

0

10

20

30

head

ing

erro

r (de

gree

s)


0 50 100 150 200 250 300 350 400-1.5

-1

-0.5

0

0.5

1

1.5he

adin

g er

ror (

degr

ees)


Figure 4.12: Magnetometer-Based Calibration Simulation, Hard and Soft Iron Errors Present with Magnetometer Sensor Noise Added.


The simulation results show a very repeatable residual heading error in subplot (d),

possibly due to a computational imprecision; even in Figure 4.8 when no errors were

present. Regardless, a worst-case error of ~0.08 degrees is considered by the author to be

very good.

Besides the advantage of avoiding errors from an approximated model, this method of

calibration only requires data from the compass (and pitch/roll sensors which are not

being considered in this thesis). As in the least-squares approach, a key factor in

collecting the magnetometer data for this calibration is that vehicle complete n number of

complete and constant rate revolutions, providing equal number of measurements from

each direction. To get a better idea how this algorithm compares with the least squares

method, the true heading values used in the least squares simulation paths should be used

to compute the corresponding magnetometer values. This would help determine how

much of an unbalanced data set would affect the calibration process. Though not

calibrated when the data is collected, the compass will provide repeatable readings; good

enough for detecting when a circle is completed. The disadvantage of this algorithm is

that (4.18) could be more computationally intensive (even after simplification) than (4.1)

and (4.2), but the importance of this is largely dependant on the computer platform used.

As in the least squares approach, compass misalignment is not accounted for and would

be determined using GPS information from a straight-line path. Overall, this compass

calibration approach shows promise of providing good calibration results.

5. AUV Design and Construction The first autonomous underwater vehicle created in the ASCL, MOD0, follows design

cues from the U.S. Naval Academy’s USNA-1 AUV. The USNA-1, built by Dr. Daniel

Stilwell and Dr. Carl Wick, demonstrated that the essential components of an AUV can

be housed in a 3-inch diameter hull [8]. USNA-1 is somewhat limited in its performance

and usefulness, with sensors onboard measuring heading, heading rate, and pressure.

Many lessons were learned from this vehicle, mainly in material selection, seal design,

weight distribution, and component placement.

MOD0, pictured in Figure 1.5 and Figure 1.6, improves on the USNA-1 design with

increased performance in navigation, communication, motion control, and modularity.

As with USNA-1, MOD0 is neutrally buoyant, meaning the vehicle neither sinks nor

floats when in the water. MOD0 is passively stable in pitch and roll; accomplished by

moving the center of buoyancy directly above the center of gravity. Table 5.1 shows the

general specifications of the vehicle. Because the vehicle is still in its infancy, mission

sensors such as conductivity, temperature, sonar, and oxygen sensors have not yet been

incorporated on the vehicle. The main objectives in creating MOD0 were that the vehicle

remain as small and inexpensive as possible and be easy to service and deploy.

Commercial-off-the-shelf (COTS) components, readily available construction materials,

and simple fabrication techniques are used whenever possible.

Figure 5.1: MOD0 Fully Assembled.

55

AUV Design and Construction 56

Figure 5.2: MOD0 with Hull Removed.

Parameter SpecificationLength 91 cmHull Diameter 8.9 cmMass ~5.3 kgStability passively stable in pitch and rollMotion Control, Propulsion brushless DC motor with encoder

feedback and custom driver to control shaft velocity

Motion Control, Steering three independantly actuated fins, driven by R/C servo motors

Computer control system Rabbit 3000 microprocessorPower 21.6 volt, 80 watt-hour NiMh

battery stackCommunications 57Kbps bidirectional, 60 miles in

air (with ideal antenna)Data storage 8MB FlashRange 17km (anticipated)Cruising speed 2-3 knots (1-1.5 m/s)Max depth 200 meters (anticipated)

Table 5.1: MOD0 General Specifications.

5.1 Mechanical Description The mechanical structure of MOD0, shown in Figure 5.3, consists of the tail section,

electronics bay, front bulkhead, hull (not shown), and nose cone. The main objective in

the mechanical design was to create an AUV structure easy to fabricate and assemble. A

list of raw materials used in MOD0 is shown in Table 5.2. Ferrous metals are avoided

because of their corrosion and the magnetic soft iron error they cause.


Front Bulkhead Propeller Fins (3)

Nose Cone Electronics Bay Tail Section

Figure 5.3: MOD0 Mechanical Sections (hull not shown).

Material Locations UsedGeneral Plastics Manufacturing Co. 3315 High Density Last-A-Foam

Tail Section

Alloy 6061 Aluminum Tail Section, Electronics Bay, Front Bulkhead

Type 303 Stainless Steel Propellor and Fin ShaftsType I PVC Plastic Nose ConeSchedule 40 PVC Pipe HullPolytek EasyFlo 60 Plastic Resin Fins

Table 5.2: Raw Materials Used in MOD0.

The entire vehicle structure is created using 3-D CAD software by the author to verify

design ideas. Though some minor modifications have been made during fabrication, the

AUV is built as initially intended. The author built the AUV in the ASCL and the ECE

machine shop. The ISE and AOE machine shops also provided assistance when needed.

The mechanical components are detailed below in order of their assembly.

5.1.1. Tail Section The AUV tail section allows the propeller shaft and fin shafts to penetrate to the vehicle

exterior. The tail also seals the rear of the vehicle to the hull and provides a mounting

point for the motors and electronics bay. Shown in Figure 5.4 and Figure 5.5, it is

created from high-density foam bonded to a piece of aluminum tubing. The foam is

chosen because of its light weight, ease of machining, and availability. Being less dense

than water, the foam helps to counter the weight of the propulsion motor at the rear of the

vehicle. After machining, the tail receives coats of epoxy resin to seal the rough foam

surface. The aluminum ring is added to provide a smooth surface on which to create the

hull seal o-ring groove. The ring is bonded to the foam using epoxy and then the o-ring


groove machined according to specifications in [9]. The ring also provides for a sturdy

point on which to mount the electronics bay.

Figure 5.4: Cutaway Drawing of the Assembled Tail Section.

Dogbone

Couplers(2)Electronics

Bay

Bearings(2)

Propeller

Lock Collar

O-rings(2)

Driveshaft

Nut

Fins(3)

Control Wires(3)

Servos(3)

O-ring

O-rings (2/shaft)

Fin Shafts(3)

Cranks(3)

Tail Section

Figure 5.5: Tail Section Partially Disassembled (Fin and Propeller Shaft Housings Hidden).


The tail section has four penetrations: the propeller shaft and three fin shafts. Each of

these stainless steel shafts slide into the tail section through aluminum housings bonded

to the tail section. The propeller shaft is suspended in its aluminum housing by two ball

bearings and holds two o-rings which rub against the housing to seal the penetration. An

RC (remotely controlled) airplane propeller is clamped to the propeller shaft between a

threaded lock collar and nut. On the inside of the tail section the propeller shaft connects

to the drive motor using aluminum couplers attached to the propeller and motor shafts,

and an RC “dog bone” shaft between the couplers. This type of coupling allows for easy

assembly and misalignment between the shafts. When the tail section mounts to the

electronics bay, the dog bone shaft is held captive between the couplers.

The three equally spaced fin shafts use two o-rings each to seal their penetrations.

Inside the tail section a crank arm is fastened to the fin shaft. Stainless steel wire (the

control wire) connects the fin shaft crank arm to the crank arm of an RC airplane servo

motor. It is important that the crank arms are the same length, and the control wire is the

same length as the distance between the fin and servo shafts. This creates a

parallelogram and ensures a 1:1 relationship between the change in the servo motor angle

and the fin angle, making calibration of the fins easier. The servos sit in slots cut in the

tail section and are secured via a foam ring, clamping them in place when the tail section

is mounted to the electronics bay. The cast fins mount to the fin shafts via a hole drilled

to receive the stainless steel pin in the base of the plastic fin. The pin is secured to the fin

shaft with a set screw.

The fins are made using a two-part liquid plastic resin. First, a rubber mold of the

master fin is cast. After curing, the master fin is removed and the liquid plastic is mixed

and poured into the mold, shown in Figure 5.6. The pin mentioned above is inserted and

the plastic is allowed to cure. Once cured, the fin is removed and trimmed as needed.


Figure 5.6: Pin Ready for Casting (left), Completed Fin (center), Rubber Mold (right).

5.1.2. Electronics Bay and Hull The electronics bay houses the propulsion motor, batteries, and all of the circuit boards

and sensors which must remain dry. Shown in Figure 5.7, the bay consists of two square

aluminum bars stock (referred to as “rails”) bolted to two aluminum rings. The drive

motor is fastened to the rear aluminum ring and then the electronics bay is fastened to the

tail section using machine screws. Batteries and electronics are attached to the rails using

machine screws and standoffs.

Figure 5.7: Electronics Bay, Motor Installed.

Once the electronics bay is attached to the tail section and circuitry is installed on the

bay, the hull is slid over the vehicle from the front. Standard 3” Schedule 40 PVC


(polyvinyl chloride) pipe makes up the AUV hull. Expansion of the vehicle payload is

possible by simply using longer rail sections and a longer piece of pipe.

5.1.3. Front Bulkhead and Nose Cone The front bulkhead and nose cone are the last pieces of hardware to install on the AUV

and first to be removed. Thus, they are designed to attach and detach easily. The front

bulkhead is an aluminum disc, machined to accept an o-ring which seals the bulkhead to

the hull. An 8-contact connector and pressure transducer penetrate through the bulkhead

and are sealed using o-rings and silicone sealant.

With the hull on the AUV, the front bulkhead slides down the hull about 25mm and

attaches to the front of the electronics bay via a “twist-lock” (bayonet) joint. Figure 5.8

shows the front bulkhead and the mating surface on the electronics bay. Two keyhole

slots in the electronics bay’s front ring line up with two screw heads in the front

bulkhead; when the bulkhead is rotated clockwise it secures to the electronics bay. A

removable installation handle makes this an easy task.

Bayonet Screw Heads(2)

Front Bulkhead

Electronics Bay

Pressure Sensor

O-ring

Bayonet Slots(2)

Electrical Connector

Figure 5.8: Front Bulkhead Mounting Detail.

The nose cone, shown in Figure 5.9, is the final component attached to the AUV. With

hydrodynamic efficiency not of primary importance, the nose profile is chosen (but not


engineered) to be smooth. Machined from PVC plastic, the inside of the nose cone is

hollowed to allow for mounting sensors requiring exposure to water, such as a sonar

altimeter. A pad machined on the top of the nose allows for mounting of the GPS

receiver antenna. The nose attaches to the hull via 3 friction-fit dowel pins.

Front Bulkhead

Pressure Sensor

Nose Cone

Hull GPS Antenna

Electrical Connector

Dowell Pins(3)

Figure 5.9: Nose Cone Ready for Installation.

5.2 Electrical Description The main goal of the electrical design is to build a system which uses as little space as

possible, yet will be easy to work with once the electronics bay is fully populated. A

block diagram of the vehicle electronics is shown in Figure 5.10, with gray blocks

indicating COTS components and white blocks indicating custom-engineered

components. The design and construction of the electronics has been a group effort by

the ASCL. Aditya Gadre is responsible for design of the AUV controller and inertial

measurement cards, with the author assisting him with AUV controller software. Aditya

is also the designer of a graphical user interface (GUI) which runs on a host PC. Dr.

Daniel Stilwell is responsible for design of the motor controller card, with Aditya and the

author assisting in the motor controller’s software. Chris Cannell is responsible for

design of the leak detector circuitry and overall system wiring. The author is responsible

for design of the power distribution card and overall system wiring.


7

Motors (Propulsion and Control Surfaces)

3TTL

8

10

3 TTL

Inertial Measurement Card

TTL 3

Motor Controller Card

TTL3

GPS

Programming Interface

RS2323

CompassAltimeter

TTL 3

AUV Controller Serial Bus

Analog Sensors (Gyros, Accelerometers, and Depth)

8MB FLASH Memory

Leak Detector

10 900MHz Radio Modem

AUV Controller Card

Figure 5.10: Electrical Control System Overview.

Motor Controller

Propeller Motor

Inertial Measurement Card

Power Distribution Card

Compass

Radio Modem

AUV Controller Card

Battery Stack

GPS

Figure 5.11: Electronics Layout on MOD0.

5.2.1. Control Electronics The AUV is controlled by a Rabbit 3000 8-bit microcontroller, running at 44.2 MHz.

The microprocessor resides on the AUV controller card. The card provides connection to

power, an 8MB FLASH memory module, and serial ports (the primary means of


communication on the AUV). The microprocessor has six built-in serial ports, with one

of the six multiplexed using custom circuitry to provide a 1 to 4 serial port expansion. In

addition, an 8-bit port and several control lines from the microprocessor are connected to

a UART (universal asynchronous receiver/transmitter) to provide two more serial ports,

making for 11 total serial ports. Two of these serial ports are at RS-232 levels and the

others are at TTL levels. Data from the Rabbit 3000 can be sent and received to a host

PC using a 900MHz radio modem connected to one of the serial ports.

Commands are sent from the microprocessor to the motor controller card to control the

vehicle motion. The card receives a serial command string (consisting of propulsion

motor speed and three fin positions) into a Microchip microcontroller and processes the

data. Movement of the three fin servo motors is accomplished by supplying a PWM

signal to the motor’s control line. The propulsion motor is a 24V, 40-watt brushless DC

motor and is driven using a 3 phase motor driver IC. The Microchip microcontroller

sends the driver a PWM and direction signal to control the motor speed. The motor’s

quadrature encoder interfaces with the microcontroller through a decoder IC and enables

a speed control routine to be implemented in software (a PD compensator is used). The

program software also rejects commands which are out of range of the fin’s physical

limitations and limits the acceleration of the propulsion motor.

5.2.2. Sensing Electronics The Rabbit microprocessor receives data from the sensors listed in Variable Sensor PrecisionHeading electronic compass 3 degreesAngular Rate rate-gyroscopes,

along each axis5 degrees/minute drift error

Pitch and Roll accelerometers .1 degreesDepth strain gauge

pressure sensorpassively stable in pitch and roll

Absolute Position

GPS

Leak Detection custom passive sensors

Table 5.3.


Variable Sensor PrecisionHeading electronic compass 3 degreesAngular Rate rate-gyroscopes,

along each axis5 degrees/minute drift error

Pitch and Roll accelerometers .1 degreesDepth strain gauge

pressure sensorpassively stable in pitch and roll

Absolute Position

GPS

Leak Detection custom passive sensors

Table 5.3: MOD0 Sensor Suite.

Angular rate about each of the three body-fixed axes is measured using two Gyration

MicroGyro dual-axis gyroscopes. Rotation rates are determined by sensing the Coriolis

accelerations that result from the rotation of vibrating masses inside the gyro. Pitch and

roll are sensed using Analog Devices accelerometers, positioned on each of the three

body-fixed axes. Each of the accelerometers measures a component of Earth’s gravity

vector, from which pitch and roll can be derived. Vehicle depth is measured using a

SenSym pressure sensor. The accelerometers, gyros, and pressure sensor provide analog

voltages which are conditioned and converted to digital signals on the custom designed

inertial measurement card. The card uses instrumentation amplifiers for signal

conditioning, a Maxim 12-bit, 8-channel ADC, and a Microchip microcontroller. The

microcontroller samples all signals at 2kHz, performs digital filtering, and sends the

signals to the Rabbit microprocessor over a serial link, at a rate of 15-20Hz.

Heading and GPS information is provided by COTS components. The compass used is

a Precision Navigation Inc. TCM2 3-axis electronic compass. It provides RS-232 output

of the three magnetic field vectors, as well as pitch, roll, temperature, and computed

heading. The TCM2 has built-in calibration programs that allow the user to calibrate for

on-board hard iron effects. A multi-point calibration uses Kalman filtering to provide

calibration [10]. It requires that the vehicle make at least two slow rotations while

providing as much pitch and roll input as possible. According to the manual, this

calibration routine does not require perfect circles to be made or that the pitch and roll

amounts be at any specific rate. The calibration routine ends by reporting a qualitative


score of the calibration, from which the user can decide whether to calibrate again.

Unfortunately, soft iron effects are not taken into account (and thus a better calibration

algorithm is desired).

When the AUV is surfaced a Motorola GPS receiver provides accurate positioning data.

The 12-channel receiver updates every second and provides data in a Motorola binary

format at 9600bps.

5.2.3. Power Electronics A battery pack of 4/3 A size NiMH cells makes up the AUV power source, providing

3.7Ah at 21.6V (80 Wh). The power distribution card supplies 12V, 5V, and 3.3V to the

AUV using small switching regulators. The card takes advantage of unused space above

the battery pack, thus not adding to vehicle length. A relay on the card switches on the

AUV power when the nose cone’s watertight connector is plugged into the front

bulkhead.

5.2.4. Software Development The Rabbit microcontroller is programmed using Z-World’s Dynamic C. Currently, the

AUV operates in a remote control mode. The program executes code sequentially,

setting up GPS and compass boards, listening for commands from the host, sending new

commands to the motor controller card, and then listening to each sensor for new data,

saving the data to FLASH memory if required by the host. A GUI is used to remotely

control the AUV and receive sensor data across a radio modem. In autonomous mode,

the microprocessor will run a control and data acquisition loop at the highest priority at

10 Hz. This loop will read and write sensor data to the FLASH memory and give

appropriate commands to the motor controller to move the vehicle along a programmed

trajectory. Less critical functions such as communicating with the host PC will run at

lower priorities.


5.3 Problems Encountered

5.3.1. Propeller Shaft Seal Design In designing the propeller shaft seal, the goal is to keep the seal’s friction against the

aluminum housing as low as possible while still maintaining a reliable seal. At full speed

and no load the motor and controller consumed 3.4 watts. Efforts to reduce friction in the

propeller shaft seal included enlarging the housing’s inside diameter to the limit

recommended in [9], switching from petroleum to silicone-based lubricant, and switching

from standard o-rings to Minnesota Rubber Quad-Rings. The standard o-ring has a

circular cross sectional profile, whereas the Quad Ring has an X-shaped cross section that

is supposed to reduce surface contact, reducing seal friction. With these modifications,

power consumption at full speed when the propeller shaft seal is loading the motor (and

out of the water) is about 20 watts.

A second aluminum housing and propeller shaft were designed and fabricated, with the

same sized o-rings placed in the aluminum housing and stationary with respect to the

shaft. This design is projected to not only reduce friction but also increase the life

expectancy of the seal based on the Gow-Joule effect [9]. By sealing on the inside

diameter of the o-ring there is less surface area contact between rotating and stationary

surfaces, resulting in less friction. After testing we found that there was not much

savings in power consumption with the new seal design and the initial design remained

on the vehicle. Figure 5.12 shows the original and redesigned seals.


Figure 5.12: Propeller Shaft Design: Original (top) and Re-designed (bottom).

5.3.2. Hull Material Selection Though it is smooth, the inside diameter of the PVC pipe is not well controlled. The

first pipe used had an average inside diameter of 3.060” (0.060” over the required

dimension) and leaked at depths of 5 to 8 feet. Pipe from a second manufacturer had a

more acceptable average inside diameter 3.011” with less deviation. This pipe provided

for a leak free seal at depths of 5 to 8 feet during several 8 hour tests (8 feet being the

deepest test tank available in the AOE department). This pipe is used on MOD0,

however, research for a better material is needed. Initial research by the author turned up

no superior replacements. Later research by ASCL member Caleb Slyvester found an

acrylic tubing to have a much better surface finish and well controlled diameters. This

material will be used on the next generation AUV, MOD1.

5.3.3. Inertial Measurement Card Noise The first version of the inertial measurement card resulted in abundant measurement

noise from all sensors. The sources of this noise were due to the use of switching

regulators (200mVPP ripple on the 5V supply line), a lack of bypass capacitors, and

amplification of noise through the signal conditioning amplifiers. With sensitivities in


the millivolt range required to make accurate measurements of roll, pitch, and yaw rates,

the measurements provided by the card are useless for dead reckoning and testing the

compass and rate gyro calibration routines. Improvements planned for MOD1 include

using DC-DC converters instead of switching regulators, use of bypass capacitors

wherever possible on the card, elimination of some signal conditioning by using sensors

with larger output scales, and large ground planes around sensitive components.

5.3.4. Rabbit 3000 Bandwidth The problem of computation time arose when trying to process and save data on the

Rabbit 3000 microprocessor. Because the Rabbit 3000 is not running a real-time

operating system, multitasking and threading techniques were not available and every

task must be run sequentially. Data from the sensor arrives asynchronously, so extra time

must be spent waiting on each set of sensor data to arrive before moving on to the next

sensor. And when in a remotely operated mode time spent communicating to the host

ties up additional computation time. Additionally, writing to the memory requires about

100 milliseconds per sensor. A data storage rate of 10Hz is desired, saving the data to the

FLASH memory. But with this much time spent saving data, new sensor data and

commands from the host PC are lost. Attempts made by the author to optimize program

operation provided little improvement and no experimental data could be collected to

evaluate the performance of the compass calibration techniques

6. Conclusions and Future Work 6.1 Conclusions

This thesis presented the motivation to build miniature autonomous underwater

vehicles and research algorithms needed to calibrate the compass and heading rate

sensors. Individual calibration algorithms were developed and verified through computer

simulations. From the results of these simulations and the previous work on compass

calibration, the magnetometer-based calibration algorithm and the individual direction

calibration routine in [4] appear to be the best approaches to compass calibration. With

the compass calibrated, least squares heading rate gyro calibration is preferred over the

adaptive based routine, providing equal performance with no need for tuning.

Design and creation of MOD0 was largely a success, thanks to persistence of the team

and the model AUV, USNA-1, from which many design ideas were gleaned. MOD0 was

deployed in remotely operated mode twice, one of which in the Virginia Tech Duck

Pond. Due to processor constraints and analog system noise, it has not seen autonomous

service and reliable sensor data collection has not been possible. Despite these problems,

the vehicle is a great first product by the ASCL members.

6.2 On the Horizon Even with the small amount of run time MOD0 has seen, many mechanical and

electrical design improvements have been considered and are already being incorporated

into the ASCL’s next generation AUV, MOD1. Besides those discussed in the previous

chapter, other improvements include slightly increasing the outside diameter by 19mm,

using a thinner wall tube for the hull (from 6.4mm on MOD0 to 3.2mm on MOD1), and

changing the mounting rail configuration. While this may not seem like much increase in

diametric size, it will allow for electronic components otherwise too big to be used as

well as the ability to stack more circuit boards, thereby reducing the length of the vehicle.

The tail section material will change from foam and aluminum to PVC. Though not

reducing weight, this change will eliminate the hassle of sealing the foam and allow a

70

Conclusions and Future Work 71

thinner walled section to be used, giving more room inside for linkages and inclusion of a

watertight connector. It has been decided that mission sensors will put outside the

vehicle in a smaller housing slung underneath the vehicle. This will keep the main

vehicle short and make for a mostly unchanging configuration of primary electronics.

Vehicle electronics are improving with a higher wattage power source (use of Li-Ion

batteries with high energy per volume than current Ni-MH batteries), smart battery

chargers, and better designed circuit boards.

6.3 Future Work The future work recommended for AUV design is simple: make it smaller, faster,

stronger, smarter, and easier to use! There is no doubt that these qualities will improve

over time, with lab experience and consultation with other AUV developers being

valuable resources.

Much work is still required to have the AUV automatically calibrate the compass and

heading rate sensors. For cases of calibration algorithms requiring even sample rates of

all directions/angular velocities, statistical methods could be researched to see if there is a

way to accommodate less than ideal data sets. The derivation problems with using the

adaptive identifier for compass calibration could be further studied, as the use of GPS

data intuitively seems to be a good approach and has not been fully explored.

With the use of a PC architecture computer in the new AUV, evaluating the

performance of algorithms introduced here (and those in prior work) should not be a

problem. The push for a more compact vehicle should encourage future researchers to

consider custom building a compass circuit board. With recently gained knowledge of

circuit board design, the ASCL may be able to design a magnetometer sensor card which

is smaller and cheaper than the board currently used.

And finally, better names should be decided for MOD0 and MOD1. Only engineers

would choose such names!

7. References [1] Posey, Carl, “Robot Subs Go to War”, Popular Science,

http://www.popsci.com/popsci/science/article/0,12543,430240-1,00.html, April 2003.

[2] Stilwell, D.J., Wick, C.E., Bishop, B.E., “Small Inertial Sensors for a Miniature

Autonomous Underwater Vehicle,” Proceedings of the 2001 IEEE International Conference on Control Applications, Mexico City, Mexico, Sept. 2001, pp. 841—846.

[3] Caruso, M. J., “Applications of Magnetic Sensors for Low Cost Compass Systems,”

Honeywell Solid State Electronics Center—Magnetic Sensors http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf.

[4] Stokey, R., “Dynamic Calibration of a Magnetic Heading Sensor Using an Acoustic

Net,” Proceedings of the Twelfth International Symposium on Unmanned Untethered Submersible Technology, Aug. 2001.

[5] Titterton, D.H., Weston, J.L., Strapdown inertial navigation technology, Peter

Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, United Kingdom, 1997.

[6] Smallwood, D.A., Whitcomb, L.L., “Preliminary Experiments in the Adaptive

Identification of Dynamically Positioned Underwater Robotic Vehicles,” Proceeding of the 2001 IEEE/RSJ Internation Conference on Intelligent Robots and Systems, Maui, HI, Oct. 2001, pp. 1803—1810.

[7] Shapiro, L., Stockman, G., Computer Vision,

http://www.cse.msu.edu/%7Estockman/Book/, 2001. [8] Stilwell, D.J., Wick, C.E., “A Miniature Low-cost Autonomous Underwater

Vehicle,” Proceedings IEEE/MTS Oceans 2001, Honolulu, HI, Nov. 2001, pp. 423—428.

[9] Parker O-Ring Handbook, Parker Hannifin Corporation, Catalog ORD 5700/US,

Cleveland, OH, 2001. [10] TCM2 Electronic Compass User’s Manual, Precision Navigation Incorporated,

Rev. 1.08, Santa Rosa, CA, Dec. 1999. [11] Healey, A.J., An, E.P., Marco, D.B., “On Line Compensation of Heading Sensor

Bias for Low Cost AUVs”, Proceedings of the IEEE Workshop on Autonomous Underwater Vehicles, Cambridge, MA, Aug. 1998, pp. 35—42.

72

http://www.popsci.com/popsci/science/article/0,12543,430240-1,00.html

http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf

http://www.cse.msu.edu/%7Estockman/Book/

Appendix A—Proofs Adaptive Identifier for Compass Calibration

The adaptive identifier used for rate gyro calibration is proposed for finding the

compass model calibration coefficients. The error model is restated

( )m E mψ ψ ψ= − (A.1)

where

( ) sin cos sin 2 cos 2m m m mE A B C D E mψ ψ ψ ψ= + + + + ψ (A.2)

Consider the kinematical relationship

( ) cos ( )( ) sin ( )

x t v ty t v t

ψψ

==

(A.3)

where v is the vehicle velocity. The Cartesian coordinates ( ), ( )x t y t are derived from

longitude and latitude GPS information. The +x axis corresponds to due north heading

(ψ=0), and a positive clockwise heading measurement is used. Expanding (4.8), let

(A.4)

1sin ( )

( ) cos ( ) , ( ) , ( ) 0sin 2 ( )cos 2 ( )

m

m

m

m

At B

f t t t tCt Dt E

ψψ β βψψ

= =

=

then

( )( )

( ) cos ( ) ( )

( ) sin ( ) ( )m m

m m

x t v t E t

y t v t E t

ψ ψ

ψ ψ

= − = −

73

( ) cos ( ) ( ) ( )

( ) sin ( ) ( ) ( )

m

Tm

Tx t v t f t t

y t v t f t t

ψ β

ψ β

= − = −

(A.5)

Consider only the first equation in (4.10). Estimate error equations are formed

ˆ( ) ( ) ( )

ˆ( ) ( ) ( )ˆ( ) ( ) ( )

ˆ( ) ( )

x t x t x t

t t

t t

tβ β β

β β

= −

= −

=

(A.6)

x t x t x t= −

where ˆ( )x t and ˆ( )tβ are estimations of ( )x t and ( )tβ . Consider the Lyapunov

candidate equation

11 1( , ) ( ) ( ) ( ) ( )2 2

T Tm x x t x t t tβ β β−= + Λ (A.7)

where is positive definite and 2 2R ×Λ∈ ( , ) 0 ( ), ( )m x x t tβ β≥ ∀ . Showing that the time

derivative of (A.7) is negative semi-definite proves that the state estimate error and the

coefficient estimate errors ,x β are bounded. The time derivative of (A.7) is computed

1( , ) ( ) ( ) ( ) ( )T Tm x x t x t t tβ β β−= + Λ (A.8)

Consider the following candidate for ˆ( )x t

ˆˆ( ) ( ) cos ( ) ( ) ( )Tmx t kx t v t f t tψ β = + − (A.9)

where . The corresponding estimation error for 0k < ( )x t

ˆ( ) ( ) ( )ˆ( ) cos ( ) ( ) ( ) cos ( ) ( ) ( )

ˆ( ) cos ( ) ( ) cos ( ) ( )

Tm m

T T

x t x t x t

kx t v t f t t v t f t t

kx t v f t t v f t t

ψ β ψ

β β

= −

T β = + − − − = − +

(A.10)

Substituting (A.10) into (A.8)

1

1

1

( , ) ( ) ( ) ( ) ( )ˆ( ) ( ) cos ( ) ( ) cos ( ) ( ) ( ) ( )

ˆ( ) ( ) ( ) cos ( ) ( ) cos ( ) ( ) ( ) ( )

T T

T T T T

T T T T T

m x x t x t t t

x t kx t v f t t v f t t t t

kx t x t vx t f t t v f t t t t

β β β

β β β β

β β β β

−

−

−

= + Λ

= − + + Λ

= − + + Λ

(A.11)

The final step to proving that the adaptive algorithm converges is to show that (A.11)

negative semi-definite. The time derivative of the coefficient estimation error, ( )tβ

should be chosen such that the 4th term of (A.11) cancels out the 2nd and 3rd terms.

Unfortunately, a suitable ( )tβ could not be determined. A different Lyapunov

equation could be chosen, but none were found. The main problem with this method is

that the calibration coefficients are contained within the sinusoid function in (4.10).

Vita Jared Mach was born and raised in Morristown, Tennessee. Jared graduated from

Morristown Hamblen High School—West in 1995. He attended Tennessee

Technological University in Cookeville, TN and graduated in May of 2001 with a

Bachelor’s of Science degree in Electrical Engineering. He then went on to attend

Virginia Polytechnic Institute and State University in Blacksburg, VA where he

completed a Master’s of Science degree in Electrical Engineering. Research areas of

interest while an undergraduate and graduate student included digital systems, control

systems, robotics, and mechanical design. Jared now resides in Charlottesville, VA and

is employed by Sperry Marine.

76

Toward Auto-Calibration of Navigation Sensors for Miniature Autonomous Underwater Vehicles · 2020....

Documents

Transcript of Toward Auto-Calibration of Navigation Sensors for Miniature Autonomous Underwater Vehicles · 2020....