A Multi-Model EKF Integrated Navigation Algorithm for Deep Water AUV

Abstract

A novel integrated navigation algorithm, multi-model EKF (Extended Kalman Filter) integrated navigation algorithm, is presented in this paper for the deep water autonomous underwater vehicle. When a deep water vehicle is performing tasks in the deep sea, the navigation error will accumulate over time, if it relies solely on its own inertial navigation system. In order to get a more accurate position for the deep water vehicle online, an integrated navigation system is constructed by adding the acoustic navigation system. And because it is difficult to establish the kinematic model and the measurement model accurately for the deep water vehicle in the underwater environment, we propose the Multi-model EKF integrated navigation algorithm, and estimate the measurement errors of beacons online. Then we can estimate the position of the deep water vehicle more accurately. The new algorithm has been tested by both analyses and field experiment data (the lake and sea trial data), and results show that the multi-model EKF integrated navigation algorithm proposed in this paper significantly improves the navigation accuracy for the deep water vehicle.

Keywords

AUV EKF Integrated Navigation System Beacons Multi-model Estimation

1. Introduction

In the last decades, advances in marine technology have made it possible for people to explore the ocean. And ocean has been a field where many countries compete for resources. Currently, as an effective tool, AUV (Autonomous Underwater Vehicle) helps people explore marine resources, and gets more and more attention. Especially, in some developed countries such as USA, UK, etc., autonomous underwater vehicle has been widely used in both military and civilian fields. AUV is now being performing a variety of tasks as an important role, including oceanographic surveys, demining, and bathymetric data collection in marine and riverine environments [1 –3].

Deep water AUV performs tasks in the harsh environment, where the depth of water reaches 4000m, and it needs to locate its position in real time. Among the myriad of navigation systems and sensor devices, the Global Positioning System is a very popular choice. But for an underwater vehicle, GPS is not a solution due to the strong attenuation that the electromagnetic field suffers in water. High accuracy navigation positioning has become an urgent and difficult challenge for the deep water AUV [4, 5].

Some solutions have been sought in the past decades and there are two general classes of technologies commonly utilized for underwater navigation [6 –8]: acoustic navigation and inertial navigation. Acoustic navigation is exemplified by a LBL (Long Baseline) navigation system, which consists of a grid of acoustic transponders pre-surveyed onto the sea floor or ships. An underwater vehicle can triangulate its position using range measurements from these transponders. The distance between two transponders in the LBL system, known as the baseline length, varies from 100 meters to more than 6000 meters. LBL navigation system avoids the azimuth measurements between AUV and the underwater beacons, because the complexity of underwater environments makes a great challenge to measuring azimuth relying on the underwater sound. Especially when AUV is far away from the beacons, the errors of azimuth measurements are larger than the errors of range measurements. Therefore, the long baseline beacon communication with range-only measurements makes a great improvement for the reliability of measurement data, and the measuring principle is simple, so it is easy to deal with the measurement data. What we only need to do is that we pre-deploy 3 or 4 acoustic beacons on the seabed. And when the AUV is working, it doesn't need the surface vessels or other auxiliary equipment for the further service. So it can widely perform various tasks. But if the acoustic navigation system is solely used for a vehicle, there will be two apparent shortcomings or problems [9, 10]: (i) low data updating rate results that there is not a good continuity for the filter (for increasing the accuracy of navigation, usually, the filter technologies are used for the data fusion based on the information generating from sensors); (ii) sometimes outliers are hard to eliminate (outliers should be deleted before the measurements update the filter, because outliers would decrease the performance sharply). Of course, there are other many factors, such as, the measurement transmission delays for the computation of the AUV position, the multi-paths, the nonlinear measurement equations, the non-constant sound speed in the explored zone, and the effect of water currents. But these factors are the reasons why the navigation accuracy decreases, and we cannot establish accurate models for all the factors, because of the complexity. The shortcomings i and ii are the combined effects of all the known and unknown factors. That is, if we can effectively deal with the shortcoming i and ii, it is equivalent to indirectly dealing well with the factors.

Inertial navigation systems rely on precision instrumentation, such as accelerometers and gyros or magnetic compasses, to measure the AUV linear accelerations and attitudes (pitch, roll and yaw angles and corresponding angular rates) in order to estimate the AUV position [11]. Based on the dead reckoning, with heading and pitch angle and forward speed information, navigation algorithms estimate the position of the AUV. However due to sensor drift, dead reckoning algorithm alone is not sufficient, as navigation errors will accumulate over time.

In order to improve the navigation performance, aiding devices have been considered in the design of integrated navigation solutions [12 –15].

Consider such an integrated navigation system where there is a set of fixed landmarks arranged in an LBL configuration, where acoustic beacons or transponders are installed, and assume that the vehicle measures the range to each of the acoustic beacons or transponders, as depicted in the Fig.1. Further assume that the vehicle is equipped with an inertial measurement unit (IMU), consisting of two triads of orthogonally mounted accelerometers and rate gyros or magnetic compasses, and an attitude and heading reference system (AHRS). This integrated navigation system utilizes range of an AUV from a single acoustic beacon at a known location, along with velocity (heading and speed) of the AUV, to estimate the position of the AUV. It enables an AUV to compute its position in real time. And this integrated navigation system performs well than the single LBL navigation system or inertial navigation system.

Figure 1.

The integrated navigation system for the deep water vehicles

Sometimes, there are a lot of redundant measurements for the integrated navigation system, so we may obtain the more accurate AUV position possibly. In the harsh environments of deep sea, it doesn't ensure that there are enough and stable beacon echoes or measurements we can detect for every sampling period; sometimes, there is no measurement detected during a sampling period in the most extreme case. In other words, there are three or four measurements theoretically for the calculating equations during the sampling period. The measurement means the elapsed time of the process that AUV transmits a signal (sound waves) to the beacons, beacons receive the signal and respond, and then AUV receives the response. If there are only one or two beacons to respond, then the AUV position cannot be estimated by solving a suitable geometric algorithm (static position estimation). The less but still valuable measurements cannot be used effectively. It's a waste of information resources. Meanwhile, in the acoustic navigation system, underwater sound speed directly determines the navigation accuracy of the range measurements. And the speed of sound in the sea cannot be viewed as a constant value. It changes along with the space and time. But the geometric algorithm to solve the position of AUV neglects the measurement errors about the sound speed, namely, the algorithm cannot effectively deal with the errors of time-measurement or range-measurement.

In order to avoid the flaws mentioned above, the literatures [16, 17] solve the problems by EKF integrated navigation algorithm. Literature [17] relies on acoustic round-trip travel time measurements that are processed by EKF algorithm. The EKF has the ability to incorporate the kinematic model into the estimation algorithm, improving the navigation accuracy. What's more, this algorithm can estimate the AUV position with a small amount of updating data (one or two measurements), and also adaptively estimate the measurement errors.

