Bayesian Terrain-Based Underwater Navigation Using an Improved State-Space Model

Transcription

1 Bayesian Terrain-Based Underwater Navigation Using an Iproved State-Space Model Kjetil Bergh Ånonsen Departent of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondhei, Norway University Graduate Center, NO-227 Kjeller, Norway Oddvar Hallingstad Departent of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondhei, Norway University Graduate Center, NO-227 Kjeller, Norway Ove Kent Hagen Norwegian Defence Research Establishent (FFI), NO-227 Kjeller, Norway Abstract This paper focuses on terrain aided underwater navigation as a eans of aiding an inertial navigation syste. It is assued that a prior ap is present and Bayesian ethods are used to estiate the position of the vehicle. Traditionally this has been done using a crude low-diensional odel in the Bayesian filters. An iproved state-space odel is introduced, ipleented in a particle filter/sequential Monte Carlo filter and tested on real AUV (autonoous underwater vehicle) data. Copared to conventional filter odels, the new odel yields soother, slightly ore accurate results, though probles with overconfidence occur. I. INTRODUCTION This paper focuses on the developent of an iproved state-space odel for underwater terrain based navigation copared to that used in [1, [2. The focus is on AUVs (autonoous underwater vehicles), although the techniques presented can also be used by other underwater vehicles, lie reotely operated vehicles (ROVs) and subarines. Most AUV navigation systes are based on inertial navigation, see e.g. [3. Inertial navigation systes drift off with tie, even when velocity aiding is used. In order to allow extensive suberged operations, additional position fixes are needed. As GPS is not available under water, one is often dependent on acoustic aiding of the vehicle, either fro a other ship or fro using underwater acoustic transponders. In order to increase the autonoy of the vehicle and avoid costly pre-deployent of underwater transponders, terrain-based navigation is a favorable alternative. As an AUV in any cases carries a bathyetric sensor, it is natural to utilize bathyetric inforation in the navigation of the vehicle. For the ethods presented here we assue that there exists a prior bathyetric ap database of the area. We use a ultibea echo sounder (MBE) as bathyetric sensor. MBEs are very well suited for terrain-based navigation, as a large area behind the vehicle is covered by the sensor. Terrain-based navigation has been used for decades in aircraft and cruise issiles [4, [5, but for underwater vehicles the technique is rather new, though soe papers have been published over the last few years, e.g. [6, [7, [2. A variety of different terrain-based navigation ethods have been proposed in the literature. Aong the ore sophisticated are the Bayesian ethods, in which the position of the vehicle is estiated using a state-space odel. Due to the strong nonlinearity of the easureents, Kalan filter-based ethods have proven not well suited in ost cases. Instead nonlinear Bayesian ethods lie point ass (PMF) and particle filters (PF) have been successfully applied to underwater navigation [5, [7. Due to the high coputational coplexity of these ethods, a crude, lowdiensional state-space odel is used, often with a two or three diensional state-space vector. In reality, the process and easureent equations are uch ore coplicated than what is described in these low-diensional odels. This discrepancy has often proven to yield non-consistent estiate covariances [2. The estiated standard deviations are typically saller than the true estiation errors, which is unfavorable when the estiated position is to be integrated into the AUV navigation syste. In this paper we extend the state vector, incorporating ore inforation fro our nowledge of the underlying syste. An iproved state-space odel is presented, and the odel is tested using Bayesian ethods on real AUV data collected by a HUGIN vehicle equipped with an MBE. The HUGIN AUVs are anufactured by Kongsberg Maritie AS. II. THE ESTIMATION METHODS We here present the different odels that we use for our terrain navigation algoriths. First we introduce the different coordinate systes used in the odels. A. Coordinate Systes Table I gives an overview of the various coordinate systes used in this paper. B. Syste Model We first present the syste (truth) odel. We consider the following generic odel for the otion of our AUV,

2 Sybol e n b b TABLE I COORDINATE SYSTEMS USED IN THIS PAPER. Description Earth centered, earth fixed coordinate syste Local north, east down (NED) coordinate syste Body fixed coordinate syste Body fixed, roll and pitch copensated coordinate syste Map coordinate syste (earth fixed) x +1 = x + v + u + v, (1) v +1 = g(v ) + v, (2) where x = (x N,, x E, ) T is the horizontal AUV position vector (north, east) decoposed in the {e}-frae, u is the position change fro tie step to + 1, calculated fro the inertial navigation syste, and v and are white noise sequences. Equation (2) odels the strongly correlated error propagation of the inertial navigation syste. Note that we do not specify the diension of v nor the function g, so this generic odel can be ade arbitrarily coplex. The syste easureent equation is given by z = h(x ) + w. (3) v In [2 the proble of estiating the depth bias in the easureent was treated, and it was shown that by adding a separate state for the depth bias ore accurate and robust ethods were obtained in the presence of unnown depth biases. In this paper we assue that the dept bias has been properly copensated for in our data and estiate solely the horizontal AUV position. If necessary, a depth bias state can of course be added to the odels presented here too, at the cost of slightly ore coputationally deanding algoriths. Since we are dealing with MBEs, the botto depth easureent z is a vector easureent. The function h(x ) denotes the true ean sea depth at position x. The ter w denotes the sensor easureent noise, which is assued to be white. In order to siplify the atheatical description of our algoriths, we will express our true position as an offset, δx, fro the position estiate x of the INS. Our process then becoes, using that x +1 = x + u, with the easureent equation C. Conventional Filter Model δx = x x, (4) δx +1 = δx + v + v, (5) v +1 = g(v ) + v, (6) (7) z = h( x + δx ) + w. (8) Due to the coputational requireents of the Bayesian ethods we want to use for solving our terrain navigation proble, we have to restrict ourselves to a sipler odel than that described above. This is particularly the case for the point ass filter [5, in which a axiu of 3 states is practically feasible. For the particle filter, the diension is of less iportance, but the nuber of particles needed for convergence increases with the nuber of states [8. Conventionally, the filter odel used in terrain navigation is forulated directly in the {e} coordinate frae, and the drift is assued independent in the north and east directions, respectively. This is of course a ajor assuption, as the drift is ore liely to be independent along the surge and sway directions of the vehicle. The fact that we use a different odel in the filter fro that in the true syste is in itself a ajor source of error in the terrain navigation estiates. To distinguish between the syste and filter odels, we attach asteriss to variables in the filter odel. Our filter odel reads, using the sae delta notation as in (4), δx = x x, (9) δx +1 = δx + v, (1) z = h (x ) + w = h ( x + δx ) + w, (11) where δx = (δx N,, δx E, ) T and x = (x N,, x E, ) T, with the assuptions p(v, v l ) = p(v )p(v l ), (12) p(w, w l ) = p(w )p(w l ), (13) giving noise sequences that are statistically independent fro tie step to tie step. In (1) v and v have been replaced by the white noise sequence v. We also need to specify the densities of the noise sequences and the initial position offset, δx. A convenient, but not necessary, assuption is to assue Gaussian densities, p(δx ) = N (, P ), (14) p(v ) = N (, Q ), (15) p(w ) = N (, R ). (16) Equation (14) indicates that the initial position has a Gaussian density centered around the estiated INS position x. We further assue that the process noise, easureent noise and initial position are uncorrelated. The function h (x ) indicates the depth at position x and is given by the digital terrain ap. We use terrain aps consisting of gridded nodes, and the depth values given by h are found by bilinear interpolation over the grid. The noise sequence w in (11) therefore odels both ap noise (including interpolation errors) and the sensor noise. The easureent z is the total sea depth at the current AUV position, and it is coputed as the su of the AUV depth, given by a pressure sensor, and the AUV altitude above the MBE footprint on the sea floor for each MBE bea. The noise sequence w therefore contains contributions fro ap errors, pressure sensor noise and bathyetric sensor noise. For a detailed analysis of the depth accuracy of the HUGIN class AUVs, we refer to [9. 2

3 D. The Recursive Bayesian Filter Equations Let Z be the augented easureent vector consisting of all the easureents up to tie step. Fro Bayes forula (see e.g. [1) and our filter odel, (9) (11), we have where α = p(δx Z ) = p(z δx, Z 1)p(δx Z 1) p(z Z 1 ) = α 1 p w (z h ( x + δx )) p(δx Z 1 ) (17) [ pw (z h ( x + δx )) p(δx Z 1 ) dδx. The iniu ean square error (MMSE) [1 estiate is then given by with the covariance atrix δˆx = E{δx Z } = δx p(δx Z )dδx, (18) [ ˆP = (δx δˆx )(δx δˆx ) T p(δx Z ) dδx. (19) For the tie update of our position density we have, p(δx +1 Z ) = p(δx +1, δx Z )dδx = = p(δx +1 δx, Z ) p(δx Z )dδx p v (δx +1 δx ) p(δx Z )dδx. (2) Given the density of the initial position, p(δx ), (17) and (2) can now be used recursively to obtain the density of the position offsets for each tie step. However, the integrals in the equations are not analytically solvable, and we therefore need to evaluate these integrals nuerically. We will here use a particle filter (sequential Monte Carlo filter) for the evaluation of these equations. The particle filter gives estiates both of the vehicle position and its covariance. If the estiates are to be integrated into the inertial navigation syste, it is crucial that the estiated covariance is accurate as possible to avoid inconsistencies in the syste. E. The Bayesian Bootstrap Particle Filter Particle filtering has gained attention the recent years for estiation in nonlinear systes, in areas such as signal processing, tracing and navigation, to nae a few. Introductions to the principles of particle filtering can be found in [11, [12. We will here use the Bayesian Bootstrap particle filter, which is one of the siplest particle filter algoriths, first presented in [13. The algorith estiates the posterior density p(δx Z ) at tie step by a set of particles, {δx i } i=1,...,n, where N denotes the nuber of particles. The posterior density can be written as p(δx Z ) 1 N N δ ( δx δx i ), (21) i=1 where δ( ) is the (2-diensional) Dirac delta function. At each tie step, starting with the particle set {δx i } i=1,...,n, each of the particles are first predicted independently, by propagating the through the filter process equation (1). In our case this eans adding a noise sapled fro the process noise distribution, yielding the new particle set { } δx i +1. For each particle, a lielihood i=1,...,n weight w+1 i p(z +1 x i +1 ) is coputed, using the easureent z +1. The weights are then noralized, N i=1 wi +1 such { that } δx i +1, w+1 i i=1,...,n ent N ties, with the probability w j +1 = 1. The weighted particle set is now resapled with replace- of resapling particle δx j +1. We here use the systeatic resapling procedure described in [11. This copletes the cycle, and we now have a particle set approxiately distributed according to p(δx +1 Z +1). The reason why we have chosen the Bayesian Bootstrap algorith, is that it is one of the siplest particle filters to ipleent. Other algoriths, lie the Sequential Iportance Resapling (SIS) algorith [12 have also been tested on our data, with very siilar results to those fro the Bayesian Bootstrap. We therefore concentrate on the Bayesian Bootstrap algorith in this paper. F. Iproved State-Space Filter Model The odel presented in Section II-C is very siple in several aspects. Firstly, the errors fro tie step to tie step are assued uncorrelated, which is unrealistic. Secondly, the drift in the INS are assued independent in the north and east directions. In reality, when the INS is run with velocity aiding, e.g. fro a Doppler velocity log, the drift is typically larger along the vehicle trac than across the vehicle trac [14. It is therefore ore natural to develop the odel in the vehicle s body frae, the {b}-frae, and then later transfor the result to the {}-frae or {e}-frae. For convenience, we will in fact develop our odel in the {b }-frae, which is a roll and pitch copensated body frae. This frae will have the sae heading angle ψ as the {b}-frae, whereas the roll and pitch angles are zero, copared to the {n}-frae. The reason for using this frae is that the MBE easureents are autoatically given in this frae fro the MBE syste. In the conventional filter odel a white noise odel was used for the drift. A natural extension to this odel is to use a first-order Marov odel, see [1, for the drift in 3

4 the {b }-frae. The position offset will be described in the {}-frae, which is a local ap frae. Using a flat earth approxiation, the orientation of the {}-frae coincides with that of the {n}-frae, the only difference being that the origin of the {n}-frae is oving with the vehicle. The filter odel now becoes [ δx +1 = v b +1 [ I2 2 R b 2 2 G v [ δx v b [ [ R + b I 2 2 γ b ζ b where ζ b and γ b is a white noise sequence with, (22) E[ζ b (ζ b ) T = Q b δ l, (23) E[γ b (γ b ) T = Γ b δ l. (24) The superscripts on the vectors indicate which coordinate syste they are represented in. The atrix G v is the transition atrix for the discrete 1. order Marov process v b. This process is the discretization of the continuous 1. order Marov process v = [ 1 τ 1 1 v + ζ, (25) τ 2 where τ 1 and τ 2 are positive tie constants. The atrix G v is therefore given by G v = [ exp( T τ 1 ) exp( T, (26) τ 2 ) where T is the tie step in the discrete-tie Marov process. The transforation atrix R b is siply a heading copensation: [ R cos ψ sin ψ b =, (27) sin ψ cos ψ where ψ is the heading estiate taen fro the real-tie navigation syste. This concludes the developent of our new state-space odel. Letting ξ = [ (δx )T (v b )T T, where the asteris again indicates that this is a filter odel, our odel can be written on the for ξ +1 = F ξ + ν, (28) z = h (ξ, x ) + w, (29) which is standard state-space for with white noise [ [ ν R = b 2 2 γ b. (3) 2 2 I 2 2 We will again use a particle filter for this odel, this tie with 4 states in the state vector. ζ b North () Start East () Fig. 1. A. Experiental Setup Contour ap and AUV trajectory. III. COMPUTATIONAL RESULTS In this section we present results fro the aforeentioned ethods, using real data fro a HUGIN AUV. See [3 for a description of the HUGIN class AUVs. The data were collected by a HUGIN 1 vehicle in Noveber 21, in an area in the Oslofjord. The vehicle was equipped with a Kongsberg Maritie EM3 MBE (3 Hz), which together with a pressure sensor yields the total sea depth along a line across the trac of the vehicle. The MBE has 127 beas, but here only up to 92 beas were used. As in [2, a sub-sapling procedure was used on the MBE beas in order to counter for unodeled correlations within and between MBE pings, steing fro the fact that soe of the MBE beas will hit the sea floor within the sae ap grid cell. We used our algoriths to estiate the position offset fro the real-tie navigation solution x. To investigate the robustness of the estiators to errors in the initial position, the ethods were initialized with errors in the agnitude of 5 1 eters. For ground truth we used a soothed trajectory obtained using NavLab postprocessing of the real-tie DVL, IMU and DGPS/USBL data [15. The accuracy of this solution is approxiately 1 eter (1σ) [16. A gridded terrain ap, with 1 eter grid resolution, was used. This ap was constructed using easureents fro a Kongsberg Maritie EM12 MBE (1 Hz) fro a surface vessel, and the ap data are therefore statistically independent fro the EM3 easureents. Fig. 1 shows a contour ap of the area together with the AUV trajectory. The AUV travels fro an area with a relatively rough terrain to a flat area, where it oves in a lawn ower pattern, before returning to the rough area again. As all terrain navigation algoriths require soe terrain variation to wor, we expect poor terrain navigation perforance in the flat area, which was seen in [2, in which the sae data were used. B. Results In order to test the behavior of the algoriths on real AUV data, the two different state-space odels were ipleented in Matlab using the Bayesian Bootstrap particle 4

5 TABLE II FILTER DRIFT NOISE PARAMETERS USED IN MONTE CARLO RUNS. Method τ 1 [s τ 2 [s White noise paraeters ID [ q 11 q22 [/s PF2D [.5.5 (north, east) PF4 1 1 [.1.5 (surge, sway) PF4 1 1 [.1.5 (surge, sway) filter. Each ethod was extensively tested, in order to find the optial paraeter settings. The algoriths were started with an offset fro the true solution of 5 eters in each horizontal direction, and both variants were able to relatively quicly obtain good estiates in terrain suited for terrain navigation, when copared to the ground truth described above. Sequences of 1 seconds of navigation data were used each tie the algoriths were started, in order to siulate a real scenario in which the terrain navigation syste is started whenever a position fix is needed. As soon as a position estiate is obtained, this should be fed bac to the navigation syste. Since we are dealing with sequential Monte Carlo ethods, the estiates will be slightly different each tie the algoriths are run. It is therefore necessary to conduct Monte Carlo runs, where the algoriths are run a nuber of consecutive ties using the sae navigation data, to assess the quality of the two variants. Table II shows the drift noise paraeters used in the filters in the various runs. Notice the low white noise paraeters used in the iproved odel. Fig. 2 shows the average horizontal errors fro 25 Monte Carlo runs 1 2 seconds after the start of the run, when the vehicle is in an area well suited for terrain navigation. The red line, labeled PF2D, shows the results using the 2-diensional odel, whereas the blue and blac lines, labeled PF4, are the results obtained fro the iproved state-space odel, with drift paraeters τ 1 = τ 2 = 1 s and τ 1 = τ 2 = 1 s, respectively. These paraeters turned out to yield the best accuracy in the estiates. As can be seen in Fig. 2, the iproved filter odels see to yield a ore stable solution than the conventional one, which has a tendency of fluctuations. This is particularly evident in the interval between 2 and 4 seconds, where the estiate error fro the conventional odel suddenly increases for a while, before returning to around 5 eters after around 5 seconds. This behavior is also seen to soe extent in the results fro the iproved odel, but especially in the τ = 1 s case the fluctuations are of uch saller agnitude, yielding soother estiates. The reason for this fluctuating behavior is ainly that one has to use a uch higher white noise paraeter for the drift in the particle filter for the conventional odel than what is the case in the iproved odel, which has soe tie correlation in the process noise. This effect was evident throughout all our tests in the suited areas; the iproved filter odel estiates are soother and have a lower tendency of fluctuations and hence lower probability of filter divergence. Siilar results can be seen in Fig. 3, where corresponding results 3 4 seconds fro the start of the run are shown. Here the tendency of fluctuations in the conventional results are even ore evident. Though all of the variants yield estiates of high accuracy, within 1 Horizontal error () PF2D PF4, τ = 1 PF4, τ = t (s) Fig. 2. Coputational results fro 25 MC runs, 1 2 seconds fro start of run. Horizontal error () PF2D PF4, τ = 1 PF4, τ = t (s) Fig. 3. Coputational results fro 25 MC runs, 3-4 seconds fro start of run. eters, which is the horizontal ap resolution, the results fro the iproved odels are superior when it coes to soothness of the estiates. The estiates fro the iproved odel with drift paraeter τ = 1 s are the ost accurate and stable. As seen before in [1 and [2 the covariance estiates in Bayesian terrain navigation have proven to have a tendency of overconfidence, i.e. the estiated covariances are too low copared to the true uncertainties in the estiates. This was one of the rationales for deriving the iproved state-space odel. It turns out, however, that the covariance proble is not solved by the iproved odel. In fact, the estiated covariance is uch lower when the iproved filter odel is used. Fig. 4 and 5 show the estiated north and east offsets and their standard deviations for the conventional and iproved odels, respectively. These results are fro one single run with the particle filters. For a consistent estiate, the estiate should lie within the 1 σ bounds around 68% of the tie, if we assue a Gaussian distribution. This is achieved for the conventional odel, but for the iproved odel it is far fro true. This eans 5

6 5 95 North offset[ East offset[ Horizontal error () PF2D PF4, τ = 1 PF4, τ = Fig. 4. Estiated north and east offsets (blue lines) and standard deviations (green lines), conventional filter odel (single run) t (s) Fig. 6. Coputational results fro 25 MC runs, 9-1 seconds fro start of run. North offset[ East offset[ Fig. 5. Estiated north and east offsets (blue lines) and standard deviations (green lines), iproved filter odel with τ 1 = τ 2 = 1 s (single run). that there is still a discrepancy between the true syste and our odel. The iproved soothness of the estiates when using the iproved odel therefore coes at the cost of poorer covariance estiates. A higher covariance in the iproved odel can be attained by using a higher white noise paraeter, but this severely degrades the stability and soothness of the estiates. The behavior of the two odels in flat terrain is shown in Fig. 6. Here the vehicle is traversing the flat terrain in a lawn-ower pattern, as can be seen in Fig. 1. As expected, neither the conventional nor the iproved odel does well here, as the terrain does not have enough variation for the terrain navigation algoriths to obtain distinct fits. None of the results are accurate in this area, with typical horizontal uncertainties of 5 1 eters. It should be noted, however, that to soe extent the covariance atrices estiated by the particle filter are able to reflect the uncertainties in the estiates both for the conventional and the iproved odel. However, there is a tendency of overconfidence in both cases. Fro these results we conclude that nothing is gained in the flat area by using the iproved odel. On the other hand, nothing is lost either. The terrain siply does not contain enough inforation within the current sensor and ap accuracy for terrain navigation to wor satisfactorily. IV. CONCLUSIONS AND FUTURE WORK In this paper we have developed a new odel for Bayesian terrain navigation of AUVs by incorporating ore inforation about the inertial navigation syste than what is done in conventional terrain navigation. The new odel was developed in the vehicle body frae, using a first order Marov odel for the drift error, before transforing the variables into a locally flat earth frae. Tests done with a particle filter on real AUV data show that the new odel yields soother and ore accurate estiates in suited terrain, but the estiated covariances are far to sall. In other words, the estiates fro the proposed odel are highly overconfident. The iproved soothness and accuracy therefore coe at the cost of poorer covariance estiates. The difference fro the conventional odel is insignificant in flat terrain. The inconsistency is a ajor proble with the new odel and needs to be solved in future wor. The ain reason for the inconsistency is that the relatively siple filter odel does not odel the true syste accurately enough. One therefore has to include ore states in the state vector in an effort to obtain a ore realistic drift odel in the filter. As high diension particle filters require a very large nuber of particles, one ay have to resort to Rao-Blacwellization [17, [18, a cobination of Kalan and particle filtering. ACKNOWLEDGMENTS This wor is part of the UNaMap project, funded by the Norwegian Research Council, Kongsberg Maritie AS, and Kongsberg Defence & Aerospace AS. The wor is carried out in close cooperation with the Norwegian Defence Research Establishent (FFI). 6

7 REFERENCES [1 K. B. Ånonsen, O. Hallingstad, O. Hagen, and M. Mandt, Terrain aided AUV navigation - a coparison of the point ass filter and terrain contour atching algoriths, in Proceedings fro UDT Europe 25, Asterda, the Netherlands, June 25. [2 K. Ånonsen and O. Hallingstad, Terrain aided underwater navigation using point ass and particle filters, in Proceedings of the IEEE/ION Position Location and Navigation Syposiu 26, San Diego, CA, USA, April 26. [3 B. Jalving, K. Gade, O. Hagen, and K. Vestgård, A toolbox of aiding techniques for the HUGIN AUV integrated inertial navigation syste, in Proceedings fro IEEE Oceans 23, San Diego, CA, 23. [4 L. Hostetler, Optial terrain-aided navigation systes, in AIAA Guidance an Control Conference, Palo Alto, CA, [5 N. Bergan, Recursive bayesian estiation - navigation and tracing applications, Ph.D. dissertation, Departent of Electrical Engineering, Linöping University, Sweden, [6 I. Nygren and M. Jansson, Terrain navigation using the correlator ethod, in Position Location and Navigation Syposiu, PLANS 24, Monterey, CA, April 24. [7 B. Jalving, M. Mandt, O. Hagen, and F. Pøhner, Terrain referenced navigation of AUVs and subarines using ultibea echo sounders, in Procedings fro UDT Europe 24, Nice, France, 24. [8 D. Crisan and A. Doucet, A survey of convergence results on particle filtering ethods forpractitioners, Signal Processing, IEEE Transactions on [see also Acoustics, Speech, and Signal Processing, IEEE Transactions on, vol. 5, no. 3, pp , Mar. 22. [9 B. Jalving, Depth accuracy in seabed apping with underwater vehicles, in Proceedings fro Oceans 99, Seattle, WA, [1 Y. Bar-Shalo, X. Li, and T. Kirubarajan, Estiation with Applications to Tracing and Navigation. New Yor: John Wiley & Sons, 21. [11 M. Arulapala, S. Masell, N. Gordon, and T. Clapp, A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracing, IEEE Transactions on Signal Processing, vol. 5, no. 2, pp , 22. [12 A. Doucet, J. de Freitas, and N. Gordon, Sequential Monte Carlo Methods in Practice. New Yor: Springer Verlag, 21, ch. An Introduction to Sequential Monte Carlo Methods. [13 N. Gordon, D. Salond, and A. Sith, Novel approach to nonlinear/non-gaussian bayesian state estiation, in Radar and Signal Processing, IEE Proceedings F, vol. 14, no. 2, 1993, pp [14 B. Jalving, K. Gade, K. Svartveit, A. Willusen, and R. Sorhagen, DVL velocity aiding in the HUGIN 1 integrated inertial navigation syste, Modeling, Identification and Control, vol. 25, no. 4, pp , October 24. [15 K. Gade, Navlab, a generic siulation and post-processing tool for navigation, European Journal of Navigation, vol. 2, no. 4, pp , Noveber 24. [Online. Available: [16 K. Vestgård, R. Hansen, B. Jalving, and O. Pedersen, The HUGIN 3 survey AUV, in Proceedings fro ISOPE-21: Eleventh International Offshore and Polar Engineering Conference, Stavanger, Norway, June 21, pp [17 R. Chen and J. Liu, Mixture alan filters, Journal of the Royal Statistical Society, vol. 62, no. 3, pp , 2. [18 C. Andrieu, A. Docet, and E. Punsaya, Sequential Monte Carlo Methods in Practice. Springer-Verlag, New Yor, 21, ch. Sequential Monte Carlo Methods for Optial Filtering, pp