Variational Bayesian adaptive Kalman filter for asynchronous multirate multi-sensor integrated navigation system

Ocean Engineering 174 (2019) 108–116

Contents lists available at ScienceDirect

Ocean Engineering journal homepage: www.elsevier.com/locate/oceaneng

Variational Bayesian adaptive Kalman filter for asynchronous multirate multi-sensor integrated navigation system

T

Narjes Davari∗, Asghar Gholami Department of Electrical and Computer Engineering, Isfahan University of Technology, Isfahan, 84156-83111, Iran

ARTICLE INFO

ABSTRACT

Keywords: Error state Kalman filter Asynchronous multirate adaptive Kalman filter Variational Bayesian approximations Underwater integrated navigation system Strapdown inertial navigation system

This study considers an asynchronous multirate data integration problem in the linear state space model with unknown and time-varying statistical parameters of the measurement noises. To improve performance of the multirate adaptive Kalman filter algorithm, a multi-sensor adaptive Kalman filtering algorithm based on variational Bayesian approximations has been developed in an asynchronous multirate multi-sensor integrated navigation system. The proposed filtering algorithm estimates measurement noise variances of the sensors adaptively and also it is robust to anomalous measurements of sensors and however, multirate adaptive Kalman filter is required to use an appropriate algorithm for outlier rejection to achieve a reliable and optimal estimation of position, velocity, and orientation. A navigation system composed of a strapdown inertial navigation system along with Doppler velocity log, inclinometer and depth meter with different sampling rates is designed to evaluate performance of multirate error state Kalman filter (MESKF) and multirate adaptive error state Kalman filter (MAESKF) algorithms and the proposed algorithm. Results of two experimental tests show that the average relative root mean square error (RMSE) of the position estimated by the proposed filtering algorithm can be decreased approximately 57% and 36% when compared to that of MESKF and MAESKF algorithms, respectively.

1. Introduction The conventional Kalman filter algorithm has been widely implemented for integrating the strapdown inertial navigation system (SDINS) and auxiliary sensors (Davari and Gholami, 2017; Davari et al., 2017; Miller et al., 2010; Shabani et al., 2015; Sheijani et al., 2013; Titterton and Weston, 2004). In SDINS, the inertial measurement unit (IMU) is directly installed in the vehicle. The IMU provides measurements related to the acceleration and angular rates of a vehicle. The navigation system states such as the position, velocity, and orientation are estimated through the navigation equation of SDINS, measurement equations, state equations, and real measurements. A prior exact knowledge of the navigation system dynamics and the statistical information of the process and measurement noise covariance matrix is required for proper conventional Kalman filter algorithm performance. In most applications, the process and measurement noises are often unknown or time-varying and they are mostly dependent on the application environment and process dynamics which degrade the accuracy of estimation of Kalman filter algorithm. A classical way to solve this problem is to use adaptive filters. The adaptive Kalman filter (AKF) algorithm can estimate the dynamic state and the statistical characteristics of the measurement noise, simultaneously. The adaptive ∗

technique does not need the exact values of the priorstatistical information of the state and therefore Kalman filter algorithm performance is improved through the process of filter learning (Mehra, 1970). Moreover, the outliers within the measured data increase the estimation error and normally adaptive Kalman filters use an appropriate algorithm for outlier rejection is used to increase the system accuracy and reliability. Various adaptive filtering algorithms have been used to estimate the filter's statistical information in dynamic systems and are broadly divided into four categories of Bayesian, maximum likelihood (Hasan et al., 2010), correlation and covariance-matching methods (Mehra, 1970). The adaptive filtering based on maximum likelihood (Hasan et al., 2010) requires a quite long innovation sequence to estimate values of the process and measurements noise covariance. In this method, the process and measurement noise covariance are calculated through maximizing the likelihood function of the innovations (Raghavan et al., 2006). The main problem of the ML method is high sensitivity to outlier auxiliary data. In addition, the estimated noise covariance cannot be adjusted immediately with the recent innovation sequences. Moreover, data fusion used in navigation applications with this method is too time consuming and cannot be used in real time applications (Mohamed and

Corresponding author. E-mail addresses: [email protected] (N. Davari), [email protected] (A. Gholami).

https://doi.org/10.1016/j.oceaneng.2019.01.012 Received 14 December 2017; Received in revised form 23 May 2018; Accepted 4 January 2019 0029-8018/ © 2019 Elsevier Ltd. All rights reserved.

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