But for the harsh underwater environment, even though EKF integrated navigation system is adopted, it is also difficult to establish accurate AUV kinematic model. Because there are many unknown factors that affect the AUV movement in the deep sea. That will lead to a great impact on the navigation accuracy, and reduce the estimation performance. For this problem, the multiple models navigation algorithm is considered as a very effective algorithm for the underwater applications [18 –23]. The multiple models navigation is based on the fact that the behavior of an AUV cannot be characterized at all times by a single model, but a finite number of models can adequately describe its behavior in different stages/regimes. Then during the subsequent filtering process, it reasonably and adaptively chooses or switches appropriate model from the models set for the filter, and then the accuracy and performance of navigation may be improved significantly. But for the multiple models navigation, there are also some flaws. It is difficult to establish the appropriate models set a priori, and how to effectively and quickly choose or switch the model with low computational complexity, in order to meet the real-time requirement, especially when the number of models in the models set is relatively large. For the problems mentioned above, based on the multiple models algorithm, PDA (Probabilistic Data Association) theory [24, 25] and EKF algorithm, this paper proposes a novel navigation algorithm, the multi-model EKF integrated navigation system, which will be described in detail latter. After simulations and analyses of the real lake and sea trial data, this novel algorithm significantly improves the AUV navigation accuracy, compared with standard EKF integrated navigation algorithm.

The structure of this paper is shown as follow. Section 2 gives a brief description of the integrated navigation system. Section 3 introduces the EKF integrated navigation system with the harsh underwater environment, and the problems/flaws of current underwater navigation algorithms. Then we provide the novel algorithm in the section 4. The analyses and simulations in the section 5 show a notable improvement in the performance of the novel algorithm over the standard EKF integrated navigation algorithm with the lake and sea trial data. Finally, section 6 summarizes the main conclusions and results of the paper.

2. Integrated navigation system

Integrated navigation system we had mentioned in the paper is composed of the inertial navigation system and acoustic navigation system (LBL). Prior to AUV performing tasks, we should calibrate some key parameters about navigation system, such as the AUV heading correction coefficient, the speed correction coefficient about the trial sea field, four acoustic beacons (the positions of four acoustic beacons need to ensure that the AUV spatial position state is observable [26]), and the corresponding sound speed of the trial field [27, 28].

What I want to emphasize is that the sound speed changes along with the different sea area and the different depth of the same sea area. If there is something wrong or a relative large error for the sound speed calibration, it will lead to a very serious impact on the final filtering accuracy. Here we will give a brief and necessary description about the transmission of signals between AUV and beacons. Firstly, AUV runs by recursively calculating the current position of AUV along with the inertial navigation. Meanwhile AUV transmits an acoustic pulse of certain frequency with a fixed period. If the beacons detect the signal from AUV, after a short processing delay, the beacons response to AUV with special frequency signals. At last the AUV receives the corresponding signals with special frequency flags, which can distinguish different beacons. With the sound speed, AUV can calculate the slant range to the beacons. Then the more accurate position information of AUV will be estimated by the extended Kalman filtering algorithm.

With the Fig.2, here we firstly introduce the process of (multi-model) EKF integrated navigation system for better understanding the navigation system, and some details about the sub-processes will be discussed in the follow sections. When AUV reaches the scheduled depth of the sea, it runs with a default initial position (it is not accurate, and there may be a large error). The initial estimated position of AUV is relative inaccurate, before the integrated navigation system starts to work. As shown the step of “State=0” in the Fig.2, it means that there is a large uncertainty for the AUV position, and that the AUV position only be obtained by the inertial navigation system (the integrated system has not started). If AUV receives any signal from beacons, integrated navigation system starts immediately, and it should be viewed as the initial stage of the integrated navigation system, as shown the step of “initial stage of “INS””. The purpose of the initial stage of the integrated navigation is to get more accurate initial position for the subsequently nonlinear filtering process. There are four beacons for the integrated system, so three or four beacons may simultaneously make responses to the AUV during the same sampling period. As long as it meets this requirement, then we can solve the AUV's current position by a suitable geometric algorithm. If both the number and the accuracy of the AUV's positions by the geometric algorithm meet the requirements in the continuous four sampling periods, we set the AUV state as the stage of integrated navigation, as shown the stage of “INS”. No matter which stage (the stage of INS or the initial stage), if AUV receives a signal from any beacon, it will adopt multi-model EKF algorithm to estimate the AUV's position, when AUV is performing tasks. Here we provide a brief description of the filter's measurement process. After AUV transmits the signal to all the beacons, if a beacon detects the signal, then it will take $t_{2}$ to deal with the signal, and then respond to the signal. When AUV receives the special signal from the beacon, the round-trip time is $t_{4}$ . And the structure of the signal includes some important information, as shown follow.

Figure 2.

Program flow chart, “State=0” means that AUV runs only with dead reckoning algorithm (inertial navigation system), and that the initial position of the AUV is inaccurate. “INS” is the abbreviation of “integrated navigation system”. N1 and N2 are the thresholds. N1=3, N2=4. The step of “number≥N1” means that we can triangulate AUV position using three range measurements from the beacons, if it holds. The step of “number≥N2” means that the navigation stage switches from the initial stage of integrated navigation to the stage of integrated navigation, if both the number and the accuracy of the AUV positions by the geometric algorithm meet the requirements in the continuous four sampling periods.

Structure of a signal: {the index of a beacon, the round-trip time, and the spatial three-dimensional coordinates of the beacon}.

With the information above, we can estimate the AUV's position by multi-model EKF algorithm.

3. The problems of EKF algorithm and standard multiple models navigation algorithm

In the underwater application, so far, there isn't a navigation algorithm which can adapt to any underwater environment, obtain high navigation accuracy and have a good performance. But there is still something that we can do for the navigation technology progress by the way that we are acquainted with standard algorithms, know comprehensively their advantages and disadvantages, and then improve them or add more devices allowed for them to adapt to new and harsh environment. So in this section, combined with the special underwater environment, firstly we introduce the EKF algorithm and multiple models navigation algorithm, and point out their shortcomings. Then we propose the principle of our new algorithm, which is core idea of this article. At the fourth section, we introduce our novel algorithm in detail.

3.1. The design of EKF

We only consider the AUV's movement in the horizontal plane. Assume that AUV moves along a line with the constant velocity. The AUV state is a two-dimensional state vector $X_{k} = {[x_{k}^{1} x_{k}^{2}]}^{T}$ , $x_{k}^{1}$ is the abscissa value of the horizontal Cartesian coordinate system, and $x_{k}^{2}$ is the ordinate value at time k. We take the velocity component of the actual measurement velocity vector in the Cartesian coordinate system as the input parameters. The observation is the distance of signal traveling in the water $y = c \cdot Δ t$ . $c$ is the sound speed of the trial sea field, and $Δ t$ is the time of signal traveling in the water, $Δ t = t_{4} - t_{2}$ . Then the discrete model of system is established as shown follow.

X_{k + 1} = A X_{k} + B u_{k} + w_{k}^{1}

(1)

\begin{matrix} y_{k} = h (X_{k}) + w_{k}^{2} & (1) \\ y_{k} = 2 \sqrt{d_{1}^{2} + d_{2}^{2} + d_{3}^{2}} + w_{k}^{2} & (2) \end{matrix}

(2)

B = [\begin{matrix} T & 0 \\ 0 & T \end{matrix}]

(3)

u_{k} = {[\begin{matrix} v_{k} \sin θ_{k} & v_{k} \cos θ_{k} \end{matrix}]}^{T}

(4)

\begin{matrix} d_{1} = x_{k}^{1} - x_{1}^{m} & (1) \\ d_{2} = x_{k}^{2} - x_{2}^{m} & (2) \\ d_{3} = x_{k}^{3} - x_{3}^{m} & (3) \end{matrix}

(5)

For simplicity, the notation is regrouped for clarity in Table 1.

Table 1.

AUV model and multi-model EKF algorithm notation

