Heading Estimation of Fin Actuated Autonomous Underwater Vehicles

James Carrillo
University of Washington
AA 549: Estimation and Kalman Filtering
June 12, 2013
I. INTRODUCTION
The Fin Actuated Autonomous Underwater Vehicles used
in the the Nonlinear Dynamics and Control Lab at the
University Washington rely on both off-board and on-board
sensors to test different control methods and mathematical
models. State estimation and ltering greatly improve the
speed, accuracy, and response of any system dependent on
sensor data for control. A key on-board sensor used by
the vehicles is the 3D magnetic compass which provides
vehicle heading, pitch, and roll angles. The orientation
information provided by the compass is used to calculate
tangential, normal, and binormal unit vectors which dene
(at least partially) the systems state. One problem with
estimating the vehicles state is the oscillatory motion of the
compass data due to the n-actuated thrust. Estimation of
this oscillatory motion based on 3D compass measurements
will be attempted for a known vehicle path. The amplitude,
frequency, and phase shift of the heading oscillation will be
assumed as a static state.
II. MODEL
The assumed model for the 3D Compass measurement is
a sinusoidal function of constant amplitude, frequency, and
phase. The discrete time state equation for this model is
x
k+1
= x
k
=
_
_
A

_
_
(1)
where A is amplitude, is frequency, and is the phase.
The measurement model is given by
y
k
= x
1
sin(x
2
t
k
+x
3
) +v
k
(2)
where y
k
is the measured value and v
k
is a zero-mean
Gaussian noise process. Due to the relatively slow velocity
and frequency of oscillation of the sh, a discrete-time model
was assumed to sufciently represent the system. Another
choice that would have been easily implemented and re-
duced computational effort is a continuous-discrete Extended
Kalman Filter. Had a requirement been to implement a
possible on-board lter, this choice would have been ideal.
Also, because the state being estimated is constant, other
nonlinear estimators can easily accomplish the same task. A
particle lter was chosen because it less susceptible to the
unmodeled dynamics that are commonly experienced in an
underwater environment[2].
III. DATA
The data used for this project was obtained from the
University Washingtons Nonlinear Dynamics and Control
Lab and Nathan Powel. The data suffered from wrap-
around due to the heading measurements transitioning from
0 to 359 as shown in Fig. 1 below. The data was corrected,
Fig. 1. Raw heading data for a Fin Actuated Autonomous Underwater
Vehicle pursuing a zero-heading path
as shown in Fig. 2, to create an equivalent negative heading,
allowing the system to be more easily estimated and qual-
itatively observed. While this would be solved differently
on a real-time system for robustness, a simple solution was
incorporated as detailed in the Appendix. It was assumed the
data had zero-mean Gaussian noise and the nal 0.5 seconds
of measurements could be dismissed due to test error.
Fig. 2. Corrected heading data for analysis
IV. METHODS
It was initially assumed that a discrete time Kalman Filter
would sufciently estimate the heading direction. The results
shown in Fig. 4 conrmed the lter did minimize the variance
Fig. 3. Corrected heading measurements plotted against the discrete
Kalman Filter estimates.
Fig. 4. 3 variance comparison of Kalman Filter error
of the measurements, but the relative magnitude of improve-
ment did not constitute the need for a lter of this type.
Another observation of the raw data and testing video footage
revealed that a nonlinear estimator was necessary to represent
the oscillatory nature of dynamics and measurements. The
results were then expected to be similar to the static
particle lter problem 4.32 in Crassidis [1] and a comparable
method was applied.
V. RESULTS
A subsection of the data provided for this project was
comprised of static heading data to characterize the associ-
ated sensor variance. A weighted mean variance of these
stationary measurements (i.e. vehicles facing each of the
cardinal directions) determined the sensor to have an approx-
imate variance of
2
= 0.390. This value was used within
a zero-mean Gaussian noise process and the calculation of
particle weights. Assuming q(x) = p(x) is a uniform density
ranging from 0 to 3, 50000 3-state particles were generated
to initialize the lter. With the prediction of the particles
given by x
(j)
k+1
= x
(j)
k
, the lter update equations are simply
w
(j)
k+1
= w
(j)
k
exp
_

_
y
k
x
( j)
1
sin(x
( j)
2
t
k
) +x
( j)
3
_
2
2
2
_

_
(3)
w
(j)
k+1

w
(j)
k+1

N
j=1
w
(j)
k+1
(4)
where N is equal to the number of particles and w
(j)
k
is the
importance weight associated with each particle. The state
Fig. 5. System response with mean state estimate compared to measured .
Fig. 6. State differences between real-time estimates and nal averaged
value estimate.
estimates at each time step were approximated as
x
k

j=1
w
(j)
k
x
(j)
k
(5)
which could then be averaged to compute a static state
estimate. The resulting estimated dynamics (neglecting mea-
surement errors at end of dataset) compared to the heading
measurements shown in Fig. 5. The estimator appears to
accurately track the system until approximately 5.5 seconds.
To evaluate the performance of the estimator, the difference
between the real-time estimates of the state and the mean
value solution provides a disappointing insight into the real-
ities of estimating real-world data. Its apparent that assumed
model was insufcient to condently estimate this system.
An Unscented Kalman Filter was also attempted, but the
algorithm was difcult to incorporate on this type of system
(See Appendix). Circular path data was also included in the
dataset from Nathan, but only a discrete linear Kalman Filter
was shown successfully for the tests.
VI. DISCUSSION AND CONCLUSIONS
The divergence of the estimator could very well be due to
the unmodeled effects commonly associated with an under-
water environment[2]. A more powerful inuence would be
the deviation of the vehicle from its straight path. The model
represented by equations (1) and (2) assumed that the vehicle
maintains its straight trajectory from one state to another. If
the vehicle were to drift off course, it could explain the poor
performance. The model could be modied to account for an
unknown vehicle path or be coupled with current localized
position and would most likely produce better results. Pairing
the results with a physical observance of the actual test would
also provide some more insight into environmental variability
or specic data points.
REFERENCES
[1] Optimal Estimation of Dynamic Systems. CRC Press, 2012.
[2] K. A. M. et al, geometric methods for modeling and control of free-
swimming n-actuated underwater vehicles, IEEE Transactions on
Robotics, 2007.