Unscented Kalman Filtering on Manifolds for AUV Navigation - Experimental Results Stephen T. Krauss1and Daniel J. Stilwell1

2025-04-24 0 0 7.76MB 6 页 10玖币

侵权投诉

Unscented Kalman Filtering on Manifolds for AUV Navigation -

Experimental Results

Stephen T. Krauss1and Daniel J. Stilwell1

Abstract— In this work, we present an aided inertial naviga-

tion system for an autonomous underwater vehicle (AUV) using

an unscented Kalman ﬁlter on manifolds (UKF-M). The inertial

navigation estimate is aided by a Doppler velocity log (DVL),

depth sensor, acoustic range and, while on the surface, GPS. The

sensor model for each navigation sensor on the AUV is explicitly

described, including compensation for lever arm offsets between

the IMU and each sensor. Additionally, an outlier rejection step

is proposed to reject measurement outliers that would degrade

navigation performance. The UKF-M for AUV navigation is

implemented for real-time navigation on the Virginia Tech 690

AUV and validated in the ﬁeld. Finally, by post-processing the

navigation sensor data, we show experimentally that the UKF-

M is able to converge to the correct heading in the presence of

arbitrarily large initial heading error.

I. INTRODUCTION

For an autonomous underwater vehicle (AUV) without

access to GPS signals while under water, inertial navigation

is one of the main sources of localization information.

The basic premise behind inertial navigation is that a ve-

hicle equipped with an inertial measurement unit (IMU)

can integrate measurements of angular velocity and linear

acceleration into estimates of attitude, velocity, and position.

However, it is well known that integration of small sensor

errors over time causes a buildup of error in the inertial

navigation estimates [1]. In order to correct the buildup of

error, a Kalman ﬁlter is commonly used to combine IMU

measurements with measurements from other sensors on

an AUV, such as a Doppler velocity log (DVL) and depth

sensor. While the extended Kalman ﬁlter (EKF) is one of the

most commonly used ﬁlters, other works have demonstrated

advantages to using other variants of the Kalman ﬁlter, such

as the unscented Kalman ﬁlter (UKF).

In this work, we expand upon the work done in [2], where

the authors propose the use of an unscented Kalman ﬁlter on

manifolds (UKF-M) for AUV navigation. First, we explicitly

describe the sensor models used in the measurement update

step of the UKF-M and include compensation for lever arm

effects caused by the mounting locations of the sensors on

the AUV. Next, we propose UKF-M retraction and inverse

retraction functions that allow position standard deviation to

be reported in meters instead of radians. Additionally, we

introduce a measurement validation algorithm to the UKF-

M measurement update step in order to reject measurement

outliers that would otherwise affect navigation accuracy.

Finally, we validate a real-time implementation of the ﬁlter in

1Bradley Department of Electrical and Computer Engineering, Vir-

ginia Polytechnic Institute and State University, Blacksburg, VA, USA

{stkrauss, stilwell}@vt.edu

the ﬁeld using the Virginia Tech 690 AUV, shown in Figure

1, with a mission consisting of surface calibration maneuvers

and a survey pattern. Post processing of the dataset is used

to demonstrate the ability of the UKF-M to converge in

the presence of large initial heading errors. Convergence

despite large initial heading errors allows the ﬁlter to operate

without the need for a separate ﬁlter for determining initial

conditions.

Fig. 1. Virginia Tech 690 AUV aboard the Virginia Tech research vessel

in Claytor Lake, Dublin, VA, USA.

The remainder of this work is organized as follows. In

Section II, previous works related to this work are summa-

rized. In Section III, we introduce the coordinate frames and

inertial navigation equations used in this work. In Section

IV, we present the sensor models for the navigation sensors

on the 690 AUV. In Section V, a summary of the unscented

Kalman ﬁlter on manifolds is given, and in Section VI, a

simple measurement outlier rejection algorithm for the ﬁlter’s

measurement update step is presented. Finally, in Section

VII, results from a real-time implementation of the ﬁlter

tested in the ﬁeld with the Virginia Tech 690 AUV are given.

II. RELATED WORK

One of the most common approaches to reducing or

bounding the buildup of inertial navigation error is to incor-

porate measurements from other sensors through an extended

Kalman ﬁlter (EKF), such as those presented in [1], [3],

[4] and [5]. One of the disadvantages of the EKF is that

it requires linearization of the inertial navigation dynamics,

which reduces accuracy of the estimate of the state and

its covariance. Additionally, due to the linearization of the

inertial navigation equations, the EKF can be susceptible to

arXiv:2210.06510v1 [cs.RO] 12 Oct 2022

divergence if the initial state estimate is not close to the true

initial state.

The most common way this problem arises is through

initialization of the heading angle. In order to provide

an accurate initial heading angle to the EKF, a two-step

approach consisting of coarse alignment followed by ﬁne

alignment is commonly used [4]. Other approaches to the

handling of large heading error with an EKF include [6],

where an EKF model that can handle large initial alignment

errors is developed. This approach still uses an EKF, which

requires linearization, and must also switch to a different

inertial navigation model when alignment error is reduced

below a certain level, much like the coarse and ﬁne alignment

approach. In this work, use of the UKF-M allows for ﬁlter

convergence in the presence of large initial heading error

without any special modiﬁcations to the ﬁlter or system

model.

Other variations of the Kalman ﬁlter have been proposed