Variable	Description
X_k	State vector (position) at time k
A	State transition matrix. Here it is an identical matrix I_2×2
B	Input matrix of the AUV velocity
u_k	AUV velocity (AUV velocity includes water current velocity factors)
y_k	Measurements
h(⋅)	Measurement function
H_k	The linearization of h(⋅)
w_k¹	Process noise (a white noise sequence of normal random variable with zero mean and covariance Q_k)
w_k²	Measurement noise (a white noise sequence of normal random variable with zero mean and covariance R_k)
T	Sampling period
v_k	The actual speed of AUV (section 3)
θ_k	Heading of AUV
d₁	Abscissa difference from AUV to beacon m
d₂	Ordinate difference from AUV to beacon m
d₃	Depth difference between AUV and beacon m
x_k ¹	Abscissa value of AUV in the horizontal Cartesian coordinate system
x_k ²	Ordinate value of AUV
x_k ³	Depth of AUV
x¹_m,x²_m,x³_m	Position of beacon m (constant value)
Q_k	Process noise covariance matrix
R_k	Measurement noise covariance matrix
P_k	The state covariance matrix
$X_{k + 1 / k}$	The predicted state
$P_{k + 1 / k}$	The predicted state covariance
$y_{k + 1 / k}$	The predicted measurement
S_k	The innovation covariance
K_k	Kalman gain matrix
v_k	The weighted innovation (section 4)
$β_{k}^{i}$	Associated probability of ith ( $i = 1, \dots, N$ ) measurement at time k
$β_{k}^{0}$	The probability if there are not measurements falling into the validation region of the trajectory.
$v_{k}^{i}$	ith innovation
y_i	ith validated measurement
$X_{k / k}$	The state vector after measurement updating
$P_{k / k}$	The state covariance matrix after measurement updating
b	Coefficient (there is no measurement that originates from the target)
e_i	Coefficient (the posteriori probability that $y_{i}$ originates from the target)
λ	The density of clutter
P_D	Detection probability of a target of sensors
P_G	Probability of the correct measurements falling into the validation region

What's more, the values of some parameters can be got by sensors online implemented on the AUV, such as $v_{k}, θ_{k}, x_{k}^{3}$ , or calculated by the measurements, such as $u_{k}$ .

What I need to emphasize is that Equation (2) neglects the influence factors of AUV movement. The routes of sound wave traveling from AUV to beacons and from beacons to AUV are treated as the same routes. This approximation is feasible when the AUV's speed is relatively small or the distance between AUV and beacons is relatively close. If the conditions above are not met, we need to adopt new more accurate measurement equations. As shown as the Fig.3, assume that AUV runs from left to right, and transmits a signal at point A when time is 0. After some time $t_{1}$ , a beacon receives the signal and responses to AUV. When AUV receives the signal from the beacon at time $t_{4}$ , and the AUV's position has changed, at point B. If AUV's speed is 2m/s, and the time costing during the process is 8s, so the AUV movement distance is 16m. This is not a desirable result for underwater high accuracy navigation and positioning. So if AUV's speed is large or the distance from AUV to beacons is remote, we should adopt the modified measurement equations as shown as follow.

Figure 3.

It is the measurement error analysing, when AUV is running. Assume that AUV moves along a line with the constant velocity. At time 0, AUV transmits a signal at point A. After time $t_{1}$ , a beacon receives the signal and after a processing delay $t_{2}$ , the beacon responses to the AUV. At time $t_{4}$ , AUV receives the special signal from the beacon, where the position of AUV is point B. So the time interval that sound wave travels from point A to the beacon is $t_{1}$ , and the time interval that sound wave travels from the beacon to point B is $t_{3} = t_{4} - t_{1} - t_{2}$ . For simplicity, $y_{k}^{1}$ denotes the distance between point A and the beacon, and $y_{k}^{2}$ denotes the distance between the beacon and point B. Here it should be noted that the values of $t_{2}$ and $t_{4}$ are known ( $t_{2}$ is a constant parameter which denotes the time costing of a beacon processing the signal; $t_{4}$ is the time measurement by the sensors of AUV). The values of $t_{1}$ and $t_{3}$ are unknown (the sum of $t_{1}$ and $t_{3}$ is known), so we cannot obtain the values of $y_{k}^{1}$ and $y_{k}^{2}$ with the method of the time multiplying the sound speed (but we can obtain the value of the sum of $y_{k}^{1}$ and y_k²).

y_{k} = y_{k}^{1} + y_{k}^{2} + w_{k}^{2}

(6)

\begin{matrix} y_{k}^{1} = \sqrt{{(d_{11})}^{2} + {(d_{12})}^{2} + {(d_{13})}^{2}} & (1) \\ y_{k}^{2} = \sqrt{{(d_{21})}^{2} + {(d_{22})}^{2} + {(d_{23})}^{2}} & (2) \\ d_{11} = x_{k}^{1} - v_{e} t_{4} - x_{1}^{m} & (3) \\ d_{21} = x_{k}^{1} - x_{1}^{m} & (4) \\ d_{12} = x_{k}^{2} - v_{n} t_{4} - x_{2}^{m} & (5) \\ d_{22} = x_{k}^{2} - x_{2}^{m} & (6) \\ d_{13} = x_{k}^{3} - x_{3}^{m} & (7) \\ d_{23} = x_{k}^{3} - x_{3}^{m} & (8) \\ v_{e} = v_{k} \sin θ_{k} & (9) \\ v_{n} = v_{k} \cos θ_{k} & (10) \end{matrix}

(7)

where y_k¹ denotes the distance between point A and the beacon at time k, and $y_{k}^{2}$ denotes the distance between the beacon and point B. $d_{11}$ , $d_{12}$ and $d_{13}$ denote the abscissa, ordinate and depth difference between point A and the beacon. $d_{21}$ , $d_{22}$ and $d_{23}$ denote the abscissa, ordinate and depth difference between the beacon and point B. $x_{k}^{1}$ , x_k² and $x_{k}^{3}$ denote the position of AUV, $x_{1}^{m}$ , $x_{2}^{m}$ and $x_{3}^{m}$ denote the position of beacon m. $v_{e}$ is the eastward speed of AUV (speed of eastward movement in the horizontal Cartesian coordinate system), and $v_{n}$ is the northward speed of AUV (speed of northward movement). $t_{4}$ is the travel time of the sound wave between AUV transmitting the signal and AUV receiving the signal. $v_{k}$ is the actual speed of AUV, and $θ_{k}$ is the heading angle of AUV.

It can be obtained from Equation (6) and (7), that the measurement equation is a nonlinear equation, so we need to linearize the measurement equation for the EKF filter. In summary, the process of EKF filter is shown as follow.

Step1: Set the initial state $X_{0 / 0}$ and $P X_{0 / 0}$ .

Step2: Time updating; the mean $X_{k + 1 / k}$ and the covariance $P X_{k + 1 / k}$ of AUV propagate with time before measurement updating according to the following “time update” equations.

X_{k + 1 / k} = A X_{k / k} + B u_{k}

(8)

P X_{k + 1 / k} = A P_{k / k} A^{T} + Q_{k}

(9)

Step3: Measurement updating; when a measurement is received, the mean $X_{k + 1 / k + 1}$ and covariance $P X_{k + 1 / k + 1}$ are given by the following “measurement update” equations.

K_{k + 1} = P_{k + 1 / k} H_{k + 1}^{T} {(H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T} + R_{k + 1})}^{- 1}

(10)

X_{k + 1 / k + 1} = X_{k + 1 / k} + K_{k + 1} (y_{k + 1} - h (X_{k + 1 / k}))

(11)

P_{k + 1 / k + 1} = (I - K_{k + 1} H_{k + 1}) P_{k + 1 / k}

(12)

Step4: Step to Step2 for the next sampling period.

3.2. The problems of EKF navigation algorithm for underwater environment

For the harsh underwater environment, there are some problems about the standard EKF navigation algorithm. There are a lot of impulses in the filtering trajectory. The reasons are that there are too many unknown disturbance factors, and that we cannot establish the suitable mathematical models for those factors in the underwater environment in the kinematic equation. And the models are time-varying and space-varying. So we need a variable model to approximate the navigation process. Meanwhile the sensor itself is also a significant factor for the measurement model, because there are many internal and external factors affecting the measurement accuracy, such as faults, noise, clutters and disturbance, especial the position information of beacons. It is difficult and impossible to establish a model for the every factor. For example, in the measurement equation, the sound velocity is time-varying and space-varying in the water. If we want to get the map of sound velocity about an area, we need huge amounts of data about the time and space information of the area. For another example, if there are relatively large calibration errors for the position of a beacon, the range measurements between AUV and this beacon are unreliable, which will deteriorate the performance of filters and decrease the navigation accuracy. Therefore, it is necessary to estimate the key parameters of the measurement equation.

3.3. Multiple models navigation algorithm

Multiple models navigation for an AUV is similar to the multiple models target tracking for a single target. Multiple models navigation includes two important modules: the models sets and managing the models set [18]. The models set means that we should establish a finite number of possible models for an AUV. The values of models set for the navigation algorithm are that the occurrence of AUV maneuvers or measurement errors can be explicitly included in the kinematic equation or measurement equation. Every model corresponds to a behavior mode, and the mode is assumed to be among the possible modes of the set of all models states at all times, for example of a constant velocity model and several maneuver models. Managing the models set indicates that how to switch or choose the appropriate model from the models set for the filter at every measurement updating process. The standard managing method is that the models are governed by a discrete stochastic process with a set of transition probabilities. The transition probability matrix governing the Markov chain and the priori probability of each model are known. The probabilities of models are also propagating through the transition probability matrix, along with the filter updating process. And based on the values of probabilities of models, we can choose or fuse the models with different filter technologies such as KF, EKF, UKF or PF, to obtain better navigation performance.

3.4. The problems of multiple models navigation algorithm

As we all know, the larger the number of the models is, the higher the navigation accuracy of an AUV is, the better the navigation performance is, but the heavier the computational load is. Some algorithms are proposed to achieve an excellent compromise between performance and complexity/computational load, because performance and complexity is also an important issue for solving the navigation problems in the underwater environment. What's more, because navigation system equation and measurement equation have time-varying characteristics and there are some calibration errors for some key parameters, there are a lot of outliers in the measurement data, which should be deleted in advance. So how effectively eliminating the outliers is another important issue.

For the above problems, we propose the multi-model EKF integrated navigation algorithm. Firstly, it needs to establish models set for the system: (i) for the system equation, based on the sampling period $T$ , we need to establish simple and effective models set; (ii) for the measurement equation, we estimate the measurement covariance matrix adaptively. Secondly, we create a filter for every model, and they estimate the position of AUV by themselves. At last, based on the probabilities of models, the weighted output of all filters is a more accurate navigation position. What's more, based on the measurement covariance matrix information, the new algorithm eliminates the outliers by the fixed and suitable threshold. In addition, the probabilities of models are determined by a new algorithm, PDA. Namely, here we provide a new unique aspect of choosing the models or fusing the results of different filters corresponding to different models.

4. Multi-model EKF navigation algorithm

For the case of the uncertain system model, according to the scope of the modeling error, multi-model EKF algorithm creates several filters for the different models which have been established before performing the task. Every filter runs by itself, and is assigned a weight when the new measurements are detected. At last, we view the weighted sum of all filters as the filtering output of the algorithm. The most important steps are that we should establish suitable models set in advance and that we need to assign a reasonable probability to the every model/filter, as the probability means that the filtering result of the corresponding model accounts for the really existed but unknown result by weight. The algorithm of assigning probability is based on the PDA theory [25]. Moreover, in order to further improve navigation accuracy, multi-model EKF navigation algorithm also adaptively estimates the measurement error covariance matrix and eliminates outliers.

4.1. PDA theory

PDA is a suboptimal approach to the problems of target tracking when the source of the measurement data is uncertain. In the single target tracking system, PDA is an effective and practical data fusion algorithm, so it is widely used for tracking a single target or tracking sparse multiple targets.

PDA's basic theory: in the clutter environment, one and only one target exists, and its trajectory has been created. Due to the influence of random factors, at any time, there are more than one measurements within the valid predicted region. The measurements may come from clutters, or may be the noisy measurements originating from the target, but no target signature information is used to distinguish them. So we view indiscriminately and qualitatively all the measurements with the same properties, which are within the valid predicted region. Then we distinguish them quantitatively by the weight (we don't care that a specific measurement originates from clutters or the target). The last output is the weighted sum of all measurements.

Assume that there are m measurements $y_{i} (i = 1, \dots, m (k + 1))$ falling into the threshold (with the valid predicted region) at time k+1, and then the process of PDA is shown as follow:

Step1: Time updating (predicting):

X_{k + 1 / k} = A X_{k / k} + B u_{k}

(13)

P_{k + 1 / k} = A P_{k / k} A^{T} + Q_{k}

(14)

y_{k + 1 / k} = h (X_{k + 1 / k})

(15)

S_{k + 1} = H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T} + R_{k + 1}

(16)

Step2: Measurement updating (correcting):

X_{k + 1 / k + 1} = X_{k + 1 / k} + K_{k + 1} v_{k + 1}

(17)

v_{k + 1} = \sum_{i = 1}^{m (k + 1)} β_{k}^{i} v_{k}^{i}

(18)

v_{k + 1}^{i} = y_{i} - y_{k + 1 / k}, i = 1, …, m (k + 1)

(19)

P_{k + 1 / k + 1} = β_{k + 1}^{0} (k + 1) P_{k + 1 / k} + (1 - β_{k + 1}^{0}) P_{k + 1 / k + 1}^{c} + {\tilde{P}}_{k + 1}^{​}

(20)

P_{k + 1 / k + 1}^{c} = P_{k + 1 / k} - K_{k + 1} S_{k + 1} K_{k + 1}^{T}

(21)

{\tilde{P}}_{k + 1}^{​} = K_{k + 1} [\sum_{i = 1}^{m (k + 1)} (β_{k + 1}^{i} v_{k + 1}^{i} v_{k + 1}^{i T} - v_{k + 1} v_{k + 1}^{T})] K_{k + 1}^{T}

(22)

β_{k + 1}^{i} = {\begin{matrix} \frac{e_{i}}{b + \sum ​_{j = 1}^{m (k + 1)} e_{j}} \begin{matrix} ​ & i = 1, …, m (k + 1) \end{matrix} \\ \frac{b}{b + \sum ​_{j = 1}^{m (k + 1)} e_{j}} \begin{matrix} \begin{matrix} ​ & i = 0 \end{matrix} & \begin{matrix} ​ & ​ \end{matrix} \end{matrix} \end{matrix}

(23)

b = λ | 2 π S_{k + 1} |^{1 / 2} \frac{1 - P_{D} P_{G}}{P_{D}}

(24)

e_{i} = \exp {- \frac{1}{2} v_{k + 1}^{i T} S_{k + 1}^{- 1} v_{k + 1}^{i}}

(25)

There are several similar characteristics between target tracking by PDA algorithm and navigation by Multi-model EKF algorithm. We can view the estimation of AUV trajectory as the estimated state of a single target, and view the corresponding different predicted values of system models as the measurements of a true target. In the navigation system, if there are $N$ filters, then there are N different predicted values, and there are N innovations of measurements.

As shown as the formula (19), $v_{k + 1}^{i}$ is the innovation of the ith system model. What we need to emphasize is that all the “measurements” mentioned here all fall into the threshold, that is to say, they are all the actual measurements (predicted values of system models), and there is no clutter or false report. So it is not necessary to know the value of $b$ , and under this definition, we can set

\frac{1 - P_{D} P_{G}}{P_{D}} = 0

(26)

where the value of P_D (the detection probability of a target) is 1 and the value of P_G (the gate probability that the correct measurements fall into the valid predicted region) is also 1. Namely, it denotes that the AUV (“the target”) must be detected and the measurements originating from AUV must fall into the valid predicted region. Here we simplify the problems reasonably, based on the large amount of experiment experience.

After a normalization process, $β_{i}$ the probability of ith system model is

β_{k + 1}^{i} = \frac{e_{i}}{\sum_{j = 1}^{N (k + 1)} e_{j}} \begin{matrix} ​ & i = 1, …, N (k + 1) \end{matrix}

(27)

e_{i} = \exp {- \frac{1}{2} v_{k + 1}^{i T} S_{k + 1}^{- 1} v_{k + 1}^{i}}

(28)

This is the basic theory of PDA, and the probability calculation from PDA theory to Multi-model EKF navigation algorithm. Then in the following, the Multi-model EKF navigation algorithm will be discussed in more detail: the establishment of multi-model system and estimating algorithm (design of filter).

4.2. Multi-model navigation system

For the uncertain system, multi-model means to establish several suitable reference system models. For the formula (1), the variation of the statistical properties of parameter $w_{k}^{1}$ reflects the variation of the system process. Assume that $w_{k}^{1}$ is the zero-mean Gaussian white noise, $E (w_{k}^{1} \cdot w_{k}^{1 T} = Q_{k})$ . And based on the scope of system process noise, we can initially determine the value $Q_{m i n}$ and $Q_{m a x}$ ( $Q_{m i n} \leq Q_{k} \leq Q_{m a x}$ ). $Q_{m i n}$ means the minimum value, and always, the value of $Q_{m i n}$ can be set as 0 (matrix), because sometimes we consider that one of the models in the models set is the true model of the system and that there is no model error. $Q_{m a x}$ means the maximum value, and it is somewhat difficult for determining the value of $Q_{m a x}$ . Here we provide two methods to determine the value of $Q_{m a x}$ : (i) analytically calculating the value of $Q_{m a x}$ in the aspect of deriving the rigorous mathematical formula; (ii) for simplicity, roughly determining the relatively larger scope for considering more unforeseeable circumstances and uncontrollable factors. For the first method, the continuous-time linear system model i is described by:

\dot{x} (t) = A (t) x (t) + B (t) u (t) + F_{i} (t) w (t)

(29)

w (t) \sim N (0, q_{i} (t))

(30)

E [w (t) w^{T} (τ)] = q_{i} (t) δ (t - τ)

(31)

where $x (t)$ is the $n \times 1$ system state vector at time t, $u (t)$ is the $r \times 1$ system input vector (control vector), and $w (t)$ is the $p \times 1$ process noise. $A (t)$ , $B (t)$ and $F_{i} (t)$ are the $n \times n$ , $n \times r$ and $n \times p$ matrix respectively. $q_{i} (t)$ denotes the process noise covariance matrix for the continuous-time system. The discrete-time process of Equation (29) is described by:

x (t_{k + 1}) = Φ (t_{k + 1}, t_{k}) x (t_{k}) + \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, τ) [B (τ) u (τ) + F_{i} (τ) w (τ)] d τ

(32)

where $Φ (t_{k + 1}, t_{k})$ is the transition matrix at time step $t_{k}$ to the state at time $t_{k + 1}$ ,

Φ (t_{k + 1}, t_{k}) = e^{\int_{t_{k}}^{t_{k + 1}} A (t) d t}

(33)

So the value of w_k¹ at time $t_{k + 1}$ in the Equation (1) has the relationship,

w_{k + 1}^{1} = \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, τ) F_{i} (τ) w (τ) d τ

(34)

E [w_{k + 1}^{1}] = \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, τ) F_{i} (τ) E [w (τ)] d τ = 0

(35)

\begin{array}{l} E [w_{k + 1}^{1} w_{k + 1}^{1 T}] = E [\int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, t) F_{i} (t) w (t) d t \cdot \int_{t_{j}}^{t_{j + 1}} w^{T} (τ) F_{i} ​^{T} (τ) Φ^{T} (t_{k + 1}, τ) d τ] \\ = \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, t) F_{i} (t) [\int_{t_{j}}^{t_{j + 1}} E [w (t) w^{T} (τ)] F_{i} ​^{T} (τ) Φ^{T} (t_{k + 1}, τ) d τ] d t \\ = \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, t) F_{i} (t) [\int_{t_{j}}^{t_{j + 1}} q_{i} (t) δ (t - τ) F_{i} ​^{T} (τ) Φ^{T} (t_{k + 1}, τ) d τ] d t \\ = \int_{t_{k}}^{t_{k + 1}} Φ (t_{k + 1}, t) F_{i} (t) q_{i} (t) F_{i} ​^{T} (t) Φ^{T} (t_{k + 1}, t) d t \\ = Q_{k + 1, i} \end{array}

(36)

So, with the Equations (32 –36) and the time intervals between measurements, we can determine the accurate value of $Q_{k + 1, i}$ . Compared with the complex and accurate first method, the second method is more rough and easier to implement. For the second method, in order to deal with the more unforeseeable circumstances and uncontrollable factors, such as the calibration errors, wrong speed value of sound, sensor drift, and large errors for the beacons, we set a relatively large value for $Q_{m a x}$ . For example, if the maximum speed of an AUV is known as $v_{m a x}$ , then we can set $Q_{m a x}$ as $I \cdot {(v_{m a x} T)}^{2}$ , where $T$ is the sampling period, and I is the identity matrix. Then, we can take an appropriate group of values about $Q_{k + 1}$ . An example is shown as follow:

\begin{array}{l} Q_{k + 1, i} = \frac{N + 1 - i}{N} Q_{\min} + \frac{i - 1}{N} Q_{\max}, \\ i = 1, 2, …, N \end{array}

(37)

where N is the number of models in the models set. Sometimes, we don't know the number of models which is need for the multi-model system, and we also are not necessary to know the number, because of so many uncertain factors. But here, we give a suggested value: $N = 10$ for this case, $v_{m a x} \cdot T \leq 100 m$ .

We should establish a filter corresponding to $Q_{i}$ respectively, so there are all $N$ filters running simultaneously for the navigation system. At last, we choose the second method to determine the models set. Because by analyzing the simulation results, the second method is effective, the computational load of the second method is relatively less, and the first method neglects the some important factors, such as the calibration errors, wrong speed value of sound, sensor drift, and large errors for the beacons, which is not desired in the application.

4.3. The dynamic estimation of measurement error covariance matrix

Similar to the system model, if the measurement model is not accurate, it also affects the filtering accuracy of navigation trajectory. And we know the measurement model also changes along with the time and space. Statistical properties $w_{k}^{2}$ of the measurement equation can reflect the variation of measurement model, so we can design an adaptive filter for estimating statistical properties of $w_{k}^{2}$ , $E (w_{k}^{2} \cdot w_{k}^{2 T}) = R_{k}$ .

R_{k + 1} = \frac{1}{k'} [(k' - 1) R_{k} + H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T}]

(38)

Where $k^{'}$ is the accumulated number of measurement updating during the integrated navigation stage.

4.4. Eliminating outliers

Not all of the data are the correct data in the actual measurement data, because many unknown factors may cause bad data deviating from the predicted trajectory. We call those bad data as outliers. These outliers cannot be used to update the filter; otherwise it would deteriorate seriously the filtering accuracy and cause the fluctuations of the trajectory. So it is necessary for us to eliminate the outliers firstly, only reserve the reasonable data for updating the filter after we get the raw data.

The algorithm of eliminating outliers:

v_{k + 1} = y - y_{k + 1 / k}

(39)

v_{k + 1}^{T} \cdot S_{k}^{- 1} \cdot v_{k + 1} \geq γ

(40)

$γ$ is the gate threshold. If the formula (40) holds, the measurement y is viewed as outliers. We should eliminate those data including y, which is not our desirable data for the filters.

Based on the PDA theory, we view the multi-model EKF algorithm in a new aspect, and determine the probabilities of models by a relatively simple and effective principle. Meanwhile, we create a reasonable models set for the system models and estimate the error information of measurement model online. Here, we describe multi-model EKF integrated navigation algorithm follow.

Algorithm Description:

Step1: Time updating (predicting the state and measurement):

X_{i} ​_{, k + 1 / k} = A X_{i} ​_{, k / k} + B u_{k}

(41)

P_{i} ​_{, k + 1 / k} = A P_{i} ​_{, k / k} A^{T} + Q_{k, i}

(42)

y_{i} ​_{, k + 1 / k} = h (X_{i} ​_{, k + 1 / k})

(43)

K_{i, k + 1} = P_{i} ​_{, k + 1 / k} H_{i, k + 1}^{T} {(H_{i} {​_{,}}_{k + 1} P_{i} ​_{, k + 1 / k} H_{i, k + 1}^{T} + R_{k + 1})}^{- 1}

(44)

X_{i} ​_{, k + 1 / k + 1} = X_{i} {​_{,}}_{k + 1 / k} + K_{i} ​_{, k + 1} (y_{k + 1} - y_{i} ​_{, k + 1 / k})

(45)

P_{i, k + 1 / k + 1} = (I - K_{i, k + 1} H_{i, k + 1}) \cdot P_{i, k + 1 / k}

(46)

Step2: Calculating probabilities (calculating the innovations between predicted measurements and actual measurement, and the probabilities):

v_{k + 1}^{i} = y_{k + 1} - y_{i, k + 1 / k}, i = 1, …, N

(47)

S_{k + 1} = H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T} + R_{k + 1}

(48)

e_{i} = \exp {- \frac{1}{2} v_{k + 1}^{i T} S_{k + 1}^{- 1} v_{k + 1}^{i}}

(49)

β_{k + 1}^{i} = \frac{e_{i}}{\sum e_{i}}

(50)

Step3: Updating the state.

X_{k + 1 / k + 1} = \sum β_{k + 1}^{i} X_{i, k + 1 / k + 1}

(51)

P_{k + 1 / k + 1} = \sum β_{k + 1}^{i} P_{i, k + 1 / k + 1}

(52)

In addition, we construct the $(N + 1)$ the filter, as the output filter. The state of this filter is $X_{k + 1 / k + 1}$ , and it runs as an independent filter. Its output value is the output of multi-model EKF navigation algorithm. And its output value is also used to estimate the measurement covariance matrix and eliminate outliers.

Program flow chart is shown in the Fig.4.

Figure 4.

Program flow chart of Multi-model EKF integrated navigation algorithm

5. Lake/Sea trial and the results analysing

In order to verify the validity of our new navigation algorithm, in this section, we use two examples to illustrate the new algorithm in detail, with the lake trial and sea trial data. The navigation platform is composed of the “Qianlong 1” AUV and some beacons on the lakebed/seabed. Data related to acceleration, angular rate and attitude of the AUV are produced by the sensors that are embedded in the AUV. The velocity of AUV can also be obtained by DVL, and the attitude can be obtained by the magnetic compass, too. The acoustic positioning system is working when AUV performs task in the lake/sea, and GPS is working when AUV runs on the surface. Before AUV dives into the sea or after AUV floats out the sea surface, AUV will get the rough position by GPS devices. It should be noted that the position by GPS is very noisy and has several position spurious peaks, not representing correctly the true position for navigation. But the data by GPS can be set as the initial position for the integrated navigation system.

In addition, the velocity measurement sensors provide the velocity in the body system, while the position we need to estimate is related to the fixed coordinate system. So as shown the formula (4), the velocity of AUV should be transformed in the same coordinate system. Sometimes, another important factor that affects the velocity measurements is water current in the actual sea. So we need to process the velocity measurements for filters in advance, and details about this process are presented by [17].

For the first example, “Qianlong 1” AUV tested in the lake from April 20 to May 2, 2014, as shown the Fig.5. Tests are carried out at a speed which belongs to the interval of usual values during missions of the “Qianlong 1” AUV: between 0.0 and 2.5 m/s. Therefore, considering the energy consumption, the value of 1.5 m/s is adopted. The total duration of this test is approximately 72 minutes. The depth of lake is 108m, and the depth of AUV trial area is 30–40m. The four acoustic beacon coordinates are (−105, 357, 30.5), (−397, 206, 32.4), (428, −407, 41.3), (198, −545, 38.5), where the data format is (longitude/x, latitude/y, depth/z) (meters) in the fixed coordinate system.

Figure 5.

The photo of “Qianlong 1” in the lake trial

In the lake trial, AUV gets a lot of raw data, and achieves satisfactory results. After obtaining the raw lake trial data, with multi-model EKF integrated navigation algorithm, we subsequently analyze and process the data, compared with the standard EKF integrated navigation algorithm. As shown the Fig. 6, it is the navigation map of standard EKF algorithm and multi-model EKF algorithm. The green line denotes the AUV's trajectories by multi-model EKF integrated navigation algorithm, the red dash-dot line denotes the trajectories by standard EKF navigation algorithm, the blue points denote the time delay solver points by geometric algorithm, and the asterisk denotes the positions of four beacons. For the first scene, at time ‘T=0’, AUV is on the lake surface and GPS provides a rough position for the AUV. From time ‘T=0’ to ‘T=1’, AUV dives into the trial area along an oblique line, and reaches 40m depth. In the process of the diving, the acoustic navigation system and GPS do not work, and only the INS works for the AUV positioning. So as shown in the figure, there are no measurements from the integrated navigation system, until AUV reaches the trial area at time ‘T=1’, AUV should be initialized again for more accurate initial position. For the second scene, there, AUV hovers or runs with a small speed for 10 minutes, and the integrated navigation system starts at time ‘T=1’, so there are a lot of measurements (in the ellipse at time ‘T=1’), where their density is larger than other place.

Figure 6.

