oppurtunistic localization

7/31/2019 Oppurtunistic Localization

1/58

Opportunistic AUV Localization using SurfaceDrifters

Sami El-Ferik, Bilal A. Siddiqui

Department of Systems EngineeringKing Fahd University of Petroleum and Minerals

Dhahran 31261

[email protected]

February, 2011
http://find/http://goback/


2/58

Introduction Literature Review Opportunistic Localization Iterative Cramer Rao Lower Bound EKF-MLDA Simulation Results

Outline

1 Introduction

2 Literature Review

3 Opportunistic Localization

4 Iterative Cramer Rao Lower Bound

5 EKF-MLDA

6 Simulation Results

7 Conclusion

Sami El-Ferik, Bilal A. Siddiqui KFUPM

Opportunistic AUV Localization using Surface Drifters


3/58


Underwater Localization Challenges


http://goforward/http://find/http://goback/


4/58



EM Attenuation in H20 GPS unavailable 10m deep.GPS signals attenuate by 50 dB @ 1 m depth.


http://find/


5/58




IMU integration drifts DR becomes inaccurate with time.



O C S


6/58




IMU integration drifts

DR becomes inaccurate with time.We have better maps of Mars, Venus and the Moon than wehave of the Earths oceans. There is also a lack of features.



I d i Li R i O i i L li i I i C R L B d EKF MLDA Si l i R l
http://find/


7/58




IMU integration drifts

DR becomes inaccurate with time.We have better maps of Mars, Venus and the Moon than wehave of the Earths oceans. There is also a lack of features.

Acoustic Communication is most suitable for underwaterapplications, but medium (seawater) is non-homogeneous.



I t d ti Lit t R i O t i ti L li ti It ti C R L B d EKF MLDA Si l ti R lt


8/58


Common Localization Algorithms



Introduction Literature Review Opportunistic Localization Iterative Cramer Rao Lower Bound EKF MLDA Simulation Results
http://find/


9/58



GPS receivers andlong/short baseline(L/SBL) transponders toget position fix by

TOA/TDOA





10/58




TOA/TDOADead reckoning (DR)with onboard sesnorse.g., Doppler velocitylogs (DVL), compass and

inertial measurementunit (IMU)





11/58






Statistical estimationtools.





12/58











13/58

pp










14/58










15/58

Literature Review





16/58

Literature Review

(CRLB) can be used to find the optimal topology of LBLtransponders, dead-reckoning precision and update raterequired for specified navigation performance [Bingham2009]

Sami El-Ferik, Bilal A. Siddiqui KFUPMOpportunistic AUV Localization using Surface Drifters



17/58

Literature Review


AUVs and GPS enabled buoys resolve the inter-node rangesbased on the travel time, without the need for AUVs tosurface [Xiang2007]




18/58

Literature Review


AUVs and GPS enabled buoys resolve the inter-node rangesbased on the travel time, without the need for AUVs tosurface [Xiang2007]

Kruger [Kruger2009] compared grid filters, particle filters and

Multiple Analytical Digital Filter (MADF) position estimatesby combining DR, GPS and LBL (using WHOI micro-modems)




19/58

Problem Statement




20/58

Problem Statement

This work follows [Arrichiello2012] where the problem ofopportunistic localization is studied.


http://find/


21/58

Problem Statement


Drifting buoys and surface vessels can be used for localization.


http://find/


22/58

Problem Statement


Drifting buoys and surface vessels can be used for localization.Drifter positions are assumed to be known.


http://find/


23/58

Problem Statement



The task is to use range and bearing information from driftersfor localization.


http://find/


24/58

Problem Statement



The task is to use range and bearing information from driftersfor localization.

This requires data association and data fusion


http://find/


25/58

Mathematical Model

State vector of the vehicle in planar motion:

xk= xk1 yk1 k1 xk yk k where, x=North position, y=East position, and =bearing, while kis the time index. XD represents the drifters position, xR and the range and bearing from drifter respectively.


http://find/


26/58