Schwarz, 1999). The covariance matching technique is based on the innovation or residual sequence and calculates the covariance of the system states by employing a moving window over the streaming data. In this method, the process and measurement noise covariance values are estimated by matching the calculated covariance value and their theoretical value (Ding et al., 2007; Hide et al., 2003; Maybeck, 1982). The correlation technique estimates the covariance matrices based on the autocorrelation function of the output or the innovation sequence. One of the disadvantages of this method is its high processing load due to the need for a large window of data. Furthermore, it does not guarantee the positive-definiteness of the matrices (Odelson et al., 2006; Oussalah and De Schutter, 2001). The Bayesian method is more comprehensive than the aforementioned methods that often approximate Bayesian method (e.g. Särkkä and Nummenmaa, 2009). Bayesian method excels the ML method in two ways. First, there are no restrictions on choosing the required data size, and second, it acts as a robust method because of using a predictive posterior distribution (Watanabe et al., 2004). The navigation accuracy is very sensitive to anomalous measurements and transient disturbances which lead to very poor estimates (Agamennoni et al., 2011). In underwater navigation, the velocity of the vehicle is measured using a Doppler velocity log (DVL) (Allotta et al., 2016; Kinsey and Whitcomb, 2004). The DVL measurements sometimes have drastic and unreal changes called the outlier (Bamnett and Lewis, 1994). The outliers may originate from instrument errors or circumstance disturbances. The variance of measurement noise is timevarying because of outlier influence. Särkkä and Nummenmaa (2009) developed “variational Bayesian approximation based adaptive Kalman filter” (VB-AKF) algorithm to solve the estimation problem with unknown and time-varying measurement noise in which only one auxiliary sensor has been implemented. The VB-AKF algorithm is a robust Kalman filter in the presence of measurement outliers. Gao et al. (2011) modified VB-AKF algorithm to be used in a single rate multi-sensor system in which the measurement sensors were similar. Performance of their proposed algorithm was evaluated through numerical simulation. Previous researches reported applications of AKF and/or VB-AKF algorithms in the field of underwater navigation related to the application of single auxiliary sensor or synchronous multi-sensors (e.g. Dah-Jing and Chen, 2011; Jwo and Weng, 2008; Yu, 2012). To the best of authors' knowledge, this paper reports the first development of an asynchronous multirate multi-sensor adaptive Kalman filtering algorithm based on variational Bayesian approximations for underwater integrated navigation systems. In this paper, the VB-AKF algorithm is modified to estimate the navigation system state and measurement noise variances of multiple asynchronous auxiliary sensors for the underwater integrated navigation system in order to enhance the performance of the AKF algorithm. Unlike AKF algorithm, the proposed algorithm can tune the covariance matrix of asynchronous auxiliary measurements adaptively. In addition, the proposed algorithm is not sensitive to outlying measurement and does not require an additional algorithm to remove the corrupted auxiliary data. The performance of the proposed algorithm is evaluated through real measurements, and its results are compared with those of AKF algorithm. In both methods, the integration structure is based on error state Kalman filter (ESKF). This paper is organized as followings; section 2 reviews the variational Bayesian approximation based on adaptive error state Kalman filter (VB-AESKF) and presents the proposed asynchronous multirate VB-AESKF algorithm. The structure of the integrated inertial navigation system is reviewed in section 2, and equations related to the SDINS, errors state and measurement models are presented in this section, as well. In section 3, performance of the proposed algorithm is evaluated using the real data sets. Finally, conclusion remarks are presented in section 4.

2. Principles of the proposed algorithm In this section, principles of the proposed VB-MAESKF algorithm are presented after a brief introduction to the integrated navigation system, VB-AESKF algorithm and variational approximation. 2.1. Integrated navigation system In integrated navigation systems, the dynamic equations of SDINS are continuous-time nonlinear error state space model. These systems consist of a set of timedependent equations such as change rates of estimated position, velocity, orientation, and bias instability. In these equations, the angular rate and linear acceleration are given in body frame and the velocity and position of the vehicle should be estimated in the navigation frame. In the dynamic equations of SDINS, it is assumed that the output error of the accelerometers and gyroscopes are the sum of bias instability and Gaussian random noise. The accelerometer and gyroscope bias instability are modeled as the summation of random walks and random constants. The general form of a continuous-time nonlinear state space model, suggested by Simon (2006), have been applied for the SDINS (equation (1)), in which x is time derivative of navigation system state, x is total navigation system state vector of the SDINS, f is the SDINS nonlinear function which describes the dynamic behavior of the navigation system, G (t ) is the noise input matrix, and w (t ) is the process noise due to uncertainty in the control inputs which is assumed to be a zero mean Gaussian white-noise distribution with known covariance matrix, Q (t ) . The control input, u (t ) , is given in equation (2), in which f b , ibb are linear acceleration and angular rate vectors, respectively.

x (t ) = f (x (t ), u (t )) + G (t ) w (t ), w

u = [f b ,

(1)

N (0, Q (t ))

b T ib ]

(2)

In this paper, the reviewed and proposed integrated navigation algorithms are based on error states of system. This paper considers the discrete-time linear error state space model as shown in the following equations. Equations (3) and (4) are error state equation and measurement error equation of ith auxiliary sensor, respectively at the case time instant (tk ) that are expressed in ESKF. The error state and measurement error equations for implementing in the multirate Kalman filter have been comprehensively reviewed in Davari et al. (2017).

x (tk ) = A (tk 1) x (tk 1) + L (tk 1) w (tk 1), w (tk ) yi (tk ) = Hi (tk ) x (tk ) + Mi (tk ) ri (tk ), ri (tk )

N (0, Q (tk ))