in the literature for nonlinear ﬁltering, such as the unscented

Kalman ﬁlter (UKF) [7]. Instead of linearizing the state

propagation and measurement equations, the UKF uses a set

of points, named sigma points, distributed such that their

empirical mean and covariance estimate the state mean and

covariance. These sigma points are propagated through the

nonlinear equations in the prediction and update steps. In

this way, the UKF can be accurate to a higher order than

the EKF and does not require linearization of the inertial

navigation equations. Approaches to using the UKF for aided

inertial navigation system have been explored in works such

as [8], [9], and [10]. However, implementation challenges

exist when using the UKF for inertial navigation. If vehicle

roll, pitch, and heading are represented in the state vector as

Euler angles, the algorithm must compensate for the fact that

0 degrees and 360 degrees are equivalent. When propagating

sigma points through the inertial navigation equations, one or

more of the sigma points may wrap, leading to an inaccurate

representation of the state mean and covariance.

The unscented Kalman ﬁlter on manifolds (UKF-M),

proposed in [11], is a modiﬁcation of the UKF that is able

to directly handle states that evolve on the manifold SO(3),

such as vehicle attitude. With vehicle attitude represented by

an rotation matrix in SO(3), the effect of wrapping of Euler

angles on sigma points can be avoided. In [2], the authors

present an inertial navigation algorithm for AUVs based on

the UKF-M. The authors demonstrate the ability of the ﬁlter

to accurately estimate the state covariance through Monte-

Carlo simulations and show that the ﬁlter can run in real

time on an AUV. In this work, we build upon the results in

[2] by using the UKF-M for inertial navigation using sensor

models with lever arm compensation and validating a the

ﬁlter in real time with data from an AUV survey mission.

III. INERTIAL NAVIGATION EQUATIONS

We adopt the notation and navigation frame inertial navi-

gation formulations presented in [4]. The inertial navigation

equations involve three reference frames: the Earth-centered

Earth-ﬁxed (ECEF) frame, the body frame, and the naviga-

tion frame.

The origin of the Earth-centered Earth-ﬁxed (ECEF) ref-

erence frame is attached to the geometric center of the Earth.

The positive z-axis of the ECEF reference frame is aligned

with the Earth’s axis of rotation and passes through the north

pole. The positive x-axis passes through the intersection of

the equator and the prime-meridian. The y-axis completes

a right-handed coordinate system [1]. Note that, since the

ECEF frame is ﬁxed to the earth, it is rotating with respect

to an inertial frame [3]. Values expressed in this frame are

indicated with an e.

The origin of the body frame is ﬁxed to a point on the

AUV with the positive x-axis passing through the nose of

the AUV, the positive z-axis pointing down, and the y-axis

completing a right-handed coordinate system. In this work,

the body frame is taken to be the IMU measurement frame.

Values expressed in the body frame are denoted with b.

The navigation frame is a coordinate frame that is tangent

to the earth’s surface with an origin co-located with the origin

of the body frame. The navigation frame’s north direction

points toward true north, the down direction points toward

the earth’s center perpendicular to the tangent plane, and the

east direction points east orthogonal to the north and down

directions. Values in the navigation frame are notated with

an n.

Fig. 2. Illustration of the inertial navigation reference frames at a moment

in time. The navigation frame origin is ﬁxed to the body frame origin with

position described by its geodetic latitude (Lb) and longitude (λb).

The nonlinear kinematic equations that relate attitude,

velocity, and position of the body frame to angular velocity

and linear acceleration of the body frame are

b=Cn

bΩb

ib −(Ωn

ie +Ωn

en)Cn

eb =fn

ib +gn

b−(Ωn

en + 2Ωn

ie)vn

pb=Tp

r(n)vn

(1)

where Cn

bis the rotation matrix from the body frame to

the navigation frame, vn

eb is the velocity of the body frame

relative to the ECEF frame, expressed in the navigation

文档加载中……请稍候！
如果长时间未打开，您也可以点击刷新试试。

下载文档到电脑，查找使用更方便

10 玖币 0人已下载

立即下载

摘要：

UnscentedKalmanFilteringonManifoldsforAUVNavigation-ExperimentalResultsStephenT.Krauss1andDanielJ.Stilwell1AbstractInthiswork,wepresentanaidedinertialnaviga-tionsystemforanautonomousunderwatervehicle(AUV)usinganunscentedKalmanlteronmanifolds(UKF-M).TheinertialnavigationestimateisaidedbyaDopplervel...

展开>> 收起<<

Unscented Kalman Filtering on Manifolds for AUV Navigation - Experimental Results Stephen T. Krauss1and Daniel J. Stilwell1.pdf

共6页,预览2页

还剩页未读，继续阅读

声明：本站为文档C2C交易模式，即用户上传的文档直接被用户下载，本站只是中间服务平台，本站所有文档下载所得的收益归上传人(含作者)所有。玖贝云文库仅提供信息存储空间，仅对用户上传内容的表现方式做保护处理，对上载内容本身不做任何修改或编辑。若文档所含内容侵犯了您的版权或隐私，请立即通知玖贝云文库，我们立即给予删除！

Unscented Kalman Filtering on Manifolds for AUV Navigation - Experimental Results Stephen T. Krauss1and Daniel J. Stilwell1

相关推荐

开通VIP享超值会员特权

作者详情

相关内容

热门标签

举报选择: