estimation - final project
TRANSCRIPT
8/13/2019 Estimation - Final Project
http://slidepdf.com/reader/full/estimation-final-project 1/3
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 filtering 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 orientationinformation provided by the compass is used to calculate
tangential, normal, and binormal unit vectors which define
(at least partially) the system’s state. One problem with
estimating the vehicle’s state is the oscillatory motion of the
compass data due to the fin-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
xk+1 = xk =
A
ω
φ
(1)
where A is amplitude, ω is frequency, and φ is the phase.
The measurement model is given by
˜ yk = x1 sin( x2t k + x3) + vk (2)
where yk is the measured value and vk is a zero-mean
Gaussian noise process. Due to the relatively slow velocity
and frequency of oscillation of the fish, a discrete-time modelwas assumed to sufficiently 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 filter, 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 filter 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 Washington’s 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 UnderwaterVehicle 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 final 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 sufficiently estimate the heading direction. The results
shown in Fig. 4 confirmed the filter did minimize the variance
8/13/2019 Estimation - Final Project
http://slidepdf.com/reader/full/estimation-final-project 2/3
Fig. 3. Corrected heading measurements plotted against the discreteKalman Filter estimates.
Fi g. 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 filter 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 filter 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 filter. With the prediction of the particles
given by x( j)k+1 = x
( j)k , the filter update equations are simply
w( j)k+1 = w
( j)k exp
−
˜ yk − 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 final averagedvalue estimate.
estimates at each time step were approximated as
x̂k ≈
N
∑ 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 toaccurately 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. It’s apparent that assumed
model was insufficient to confidently estimate this system.
An Unscented Kalman Filter was also attempted, but the
algorithm was difficult 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 CONCLUSIONSThe divergence of the estimator could very well be due to
the unmodeled effects commonly associated with an under-
water environment[2]. A more powerful influence would be
the deviation of the vehicle from it’s 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 modified to account for an
unknown vehicle path or be coupled with current localized
position and would most likely produce better results. Pairing
8/13/2019 Estimation - Final Project
http://slidepdf.com/reader/full/estimation-final-project 3/3
the results with a physical observance of the actual test would
also provide some more insight into environmental variability
or specific 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 fin-actuated underwater vehicles”,” ”IEEE Transactions on
Robotics”, ”2007”.