N (0, Ri (tk ))

(3) (4)

where, xk is an n-dimensional error state vector (n is the number of states considered for the navigation system), the error state, x = xtrue xˆ , represent the expected error between the true state, xtrue , and its estimation, xˆ . Further, A (tk ) is n × n system state transition matrix, L (tk 1) is n × dynamic noise distribution matrix, w (tk 1) is -dimensional input white noise vector which is assumed to be a Gaussian distribution with zero mean and × covariance matrix, Q (tk 1) , and is the number of modeled errors for IMU sensors. In this paper, two types of errors, bias instability and random walk for three accelerometers and three gyroscopes are considered, thus, is 12. In equation (4), yi (tk ) is a di -dimensional measurement error vector, and is the difference between the navigation solution from the SDINS and auxiliary measurement sensors, Hi (tk ) and Mi (tk ) are di × n measurement output matrix and di × di measurement noise distribution matrix, respectively for ith auxiliary sensor, and ri (tk ) represents di -dimensional measurement noise vector which is assumed to be a zeromean Gaussian white noise distribution with di × di time-varying covariance matrix, Ri (tk ) . In this paper, sensors are assumed to be independent of each other, thus their measurement noises are uncorrelated as shown in equation (5), is Kronecker delta function.

E [ri (tk ), rj (th)] = Ri (tk ) 109

ij

kh

(5)

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

In the VB-AESKF algorithm, the variational Bayesian approximation recursively approximates the joint posterior distribution of the error state and measurement noise variance, p ( x (t1:k ), Ri (tk )| yi (t1: k )) by using a factorized free form distribution (Särkkä and Nummenmaa, 2009). Then, the state and the measurement noise variances are estimated by means of a fixed-point iteration in the recursive algorithm. In this paper, it is assumed that the prior navigation system state and the process noise variance and unknown measurement noise variance are independent of each other. Prior to start the VB-AESKF algorithm, an initialization procedure is used to determine initial value for the state and its noise variance and the prior distribution, p ( x (t 0 ), Ri (t0 )) . The prior error state information, x (t 0 ) has a Gaussian distribution with m (t 0 ) and P (t 0 ) as its mean and covariance, respectively. The VB-AESKF algorithm is divided into prediction and update phases. In the prediction phase, the Chapman-Kolmogorov equation approximates the conditional distribution for the error state and the measurement noise variance given by the measurements error yi (t1:k 1) , as shown in equation (6).

p ( x (tk ), Ri (tk )| yi (t1:k 1)) =

The considered integrated navigation system consist of an IMU and m multirate auxiliary sensors with different sample periods T and T i for t = {t 0, t1, …, tk, …, t } each sensor, respectively. Let and t i = {t0i , t1i, …, t kii , …, t ii } denote the different discrete-time sets for IMU and the ith auxiliary sensor, respectively. Further, Г and Гi are the largest time instants for the IMU and the ith auxiliary sensor, respectively, and tk and t kii as shown in equation (9) are the time instant when kth measurement of IMU and kith measurement of ith auxiliary sensor are available, where T and T i are sampling period for IMU and ith auxiliary sensor, respectively.

tk = t0 + k. T , t kii = t0i + ki. T i

Without loss of generality, it is assumed that t 0 < and > T . In general, t kii may not be equal to t kjj even if time instant of two different sensors are equal, ki = kj i j . For the proposed VB-MAESKF algorithm, modified prediction and update phases have been applied. In the prediction phase, the error state and its covariance are predicted as shown in equations (10) and (11).

P (tk ) =

× p ( x (tk 1), Ri (tk 1)| yi (t1:k 1)) (6)

(7)

2.2. Variational approximation

Gamma (

2 i (tk )

i (tk ),

i (tk ))

i

(tkii ) =

+ i i

i

( )=

+ i

tkii

(11)

i

(tkii 1)

(12)

(

(13)

)

t kii 1

m

min di i

Dk

di

(14)

i=1

If time instant of IMU equals with time instant of m auxiliary sensors, as presented in equation (15), then α (Acharya et al., 2011) and β(Acharya et al., 2011) are the vectors for the parameters of the InvGamma at tk , and are given by equations (16) and (17).

tk = tk11 = tk22 = …=tkmm

(15)

)T ,

2

(tk

2

(tk )T , …,

(tk ) = [

1

(tk

(tk ) = [

1

(tk )T ,

)T , …,

)T ]

(16)

T m (tk ) ]

(17)