State Space Model

Discrete-time nonlinear model of motion:

xk+1 = Axk + B(xk) uk + F (xk) wk (1)

with input u = [v], v=commanded forward velocity and omega =commanded turn rate.Process noise w is zero mean white Gaussiannoise with covariance R

wand T is the sampling time.

A =

03x3 I3x303x2 I3x3

; B =

03x1 03x1cos(k)T 0sin(k)T 0

0 T

; (2)

F =

03x1 03x1cos(k) 0sin(k) 0

0 1

; Rw =

2w,v 0

0 2w,




27/58

Sensor Models


http://find/


28/58

Sensor Models

Digital CompassOutput Equation and linearized output matrix are

ycomp = k + comp; Ccomp =

0 0 0 0 0 1

(3)

where comp N(0, 2comp) and R() is 2D rotation matrix.




29/58

Sensor Models

Digital CompassOutput Equation and linearized output matrix are

ycomp = k + comp; Ccomp =

0 0 0 0 0 1

(3)

where comp N(0, 2comp) and R() is 2D rotation matrix.

Doppler Velocity Log (DVL) and Inertial sensors (IMU)

yDVL =

xk xk1 yk yk1 k k1T

+ DVL (4)

CDVL =

I3x3 I3x3

; DVL N(0, R

2DVL)

RDVL =

R(k)

2DVL,x 0

0 2DVL,y

RT(k) 0

0 2DVL,

http://find/


30/58



31/58

Sonar Range and Bearing from Drifters

Range from ith Drifter

yrange = XD XR + range (5)

Crange = 0 0 0 xkxDXDXR

ykyDXDXR

0


http://find/


32/58

Sonar Range and Bearing from Drifters

Range from ith Drifter

yrange = XD XR + range (5)

Crange = 0 0 0 xkxDXDXR

ykyDXDXR

0

Bearing from ith Drifter

ybear = k + bear = tan1 yD yk

xD xk k + bear (6)

Cbear =

0 0 0 ykyDXDXR

xkxDXDXR

1

The noise processes are zero-mean Gaussian.


http://find/


33/58

Iterative Posterior Cramer Rao Lower Bound


http://find/


34/58


We use an iterative formulation of PCRLB for our system thatgives the performance limits for any unbiased estimator.


http://find/


35/58


We use an iterative formulation of PCRLB for our system thatgives the performance limits for any unbiased estimator.

For measurements Yk = (y0, y1 . . . yk), states Xk and their

estimatesXk, error covariance of any unbiased estimator bebetter than:

E

Xk Xk

Xk Xk

T J1 (Xk) (7)

J(Xk) = E

2

XkXklog p(Xk; Yk)




36/58

PCRLB

[Arrichiello2012] give an elegant recursive formulation to obtain Jkfor some appropriate matrices A and C, Rw being the noisecoviariance of all sensors. Note that, terms of eqn 8 are

expectations, hence a closed form formulation and we have toresort to Monte Carlo simulations.

Jk+1 = R1k,w + E

CTk+1R

1k+1,Ck+1

R1k,wEAk Jk + EATk R1k,wAkEATk R1k,w(8)



37/58



38/58

Extended Kalman Filter with Maximum Likelihood DataAssociation (EKF-MLDA)

Data from different sensors are combined using EKF




39/58



When multiple drifters are simultaneously in the AUVsvisibility range, data association problems could arise. Weneed to associate range/bearing readings with correct drifters.




40/58



When multiple drifters are simultaneously in the AUVsvisibility range, data association problems could arise. Weneed to associate range/bearing readings with correct drifters.

Hence, we add another step inside the EKF by using MaximumLiklihood (ML) data associator. Let P denote the errorcoviarance, and let the AUV have ND drifters in its visibilityrange, x is the estimate before measurement yk update.

yk =

yTDVL yTcomp yTD,1 . . . yTD,ND

(9)

yD,j =

yrange

XD,j, x

k+1

ybear

XD,j, x

k+1

; CD,j =

Crange

XD,j, x

k+1

Cbear

XD,j, x

k+1

(10)




41/58

EKF-MLDA Algorithm





42/58

EKF-MLDA Algorithm

EKF Time Update of State and Covariance

xk+1 = Ax+k + B(x

+k )uk; P

k+1 = AP

+k A

T + F(x+k )RwF(x+k )

T(11)





43/58

EKF-MLDA Algorithm

EKF Time Update of State and Covariance

xk+1 = Ax+k + B(x

+k )uk; P

k+1 = AP

+k A

T + F(x+k )RwF(x+k )

T(11)

Modified Maximum Likelihood Data Association

for j = 1 ND do

Sj = CD,jP+k C

TD,j + Rv (12)

end for

do Associate measurement with most likely drifter

iM = arg maxj

1

2 |Sj|e 1

2 (yD,iyD,j)S1

j (yD,iyD,j)T

(13)





44/58

EKF-MLDMeasurement Update for EKF



http://find/


45/58


Data Associated output matrix

C =

CTDVL CTcomp C

TD,1M

. . . CTD,ND,M

(14)



http://find/


46/58


Data Associated output matrix

C =

CTDVL CTcomp C

TD,1M

. . . CTD,ND,M

(14)

Then, the update is:

Kk+1 = Pk+1C

T

C Pk+1CT + R

1(15)

yk+1 = yk h

x

k+1, XD1,M, ....XDND

,M

(16)x+k+1 = x

k+1 + Kk+1yk+1 (17)

P+k+1 =

I Kk+1C

xk+1

Pk+1 (18)




R d i i E i Ab f D if


47/58

Reduction in Error in Absence of Drifter

Trajectory and Error Covariance Ellipses of AUV in

the absence of drifter measurements

Error metrics in the absence of drifter

measurements. (a) EKF estimation error, (b)

distance from drifter, and (c) eigenvalues of J1k




G h f E i P f Si l D if


48/58

Growth of Error in Presence of Single Drifter

Trajectory and Error Covariance Ellipses of AUV in

the presnce of drifter measurements

Error metrics in the presnce of drifter

measurements. (a) EKF estimation error, (b)

distance from drifter, and (c) eigenvalues of J1k




I P f 5 D if


49/58

In Presence of 5 Drifters

Figure : Trajectory and Navigational Error of AUV in the presence of a 5

randomly placed drifters.




F t W k


50/58

Future Work

Future Work




F t W k


51/58

Future Work

Future Work

Adding uncertainty in drifter position and dynamics




Future Work


52/58

Future Work

Future Work


Realistic fading model of acoustic signals




Future Work


53/58

Future Work

Future Work


Realistic fading model of acoustic signalsM-ary detection decisions based on multiple drifters




Future Work


54/58

Future Work

Future Work


Realistic fading model of acoustic signalsM-ary detection decisions based on multiple drifters

Asynchronous drifter measurements




Future Work


55/58

Future Work

Future Work



M-ary detection decisions based on multiple drifters


Advanced particle and unscented filters




Future Work


56/58

Future Work

Future Work



M-ary detection decisions based on multiple drifters


Advanced particle and unscented filters

Cooperative localization.




References


57/58

References

F. Arrichiello, H. Heidarsson, and G. Sukhatme Submitted to.

opportunistic localization of underwater robots using drifters and boats.In IEEE International Conference on Robotics and Automation, 2012.

B. Bingham.

Underwater Vehicles, chapter Navigating Autonomous Underwater Vehicles, pages 3350.

In-Tech, 2009.

D. Kruger.

Path Planning and Localization of a UUV in a High Speed Estuarine Current Environment.PhD thesis, Steven Institute of Technology, Hoboken, NJ, 2009.

X. Xiang, Z. Xiao G. Xu, and X. Huang.

Coordinated control for multi-auv systems based on hybrid automata.In IEEE International Conference on Robotics and Biomimetics, Sanya, China, 2007.



Thank You


58/58

oppurtunistic localization

Documents