The comparative results of multi-model EKF navigation algorithm and standard EKF navigation algorithm (the lake trial). The green line denotes the AUV's trajectories by multi-model EKF integrated navigation algorithm, the red dash-dot line denotes the trajectories by standard EKF navigation algorithm, the blue points denote the time delay solver points by geometric algorithm, and the asterisk denotes the positions of four beacons. ‘T=0/1/2/3/4’ denotes the different navigation stage.

For the third scene, from time ‘T=1’ to time ‘T=4’, it is the stage of the integrated navigation. We estimate AUV's position simultaneously by standard EKF integrated navigation algorithm and multi-model EKF integrated navigation algorithm, and compare their performances. Here, we don't know the true AUV trajectories, but we can view most of the time delay solver points as the approximate true positions of AUV. Because these time delay solver points (blue points in the Fig.6) are formed by three or four relative correct measurements. Only very less blue points are outliers, which may seriously deteriorate the filter. If a blue point deviates from the expected position, then there must be more than one incorrect measurements for many reasons (a time delay solver point needs at least three measurements), such as the multiple paths, accidental fault of sensors, or the large calibration error. So for the fourth scene, at time ‘T=3’, there is one incorrect measurement from beacon 3. The standard EKF algorithm doesn't eliminate this incorrect measurement but the multi-model EKF algorithm does. So at time ‘T=3’, the red dash-dot line by the standard EKF has a peak, but the green line has not. For the fifth scene, at time ‘T=2’, most of blue points overlap with the green line, and have an apparent distance from the red dash-dot line, which means that the navigation accuracy of multi-model EKF algorithm is better than that of the standard EKF algorithm.

In order to further compare the performance of the two algorithms, the results of error analyzing are also shown in the Fig.7. The error means the spatial distance between the true AUV's position (the time delay solver points) and estimating positions by different algorithms. From the Fig.7, the error of multi-model EKF integrated navigation algorithm is less than the error of standard EKF algorithm, where the former error is less than 6m on average, and the latter error is larger than 12m. So the navigation accuracy has greatly been improved by our new algorithm. What’ more, the peak of the red dash-dot line corresponds to the peak at time ‘T=3’ in the Fig.6, because of the negative effects of outliers. But there is no peak for the green line, so the ability of overcoming or eliminating the outliers of the multi-model EKF integrated navigation algorithm is better than that of the standard EKF algorithm.

Figure 7.

The error analysing of the two algorithms (the lake trial). The green line denotes the estimating error by multi-model EKF integrated navigation algorithm, and the red dash-dot line denotes the estimating error by standard EKF navigation algorithm. The place where there is a peak of the red dash-dot line corresponds to the place at time ‘T=3’ in the Fig.6.

In addition, eliminating the outliers is also the key process of integrated navigation system, because outliers may seriously deteriorate the performance of filters. Here we will give a negative example for the significance of eliminating the outliers, along with the navigation results with a poor algorithm of eliminating outliers. As shown the Fig.8, if we set the value of γ with $3 σ$ probability, it is the navigation result by multi-model EKF integrated navigation. In the Fig.6, the value of threshold $γ$ is set with the $σ$ probability, so the threshold in the Fig.8 is larger than the threshold in the Fig.6.

Figure 8.

The navigation results with the threshold of $γ$ with $3 σ$ probability. Compared with the navigation results in the Fig.6, the trajectory here has more peaks, because of the negative effects of outliers.

From the Fig.8, there are many peaks of the green line, even though it is our new multi-model EKF integrated navigation algorithm estimating the position of AUV. More outliers that should be eliminated are used to update filters, and seriously deteriorate the performance of filters. So eliminating the outliers is an important issue for the multi-model EKF integrated system in the underwater navigation applications. As shown the Fig.9, the error analyzing corresponding to the results of Fig.8, it also indicates that eliminating the outliers is a significant process in order to obtain better performance, compared with the results of Fig.7.

Figure 9.

The error analysing (the threshold of $γ$ with $3 σ$ probability). Compared with the error analysing (the green line) in the Fig.7, the error line here has more sharp peaks, because of the negative effects of outliers. And apparently, the average error value is much larger than the estimating error in the Fig.7 (the green line).

The reason for generating so many outliers is due to the inaccurate calibration for the sound speed, beacons positions, or the magnetic compass. For example, from the analyzing result off-line, we find that if there is a relatively large calibration error for the beacons positions, there will be a lot of outliers generated, which can explain the phenomenon in the Fig.8. Even though the multi-model EKF integrated navigation can eliminate the most of the outliers to some extent, some useful measurements are underutilized because of the interference of outliers. So if we can estimate the trajectories of AUV and the more accurate positions of beacons simultaneously, the navigation performance will be improved further.

For the second example, “Qianlong 1” AUV also conducted a pilot application in October 2014, we also got many valuable raw dada. The trial sea max depth is approximate 5200m, the AUV trial depth is about 4000m. The four beacons positions are (−2967.5, −1047.8, 4065.6), (2056.4, −977.3, 4050.9), (−3006.8, −5047.3, 4056.2), (2948.6, −4984.9, 4053.5). The AUV speed is about 1.5m/s, the trial time up to 26 hours, and because the data are so large, we only analyze the data of the beginning four hours trial. As for the sea trial, it is more difficult to calibrate some key parameters than the lake trial, such as the calibration of sound speed, beacon positions on the seabed, AUV heading and speed coefficient. Similarly to the lake trial, we compare the navigation performance for the sea trial data, with the multi-model EKF algorithm and standard EKF algorithm, as shown the Fig. 10 and Fig. 11.

Figure 10.

The comparative results of multi-model EKF integrated navigation algorithm and standard EKF integrated navigation algorithm (the sea trial). From the figure, the green line and the blue points almost overlap. However, there is an apparent distance between the red dash-dot line and the blue points, which means the performance of multi-model EKF is better than that of standard EKF algorithm. What's more, compared with the Fig.6, there are less outliers seemingly in the Fig.10, in fact that the field scope of sea trial is much larger than the field scope of lake trial.

Figure 11.

The error analysing of the two algorithms (the sea trial). Apparently, the error of standard EKF algorithm is larger than 100m, and the error of our new multi-model EKF integrated navigation algorithm is less than 20m.

From the Fig.10 and Fig.11, the accuracy of multi-model EKF integrated navigation algorithm has significantly been improved, whose accuracy is less than 20m, and that it can overcome the negative effects of outliers. It is also clear that the error of standard EKF algorithm is more than 100m, which is unacceptable in the high accuracy navigation system. The reason for that has been analyzed above with the lake trial. What's more, compared with the Fig.6 and Fig.10, we find that there is system error for the sea trial, because the standard EKF navigation curve has a relatively steady error from the time delay solver points. After the analyzing off-line, there is a larger error of the calibration for the heading coefficient: the calibration value of heading coefficient is larger than the correct coefficient, 9.1 degree. So, the multi-model EKF algorithm may overcome the negative effect of system error to some extent, which is not our initial anticipated achievement.

What's more, with another 4 groups of Pacific sea trial data, the error statistics are shown in the following table.

From the table 2, it can be also concluded that the accuracy of multi-model EKF algorithm is much better than that of standard EKF algorithm.

Table 2.

The error analysing of four groups of data from sea trials

Data group	Standard EKF error	Multi-model EKF error
1	218.9m	11.7m
2	97.4m	7.5m
3	134.2m	9.4m
4	157.5m	12.6m

6. Conclusion