m (tk

The update phase of the proposed algorithm is executed when at least one of the auxiliary sensors data is received at tk . If time instant of the sensors do not follow equation (15), a vector S (tk ) = [S1 (tk ), S2 (tk ), …, Sm (tk )] is introduced, where Si (tk ) gains a binary value. If Si (tk ) = 1, it means that the ith sensor measurement at tk (tk ) presented in equation (17) is modified is received. Therefore, using equations (18) and (19) so that can be used in update phase.

p ( x (tk ), Ri (tk )| yi (t1:k )) = p ( x (tk )| yi (t1: k )) p (Ri (tk )| yi (t1: k )) p ( x (tk )| yi (t1: k )) = N ( x (tk )| xˆ (tk ), P (tk )) di Inv i=1

+

L (tk 1) Q (tk 1) LT (tk 1)

At tk , dimensions of matrices, H (tk ) , M (tk ) and R (tk ) and dimensions of vectors y (tk ) and r (tk ) are Dk × n , Dk × Dk , Dk × Dk , Dk × 1, and Dk × 1, respectively, in which Dk is expressed as equation (14).

The variational Bayesian method is known as an ensemble learning method. It uses a variational approximation for approximating the intractable integral of the Bayesian method where the true posterior distribution is approximated with a simpler distribution. This is a robust estimation method that reduces the computational complexity (Beal, 2003; Ghahramani and Beal, 1999; Ueda and Ghahramani, 2002). According to Särkkä and Nummenmaa (2009), it can be assumed that the error states are independent of the measurement noise variances. Therefore, the variational Bayesian (VB) approximation method can be used to approximate the conditional distribution for the error state and the measurement noise variances given measurements error yi (t1:k ) , as presented in equation (8), where N and “Inv-Gamma” represent the Gaussian and Inverse-Gamma distributions, respectively. i (tk ) and i (tk ) are parameters of Inv-Gamma distribution for measurement noise variance at tk for ith sensor. The implementation details of the VB approximation method for AKF algorithm can be found in Särkkä and Nummenmaa (2009).

p (Ri (tk )| yi (t1: k )) =

A (tk 1) P + (tk 1) AT (tk 1)

In this paper, estimated quantities are denoted by a hat, and ‘−’ and ‘+’ denote values calculated in predict and update phases, respectively. The parameters of the Inv-Gamma distribution for the ith auxiliary sensor are given in equations (12) and (13). A scale factor 0 < ρi ≤ 1 is employed to control the fluctuation of the estimated measurement noise variance. If the value of ρi is close to one, fluctuations of the estimated measurement noise variance becomes smooth; and the lower values for ρi, the larger the fluctuations.

In the update phase, if the measurement data becomes available at tk , the prior distribution is updated via Bayes rule given in equation (7), c is normalization constant. In this paper, for prediction and update phases, error state and measurement error have been modified in order to solve the problem based on ESKF.

p ( x (tk ), Ri (tk )| yi (t1:k )) 1 = p ( yi (t1:k )| x (tk ), Ri (tk )) p ( x (tk ), Ri (tk )| yi (t1: k 1)) c

Ti

(10)

xˆ (tk ) = A (tk ) xˆ +(tk 1)

p ( x (tk )| x (tk 1)) p (Ri (tk )|Ri (tk 1))

d x (tk 1) dRi (tk 1)

(9)

t0i

(8)

C (tk ) = diag (S (tk )) 2.3. The proposed VB-MAESKF algorithm

(tk ) = C (tk )

(tk )

(18) (19)

If at tk , the ith sensor data becomes available, the values of the ith sensor measurement output matrix, Hi (tk ) , and the measurement error vector, yi (tk ) , are placed on the total measurement output matrix, H (tk ) , and the total measurement error vector, y (tk ) , respectively. In the case of receiving all of the auxiliary measurements at time step tk

The algorithm proposed in this paper can be applied in integrated navigation system with dissimilar sensors whose sampling rates are different, i.e., asynchronous multi-rate sensors. Using multiple sensors could potentially provide better performance and higher navigation accuracy due to better visibility and complementary information. 110

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

Fig. 1. The proposed VB-MAESKF algorithm.

Fig. 2. Integrated navigation system based on VB-MAESKF for underwater vehicles. Table 1 Calculation burden of algorithms in per each iteration. Filter

Computational Burden

MESKF

CBMESKF = 4n3 + 3.5n2

VB-MAESKF MAESKF

CBVB

MAESKF

= CBMESKF +

CBMAESKF = CBMESKF + 2n3

(equation (15)), the measurement output matrix and the measurement error vector are expressed as equations (20) and (21).

H (tk ) = [H1 (t k11 )T , H2 (tk22 )T , …, Hm (t kmm )T ]T

y (tk ) =

[

1.5n + 4n2m + nm + 3nm2 +

y1 (t k11 )T , y2 (tk22 )T , …, ym (tkmm )T ]T

6m2

(16m3

3m2

m)

6

m

6n2 + 6mn2 + m2n

0.5m2 + n

0.5m

Receiving a new auxiliary measurement, the navigation error state and its associated covariance matrix P + (tk ) at tk are estimated through equations (23)–(29) for a predetermined number of iterations. The matrices related to received measurements (C (tk ) and C (tk ) ) follow the constraint shown in equation (30), in which I is an identity matrix. The proposed VB-MAESKF algorithm is summarized in Fig. 1.

(20) (21)

If the measurement noises for different sensors are uncorrelated, the measurement noise covariance matrix, R (tk ) may be expressed as equation (22), where Ri (tkii ) is variance of the measurement errors for ith sensor at tk .

+ (t ) k

=

1 + 2

(tk )

Rˆ (tk ) = blockdiag

R (tk ) = E {r (tk ) r (tk )T } = blockdiag (R1 (t k11 ) , R2 (t k22 ) , …, Rm (t kmm ) ) (22)

(23) 1 (tk ) , + 1 (tk )

2 (tk ) , + 2 (tk )

…,

m (tk ) + m (tk )

K (tk ) = P (tk ) H (tk )(H (tk )T P (tk ) H (tk ) + M (tk )T Rˆ (tk ) M (tk )) 111

(24) 1

(25)

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

xˆ +(tk ) = xˆ (tk ) + K (tk ) ( y (tk )

P + (tk ) = P (tk ) +

(tk ) +

=

received, the relationship between the measurement error and the error states of the velocity are expressed; at t k44 , the relationship between the measurement error and error states of the attitude are applied; and at t k55 , if only the fifth sensor (gyrocompass) data is available, the relationship between the measurement error and error states of the heading can be expressed. More details for the measurement equation of auxiliary sensors are presented in Davari et al. (2017). In order to evaluate the required computational time of the proposed algorithm and two others algorithms MAESKF and MESKF, equation (31) is used (Assimakis et al., 2013).

(tk ) + C (tk )

C (tk ) + C (tk ) = I

(tk )

(27)

(tk )T

1 H (tk ) ( y (tk ) 2

+ H (tk )T P + (tk ) H (tk ) + (t ) k

(26)

K (tk )(H (tk )T P (tk ) H (tk )

M (tk )T Rˆ (tk ) M (tk )) K

(tk ) = C (tk )

H (tk ) xˆ (tk ))

H (tk ) xˆ (tk )) 2

(28)

talg = CBalg Nalg top

(29)

(31)

where top , CBalg and Nalg are the time required to perform a scalar operation, calculation burden per iteration and number of iterations in an algorithm, respectively. The computational burden for three algorithms are summarized in Table 1, and n and m are dimensions of state vector and measurement vector, respectively. In algorithms with the same number of iterations, the calculation burdens per iteration can be compared. If the integrated navigation system uses full state (n = 15) and all auxiliary sensors (m = 9), the computational burden of MAESKF is larger than VB-MAESKF and MESKF.

(30)

In Fig. 2, a flowchart containing three phases of the integrated navigation system based on VB-MAESKF for underwater vehicles is shown. In stationary condition and before of navigation, the initialization phase in two steps, i.e., coarse alignment and fine alignment is done; and initial values for system states, their variances, and parameters of variational Bayesian are calculated. After the initialization phase, the navigation will be started which is included prediction and update phases. In the prediction phase, the error states, their variances, and α, β are predicted by using the updated values in previous time step. In the update phase, in the best conditions, at a time step, for example, tk , if all possible measurements, including Data of GPS, depth meter, DVL, inclinometer, and gyrocompass are available, the measurement error vector can be expressed as equation (21), in which m is five. At t k11 , that only the first sensor (GPS) data is available, the relationship between measurement error and error states of the horizontal position are used; at t k22 , if only the second sensor (depth meter) data is received, the relationship between the measurement error and the error state of the depth are used; at t k33 , if only the third sensor (DVL) data is

3. Experimental results To evaluate performance of the proposed algorithm in a dynamic system with unknown and time-varying measurement noise statistics, two sea tests were performed. In the first test, trajectory No.1, the vessel travelled approximately 23 km in a round trip for about 337 min; and in the second test, trajectory No. 2, the vessel travelled approximately 9.63 km for about 83 min. Fig. 3 shows these two trajectories. In order to determine initial values of the navigation system and deterministic inertial sensors errors, an alignment procedure is applied

Fig. 3. Travelled trajectory around Qeshm island in the Persian Gulf, a) trajectory No. 1, b) trajectory No. 2.

Fig. 4. The relative RMSEs of the position estimated by MESKF, MAESKF and VB-MAESKF algorithms for different values of q, (a) in the test on trajectory No.1 and (b) in the test on trajectory No.2. 112

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

in the same test cases. Results of the proposed algorithm are compared with the results of MESKF and MAESKF algorithms. The Kalman filter is often regarded as an appropriate estimator for linear Gaussian dynamic systems.The outlying measurement may result in a reduction of navigation system accuracy and performance breakdowns. Therefore, the Kalman filter requires an appropriate algorithm in order to reject the outlier data. One of the basic methods to reject outliers is Mahalanobis distance which is applied to DVL data in MESKF and MAESKF algorithms, called as rejection criterion. In this method, if 2 the squared innovation, ( y3 (t k33 ) H3 (tk33 ) xˆ (tk )) , is q times larger 3 T than the measurement covariance, (H3 (tk3 ) P (tk ) H3 (tk33 ) + R3 (tk33 )) , then the measurement is rejected. The parameter q is called as rejection coefficient and could be tuned to eliminate outliers. In order to find the best rejection coefficient for MESKF and MAESKF algorithms, several coefficient values have been applied manually to the algorithms, and finally, the coefficient with the lowest relative RMSE have been selected. The RMSE of the estimated position (in meters) and relative RMSE (in %) are calculated as shown in equations (32) and (33), where (xref , yref , z ref ) and (xest , yest , z est ) are the reference and the estimated positions in x, y and z directions, respectively.

Table 2 The technical specification of the instruments. Sensors IMU

Gyroscope

Accelerometer

DVL Depth meter GPS

Specification Bias Stability: 1 deg/hr

Angular Random Walk: 0.0667 deg/ hr Bias offset: 20 deg/hr Data Rate: 100 Hz In run bias variation: 0.25 mg @1σ Output Noise: 55 µ g/ Hz Bias offset: 50 mg Data Rate: 100 Hz Frequency: 300 kHz Accuracy: 2% ± 2mm /s@1 Data rate: 3 Hz Accuracy: < 2 m Data rate: 1 Hz Accuracy: < 10 m Data rate: 1 Hz

in stationary condition according to the procedure proposed by Chen (2008). In the two trajectories, the vessel was in stationary condition for 10 min at beginning of the travel. Subsequent to the alignment, the navigation phase is started in which the proposed algorithm estimates navigation system state by using auxiliary measurements provided by the DVL, depth meter, and inclinometer. The GPS data is used only as the reference and for initialization. More detailed descriptions from the technical specifications of the sensors could be found in Table 2. To evaluate the performance of the proposed algorithm, MESKF algorithm developed by Titterton and Weston (2004) and Farrell (2008) and MAESKF algorithm proposed by Davari et al. (2017) have been applied

RMSE =

1 n

n

i (x ref

2

i i xest ) + yref

2 i yest

i + (z ref

i=1

Relative RMSE =

RMSE × 100 Travelled Distance

i z est )

2

(32) (33)

Fig. 4 (a) and (b) show the relative RMSEs of the position estimated by using MESKF, MAESKF and VB-MAESKF algorithms for 25 different values of reject coefficient, q in the tests performed on trajectories No.1 and No.2, respectively. In trajectory No.1, minimum relative RMSEs for MESKF, MAESKF, and VB-MAESKF are obtained as 0.8%, 0.3% and 0.2% of the travelled distance for values of q, 600, 1800, and 2600, respectively. In trajectory No.2, minimum relative RMSEs for MESKF, MAESKF and VB-MAESKF are obtained as 1%, 0.9% and 0.65% for values of q, 200, 1400 and 2600, respectively. The results show that on the one hand, the accuracy of MESKF and MAESKF algorithms is highly dependent on the value of q, and its optimum value depends on the trajectory, i.e., it needs to be tuned; and on the other hand, VB-MAESK is not sensitive to the value of q. The proposed algorithm reduces the value of the squared innovation, by accurate estimation of measurement noise covariance of DVL and system states; further, it is robust to the outliers and anomalous measurements. It is clear that increasing the value of q results in a lower probability of rejection the abnormal DVL data, leading to an increase in the RMSE. Further, decreasing the value of q results in rejecting more number of DVL data. However, this procedure is not efficient enough, since it might reject the original data. Fig. 5 shows an example of the setting value of q . The top part of the figure illustrates the DVL measurements

Fig. 5. (a) The measurements of the DVL in the body frame, (b) Rejection criterion for outliers of DVL data.

Fig. 6. Relative RMSE of position estimated using different process noise variance, a) trajectory No.1, b) trajectory No.2. 113

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

sensitive to process noise covariance compared to VB- MAESKF algorithm. According to Fig. 6 (a), for trajectory No.1, the smallest value for the RMSEs of the position estimated by using MESKF, MAESKF and VBMAESKF algorithms are 184, 80 and 46 m and relative RMSE are 0.8%, 0.35% and 0.2% of the travelled distance, respectively. According to these results, the proposed algorithm improves the accuracy of the estimated position approximately 75% and 42% compared to MESKF and MAESKF algorithms, correspondingly. According to Fig. 6 (b), for trajectory No.2, the RMSEs of the position estimated by the MESKF, MAESKF and VB-MAESKF algorithms are 102, 89 and 62 meters and relative RMSE are 1.06%, 0.91% and 0.65% of the travelled distance, correspondingly. According to these results, the proposed algorithm exhibits an improvement of almost 39% and 30% in position estimation compared to MESKF and MAESKF algorithms, respectively. Fig. 7 and Fig. 8 show the trajectory estimated by MESKF, MAESKF, and VB-MAESKF algorithms compared to the reference trajectory in trajectory No.1 and No.2, respectively. Note that the reference trajectory is compiled from smoothed GPS data in the east and north directions. It should be noted that the estimated trajectories by MESKF and MAESKF algorithms are calculated using an optimal value of the rejection coefficient, q , and the process noise covariance, which leads to the minimum relative RMSE for these two algorithms. Also, the measurement noise covariance matrix has been manually tuned for MESKF algorithm. However, as an advantage for VB-MAESKF algorithm, it does not need to tune optimal value for q to reject abnormal data and the process and measurement noise variances; the proposed method is robust to outliers. Results presented in Fig. 4 show that on the one hand in underwater navigation, the outlier in DVL data decrease the accuracy of estimated position; and on the other hand, tuning rejection coefficient is very time-consuming and should be repeated for each trajectory. Hence, the use of a robust algorithm is highly efficient. In Fig. 9 (a) and (b), the absolute error of the estimated position over time using MESKF, MAESKF (using the optimum value of q and process and measurement noise variances) and VB-MAESKF algorithms are presented for trajectories No.1 and 2, respectively. The absolute error is difference between the estimated and the reference positions. In the integrated navigation for the -underwater vehicle using VB-MAESKF (Figs. 7–10) procedure of outlier rejection is not performed. It is obvious that although the key parameters of MESKF and MAESKF algorithms have been tuned for the best performance, the absolute error for VB-MAESKF algorithm over the time is much lower than the two other algorithms, in both trajectories No.1 and No.2. With reference to the two graphs in Fig. 9 (a) and (b), it can be concluded that since the test trajectory No.1 is a round trip, the absolute error is mostly bounded over the time; however, the test trajectory No.2 follows a straightahead trajectory, and thus the absolute error is growing with time. Fig. 10(a)–(d) illustrate the estimated measurements noise variance of DVL, inclinometer and depth meter sensors by MAESKF and VB-

Fig. 7. Estimated trajectory by MESKF, MAESKF and VB-MAESKF in trajectory No.1.

Fig. 8. Estimated trajectory by MESKF, MAESKF and VB-MAESKF in trajectory No.2.

in the body frame and its down part shows the rejection criterion over the time of DVL for q = 500 . Furthermore, the process noise covariance matrix needs to be manually tuned for every trajectory in order to obtain the best performance of MESKF and MAESKF algorithms. The tuning process has been performed on 12 elements of the process noise covariance matrix. Fig. 6 (a) and (b) show the relative RMSE of position estimated for different values of the process noise covariance of gyroscope bias, as an example, in the test performed on trajectory No.1 and No.2, respectively. Results of this analysis confirm that MESKF and MAESKF algorithms are more

Fig. 9. Absolute error of the estimated position in (a) trajectory No.1 and (b) trajectory No.2. 114

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

Fig. 10. Real measurements and estimated measurements noise variance for various auxiliary sensors.

115

Ocean Engineering 174 (2019) 108–116

N. Davari, A. Gholami

Table 3 Performance comparison of the MESKF, MAESKF and VB-MAESKF algorithms. Trajectory

Distance time

Approach

RMSE (m)

Relative RMSE (%)

No.1

23 km 183min

No.2

9.63 km 83min

VB-MAESKF MAESKF MESKF VB-MAESKF MAESKF MESKF

46 80 184 62 89 102

0.2 0.35 0.8 0.65 0.91 1.06

1551–1558. Allotta, B., Caiti, A., Costanzi, R., Fanelli, F., Fenucci, D., Meli, E., Ridolfi, A., 2016. A new AUV navigation system exploiting unscented Kalman filter. Ocean Eng. 113, 121–132. Assimakis, N., Adam, M., Koziri, M., Voliotis, S., Asimakis, K., 2013. Optimal decentralized Kalman filter and Lainiotis filter. Digit. Signal Process. 23 (1), 442–452. Bamnett, V., Lewis, T., 1994. Outliers in Statistical Data. Beal, M.J., 2003. Variational Algorithms for Approximate Bayesian Inference. University of London United Kingdom. Chen, H.-H., 2008. In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning. Ocean Eng. 35 (14), 1448–1462. Jwo, D.-J., Chen, J.-J., 2011. Neural Network Aided Adaptive Kalman Filter for GPS/INS Navigation System Design. Davari, N., Gholami, A., 2017. An asynchronous adaptive direct Kalman filter algorithm to improve underwater navigation system performance. IEEE Sensor. J. 17 (4), 1061–1068. Davari, N., Gholami, A., Shabani, M., 2017. Multirate adaptive Kalman filter for marine integrated navigation system. J. Navig. 70 (3), 628–647. Ding, W., Wang, J., Rizos, C., Kinlyside, D., 2007. Improving adaptive Kalman estimation in GPS/INS integration. J. Navig. 60 (03), 517–529. Farrell, J., 2008. Aided Navigation: GPS with High Rate Sensors. McGraw-Hill, Inc., New York. Gao, X., Chen, J., Tao, D., Li, X., 2011. Multi-sensor centralized fusion without measurement noise covariance by variational Bayesian approximation. IEEE Trans. Aerospace Electron. Syst. 47 (1) 718-272. Ghahramani, Z., Beal, M.J., 1999. Variational Inference for Bayesian Mixtures of Factor Analysers. NIPS, pp. 449–455. Hasan, A.M., Samsudin, K., Ramli, A.R., Abdullah, R., Azmir, R.S., 2010. Wavelet-based pre-filtering for low cost inertial sensors. J. Appl. Sci. 10 (19), 2217–2230. Hide, C., Moore, T., Smith, M., 2003. Adaptive Kalman filtering for low-cost INS/GPS. J. Navig. 56 (01), 143–152. Jwo, D.-J., Weng, T.-P., 2008. An adaptive sensor fusion method with applications in integrated navigation. J. Navig. 61 (04), 705–721. Kinsey, J.C., Whitcomb, L.L., 2004. Preliminary field experience with the DVLNAV integrated navigation system for oceanographic submersibles. Contr. Eng. Pract. 12 (12), 1541–1549. Maybeck, P.S., 1982. Stochastic Models, Estimation, and Control. Academic press, London. Mehra, R.K., 1970. Approaches to adaptive filtering. In: 1970 IEEE Symposium on Adaptive Processes (9th) Decision and Control, pp. 141. Miller, P.A., Farrell, J.A., Zhao, Y., Djapic, V., 2010. Autonomous underwater vehicle navigation. IEEE J. Ocean. Eng. 35 (3), 663–678. Mohamed, A., Schwarz, K., 1999. Adaptive kalman filtering for INS/GPS. J. Geodes. 73 (4), 193–203. Odelson, B.J., Rajamani, M.R., Rawlings, J.B., 2006. A new autocovariance least-squares method for estimating noise covariances. Automatica 42 (2), 303–308. Oussalah, M., De Schutter, J., 2001. Adaptive Kalman filter for noise identification. In: Proceedings of the International Seminar on Modal Analysis. KU Leuven, pp. 1225–1232 1998. Raghavan, H., Tangirala, A.K., Gopaluni, R.B., Shah, S.L., 2006. Identification of chemical processes with irregular output sampling. Contr. Eng. Pract. 14 (5), 467–480. Särkkä, S., Nummenmaa, A., 2009. Recursive noise adaptive Kalman filtering by variational Bayesian approximations. IEEE Trans. Automat. Contr. 54 (3), 596–600. Shabani, M., Gholami, A., Davari, N., 2015. Asynchronous direct Kalman filtering approach for underwater integrated navigation system. Nonlinear Dynam. 80 (1–2), 71–85. Sheijani, M.S., Gholami, A., Davari, N., Emami, M., 2013. Implementation and performance comparison of indirect Kalman filtering approaches for AUV integrated navigation system using low cost IMU. In: Electrical Engineering (ICEE), 2013 21st Iranian Conference on. IEEE, pp. 1–6. Simon, D., 2006. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. John Wiley & Sons. Titterton, D., Weston, J.L., 2004. Strapdown Inertial Navigation Technology. IET, London. Ueda, N., Ghahramani, Z., 2002. Bayesian model search for mixture models based on optimizing variational bounds. Neural Network. 15 (10), 1223–1241. Watanabe, S., Minami, Y., Nakamura, A., Ueda, N., 2004. Variational Bayesian estimation and clustering for speech recognition. IEEE Trans. Speech Audio Process. 12 (4), 365–381. Yu, M.-J., 2012. INS/GPS integration system using adaptive filter for estimating measurement noise variance. IEEE Trans. Aerospace Electron. Syst. 48 (2), 1786–1792.

MAESKF algorithms, and the real measurement of the sensors for the test performed on trajectory No. 1. Similar results for the test performed on trajectory No. 2 are shown in Fig. 10(e)–(h). Upper graphs in Fig. 10(a)–(h) illustrate the real measurements of the auxiliary sensors and lower graphs shows their estimated noise variance. As it is shown, at time instants that a rapid change in the auxiliary measurements happens (which seems to be the noise effect) the estimated noise variance is increased especially for VB-MAESKF algorithm to optimize the Kalman filter performance. The navigation results for two trajectories are summarized in Table 3. According to these results, the VB-MAESKF algorithm improves the accuracy of the estimated position of both trajectories on average about 57% and 36% compared to MESKF and MAESKF algorithms, respectively. 4. Conclusion This paper presented an asynchronous multirate multi-sensors adaptive Kalman filter algorithm to enhance the accuracy of the underwater integrated navigation system. In the proposed algorithm, MAESKF algorithm was modified based on variational Bayesian approximations to adaptively tuning the measurement covariance matrix of the asynchronous auxiliary sensors. Performance of the proposed algorithm has been evaluated through experimental tests performed on two trajectories and comparing to the related results obtained by using MESKF and MAESKF algorithms. Results show that the proposed algorithm is robust to outliers and anomalous measurements of the auxiliary sensors compared to MESKF and MAESKF algorithms. Furthermore, the proposed algorithm is less sensitive to non-optimal values of process noise variance than MESKF and MAESKF algorithms. Results of the tests show that the average relative RMSE of the position estimated by the VB-MAESKF algorithm is reduced by about 57% and 36% compared to MESKF and MAESKF algorithms, respectively. Acknowledgment The authors would like to thank Mr. Mehdi Emami for his help in data acquisition. References Acharya, A., Sadhu, S., Ghoshal, T., 2011. Improved self-alignment scheme for SINS using augmented measurement. Aero. Sci. Technol. 15 (2), 125–128. Agamennoni, G., Nieto, J.I., Nebot, E.M., 2011. An outlier-robust Kalman filter. In: Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, pp.

116