For the harsh underwater environment, it is hard to establish the accurate kinematic model of AUV. In order to obtain high accuracy navigation trajectory, based on the probabilistic data association theory, combined with the EKF navigation system, we propose the multi-model EKF integrated navigation system. Simultaneously, the PDA also provides a new aspect of choosing the models or fusing the results of different filters corresponding to different models, which is the important issue of our new algorithm. Moreover, we also introduce two effective modeling methods for determining the models set for the multiple models navigation system and state the significance of the eliminating outliers with the illustrating examples. With the new algorithm, we dynamically calculate the better navigation trajectories by probability. With the analyzing results of actual lake/sea trial data, there are obvious improvements of the navigation performances. The error after filtering is less than 20m, which is a very optimistic accuracy in the deep sea navigation and positioning system. Meanwhile, for the complex and uncertain system model, multi-model EKF algorithm is also an effective solution, such as unknown external disturbance factors, and system model changing frequently. At last there are still some important problems or issues that have not been completely solved for the high accuracy navigation. First, as we all know, the performance of UKF and PF is better than EKF filter [28 –30], so if we use UKF or PF technologies for the underwater navigation, the result would be more satisfactory. Second, there are some system errors for the calibration of key parameters, such as the beacon positions, sound speed, the velocity coefficient and the heading coefficient. If we have some algorithms to estimate those system errors online with the measurements, then the navigation performance would be improved further. So our next step is to solve the two problems.

Footnotes

7. Acknowledgements

This work is granted by the National Science Foundation of China (51309215).

References

Stutters

Honghai

Tiltman

Brown

(2008) Navigation technologies for autonomous underwater vehicles. Transactions On Systems, Man, and Cybernetics-Part c: Applications and Reviews. Vol. 38(4), July. 2008. pp. 581–589.

Whitcomb

(2000) Underwater robotics: Out of the research laboratory and into the field. International Conference on Robotics & Automation, San Francisco. CA 2000.

Hagen

Størkersen

Vestgard

(2003) Operational military use of the Hugin AUV in Norway. Presented at the UDT Eur. Malmo, Sweden. Jun. 2003. pp. 1–5.

Romeo

Lester

(2001) Navigation is key to AUV missions. Sea technology. Vol.42(12). Dec. 2001. pp. 24–29.

Kinsey

Eustice

Whitcomb

(2006) A survey of underwater vehicle navigation: recent advances and new challenges. In Proc. Conf. Manoeuvering Control Marine Craft. 2006, pp. 1–12.

Miller

Paul A.

Jay

Farrell Yuanyuan

Vladimir

(2010) Autonomous underwater vehicle navigation. IEEE Journal of Oceanic Engineering. Vol. 35(3), July 2010. pp. 663–678.

Liam

Sajad

Mae

Howard

(2014) AUV navigation and localization: a review. IEEE Journal of Oceanic Engineering. Vol.39(1), Jan. 2014. pp. 131–149.

Leonard

Bennett

Smith

Feder

(1998) Autonomous underwater vehicle navigation. In Proc. IEEE ICRA Workshop Navigat. Outdoor Auton. Vehicles, Leuven, Belgium, May 1998.

Atwood

Leonard

Bellingham

Moran (1995) An acoustic navigation system for multiple vehicles. In Proc. Int. Symp. on Unmanned Untethered Submersible Technology. September 1995 pp. 202–208.

10.

Bingham

Seering

Hypothesis

(2006) Improving long baseline navigation for autonomous underwater vehicles. IEEE Journal of Oceanic Engineering. Vol.31(1). Jan. 2006. pp. 209–218.

11.

Savage

(1998) Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms. Control Dyn. Vol.21(1). Feb. 1998. pp. 19–28.

12.

Lee

Jun

Choi

(2005) An integrated navigation systems for underwater vehicles based on inertial sensors and pseudo LBL acoustic transponders. InProc. MTS/IEEE Oceans Conf. Vol.1. 2005. pp. 555–562.

13.

Lee

Jeon

Kim

Choi

(2004) An integrated navigation system for autonomous underwater vehicles with two range sonars, inertial sensors and Doppler velocity log. Korea Research Institute of Ships and Ocean Engineering. 2004. pp. 1586–1593.

14.

Zhao

Gao

(2004) The experimental study on gps/ins/dvl integration for auv. In Proc. Position Location Navig. Symp. Apr. 2004. pp. 337–340.

15.

Lee

Jun

Kim

Lee

Aoki

(2007) Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle. IEEE Ocean. Eng. Vol.32(2). 2007. pp. 327–345.

16.

Zanoni

Barros

(2015) A real-time navigation system for autonomous underwater vehicle. Journal of the Brazilian Society of Mechanical Sciences and Engineering. Vol.37(4). July 2015. pp 1111–1127.

17.

Daxiong

Xisheng

Jian

(2008) Technique of navigation and position of deep water robot with ranges of acoustic beacons. Ph.D. dissertation, Graduate School of Chinese Academy of Sciences. 2008.

18.

Mazor

Averbuch

Bar-Shalom

Dayan

(1998) Inter-acting multiple model algorithms in target tracking: a survey. IEEE Transactions on Aerospace and Electronic Systems Vol.34(1), 1998. pp. 103–123.

19.

Sworder

Boyd

Eliott

Hutchins

(2000) Data fusion using multiple models. Signals, Systems and Computers, Conference Record of the Thirty-Fourth Asilomar Conference. Vol.2(2). 2000. pp: 1749–1753.

20.

Seham

Raafat

Baraka

(2009) Extended kalman filtering and interacting multiple model for tracking maneuvering targets in sensor netwotrks. International Conference on Aerospace Sciences & Aviation Technology. May 2009.

21.

Grishin Yuri

Janczak

(2000) An adaptive estimation algorithm for maneuvering target tracking. Electrotechnical Conference, 2000. MELECON 10th Mediterranean. Vol.2(2). pp. 534–537.

22.

Hongyan

Xinxi

(2004) An IMM tracking algorithm based on a novel target model. Signal Processing. 2004 7th International Conference. Vol. 3. 2004. pp. 1993–1996.

23.

Magnant

Giremus

Grivel

(2013) Jeffreys divergence between state models: application to target tracking using multiple models. Signal Processing Conference. 2013 Proceedings of the 21st European. Vol.9(13). 2013. pp 1–5.

24.

Song

(2006) Highest probability data association for active sonar tracking, information fusion, 9th International Conference. July 2006. pp. 10–13.

25.

Bar-Shalom

Tse

(1975) Tracking in a cluttered environment with probabilistic data association. Automatica, Vol.11. 1975. pp. 451–460.

26.

Daniel

Stilwell

(2007) Observability analysis in navigation systems with an underwater vehicle application. Ph.D. dissertation, Virginia Polytechnic Institute and State University. 2007.

27.

Stokey

Austin

(2002) SAMS: Transponder deployment and calibration procedure. Woods Hole, Ocean Sciences Lab (2002).

28.

Pollard

Read

(1989) An algorithm for calibrating shipmounted acoustic doppler prolers and the limations of gyro compasses. Journal of Atmospheric and Oceanic Technology, Vol.6(6). Dec. 1989. pp. 859–865.

29.

Wan

Beaverton

(200) The unscented kalman filter for nonlinear estimation. Adaptive Systems for Signal Processing, Communications, and Control Symposium. AS-SPCC. IEEE 2000. pp. 153–158

30.

Julier

Uhlmann

(1996) A general method for approximating nonlinear transformations of probability distributions. Robotics Research Group Department of Engineering Science University of Oxford. 1996.