# A Quadratic Interpolation-Based Variational Bayesian Algorithm for Measurement Information Lost in Underwater Navigation

Authors: Yuan Yang; Jiacheng Tang; Haoqian Huang; Xiaoguo Zhang; Tingting Zhang; Yujin Kuang
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

The main challenges of sequential estimations of underwater navigation applications are the internal/external measurement noise and the missing measurement situations. A quadratic interpolation-based variational Bayesian filter (QIVBF) is proposed to solve the underwater navigation problem of measurement information missing or insufficiency. The quadratic interpolation is used to improve the observed vector for the precision and stability of sequential estimations when the environment is changed or the measurement information is lost. The state vector, the predicted error covariance matrix, and the measurement noise matrix are derived based on the variational Bayesian method. Simulation results demonstrate the superiority of the proposed QIVBF compared with the traditional algorithm under the condition of measurement information lost by autonomous underwater vehicles.


---

## Body

## 1. Introduction

Ocean has already become the strategic goal of many countries because of its underdeveloped resources, marine environments, and high-tech fields [1, 2]. The autonomous underwater vehicles (AUVs) have become one of the important tools for underwater detection, environment survey, and underwater reconnaissance in the ocean [3, 4]. The navigation and positioning methods with high accuracy for AUV are the necessary conditions to acquire effective information [5], and also, they are the key technologies to determine whether AUV can work normally and return back safely [6, 7]. The more accurate navigation and positioning methods for AUV are the necessary conditions for the underwater vehicle to ensure the correct travel [8, 9].The development of underwater vehicles faces many problems; for instance, the ocean environment is complex and changeable due to all sorts of noise interferences, and the AUV may fail to represent the true sensor measurements in many actual applications, like the presence of ocean current, salt cliffs, and ships around. The measurements are sometimes unobservable or insufficient so that positions calculated are erroneous [10–12]. On the other hand, a few navigation methods based on electromagnetic transmitting cannot be used underwater since the signal attenuates quickly underwater [13]. What is more, the localization error of the MEMS system accumulates along with time [14–17]. Those above weaknesses will cause the measurement missing, which has an adverse impact on the positioning of AUV [18]. However, the debugging and arranging process would waste time and material resources. Therefore, the performances of estimation algorithms are vital to working normally for AUV [19, 20].The Kalman filter (KF), which is a widely and classical recursive filter, could provide the optimal state estimates in the linear dynamic system in the scenarios of known noise models and system models [21–24]. Additionally, the accurate measurements are difficult for the underwater integrated navigation system to describe because the performance of sensors varies with the change in the environment and measurement information probability lost [25, 26]. The joint probability density function (PDF) is used to model the information of the AUV. Then, the information of the joint PDF is determined by using the variational Bayesian (VB) method [27] in recursive process, which is the optimal. To further improve the capability of the navigation with the missing measurements, some strategies are required.The recently proposed variational Bayesian- (VB-) based adaptive KF is a deterministic approximate Bayesian method that transforms the solution of the posterior probability density function based on Bayes’ theorem into the solution of the functional extreme value [28, 29]. It has more ideal approximate estimation results and computational overhead [30, 31]. It significantly reduces the difficulty of calculation and makes high-precision filtering possible [32–35].Aiming to mitigate the underwater navigation problem of measurement information missing or insufficiency, this paper proposes a quadratic interpolation-based variational Bayesian filter (QIVBF) algorithm. The QIVBF makes better use of the quadratic interpolation (QI) method and the VB method, so that the predicted error covariance matrix and the measurement noise matrix are derived to estimate the state vector more accurately. The quadratic interpolation improves the observed vector for the precision and stability of sequential estimations when the environment is changed or the measurement information is lost.The rest of the paper is presented as follows. In Section2, the situation of the measurement lost in underwater navigation is described in detail. In Section 3, the QI method is used in measurement missing situations. In Section 4, the VB method is used to estimate the accurately predicted error covariance matrix, measurement error matrix, and state vector. Section 5 shows the simulation in underwater navigation. Section 6 gives the main conclusions of the paper.


## 2. Problem Statement

An underwater navigation function with measurement information lost is described as follows [36]:(1)Xk|k−1=fk−1Xk−1|k−1+ωk−1,Zk=ΗkXk|k+υk,where Xk−1|k−1=P^x,k−1V^x,k−1P^y,k−1V^y,k−1T is the state posterior given the observation at time k−1; P^x,k−1 and P^y,k−1 denote the positions of the AUV at the time k−1 in x and y directions, respectively. V^x,k−1 and V^y,k−1 denote the velocities of the AUV at the time k−1 in x and y directions, respectively. The Zk=Px,k−1Vx,k−1Py,k−1Vy,k−1T is the measurement vector at the time k, with Px,k−1 and Py,k−1 representing the position measurements of the AUV at the time k−1 in x and y directions, respectively, and Vx,k−1 and Vy,k−1 are the velocity measurements of the AUV at the time k−1 in x and y directions, respectively. The ωk−1 denotes the process noise, which often follows the Gaussian distribution with zero mean vector and process error covariance Qk−1. Similarly, υk means the measurement noise, which follows the Gaussian distribution with zero mean vector and measurement error covariance Rk. The fk−1⋅ is the known process function at the time k−1. The Ηk is the measurement model at the time k.However, the underwater environment is complex and time-varying. Hence, the measurement information is unavoidable lost, which is described as follows:(2)Yk=0,pro=a,Zk,pro=1−a,where Yk is the observation vector and pro means the probability.


## 3. Cubature Unscented Kalman Filter (CUKF)

### 3.1. State Model and Measurement Models of a Linear System

For linear systems, the state transfer model is [37](3)Xt=FtXt+Wt,where F can be established from system transfer models; W is the system input noise, assumed to be white with zero mean. The state vector is defined asX=δLδλδhδVEδVNδVUϕEϕNϕU∇bx∇by∇bzεbxεbyεbzMxMyMzTwith δL, δλ, and δh denoting the latitude, longitude, and height position error in ENU axes, respectively; δVE, δVN, and δVU are the east, north, and upward velocity errors in ENU axes, respectively; ϕE, ϕN, and ϕU represent the heading, pitch, and roll errors in ENU axes, respectively; ∇bx, ∇by, and ∇bz set as the biases of the accelerometer projections onto the ENU axes; εbx, εby, and εbz for the gyroscope drift projections onto the ENU axes, respectively; Mx, My, and Mz are the measured magnetic field vector in the ENU axes, respectively.The linear measurement model is defined as(4)Zt=VEINS−VEDRVNINS−VNDRVUINS−VUDRφGyro−φMagθGyro−θAcceγGyro−γAcce=HtXt+Vt,where VEINS, VNINS, and VUINS are the estimated velocities by DR (Dead Reckoning) along the east, north, and upward direction, respectively; VEDR, VNDR, and VUDR represent the measurement velocity of INS (Inertial Navigation System) along the east, north, and upward direction, respectively. φGyro and φMag are the heading measured by gyroscopes and magnetometers, respectively; θGyro and θAcce denote the pitch measured by gyroscopes and accelerometers, respectively; γGyro and γAcce describe the roll measured by gyroscopes and accelerometers, respectively; H is the observation matrix; V is a white Gaussian measurement noise with zero mean value.


### 3.2. CUKF Models

Generally, for underwater navigation, the state and measurement models of multisensor fusion are nonlinear [38, 39]. How to derive a nonlinear sequential state estimation from equations (3) and (4) is the main task. One can draw the nonlinear state and measurement models as follows:(5)Xk+1=fXk+Wk,(6)Zk=hXk+Vk,where Xk and Zk represent the state vector and measurement at time step k, respectively. f⋅ is n-dimensional nonlinear function for state transition and h⋅ is the m-dimensional nonlinear measurement function. Wk and Vk are zero mean process noise and measurement noise with covariance matrix Qk and Rk, respectively. Through discretization, it is straightforward to convert the process and measurement equations in (12) and (13) to the nonlinear process function f⋅ and measurement function h⋅ in (5) and (6).The degree of nonlinearity in the practical underwater navigation is much bigger than the theoretical analysis, especially for MEMS-grade sensors [40, 41]. It may not achieve the desired effect for traditional UKF, and this paper proposes a new algorithm of cubature unscented Kalman filter (CUKF) considering the trade-off among different aspects to improve further accuracy, real-time, and stability.The procedure for implementing the CUKF can be summarized as follows:Step 1:
Sample. Then-dimensional random variable Xk with mean X^k and covariance P^k is approximated by sigma points selected using the following equations:(7)χi,k−1=X^k−1,i=0,χi,k−1=X^k−1+aLP^k−1i,i=1,…,L,χi,k−1=X^k−1−aLP^k−1i−L,i=L+1,…,2L,wherea∈R is a turning parameter denoting the spread of the sigma points around X^k−1 and is often set to a small positive value. This parameter only affects errors caused by more than second-order matrices. The LP^k−1i is the ithcolumn of the matrix square root of LP^k−1 which is real symmetric positive definite matrix obtained through Cholesky resolution for LP^k−1.The sample points are composed of sigma points setχi,i=0,…,2L, and the corresponding weight with these sample points is described as ω0=1−1/a2, ωi=1/2La2,i=1,…,2L, ωi is the weight of ithsigma point, and ∑i=02nωi=1.Step 2:
The first prediction. Each point is instantiated through the process model to yield a set of transformed samples(8)χi,k|k−1=fk−1χi,k−1.The predicted mean and covariance are computed as(9)X^k|k−1=∑i=02Lωiχi,k|k−1,P^k|k−1=∑i=02Lωiχi,k|k−1−X^k|k−1χi,k|k−1−X^k|k−1T+Qk.Step 3:
The first update, with the process of measurement update, is as follows:The measurement vector is(10)Zk=hk−1χi,k|k−1.The first predicted measurement vector is(11)Z^k=∑i=02nωiZi,k|k−1.The first covariance matrix of the innovations is(12)P^kzz=∑i=02nωiZi,k|k−1−Z^kZi,kk−1−Z^kT+Rk.The first cross covariance matrix between the predicted state estimate errors and innovations is(13)P^kxz=∑i=02nωiχi,k|k−1−X^k|k−1Zi,k|k−1−Z^kT.The gain matrix is(14)Kk=P^kxzP^kzz−1.The updated state estimates are(15)X^k=X^k|k−1+KkZk−Z^k.The covariance matrix of errors in the updated state estimates is(16)P^k=P^k|k−1+KkP^z,kKkT.Step 4:
Cubature points generationUpdate the covariance matrix of error(17)P^k=SkSkT.Generate the cubature points(18)ξj,k=ξj,k1Tξj,k2T=Akξj+X^k,j=1,…,N.Calculate the propagated cubature points(19)μj,k+1|k=fkξj,k1,j=1,…,N,πj,k+1|k=hkξj,k1,j=1,…,N,Step 5:
The second prediction.The predicted mean and covariance are calculated as(20)X^k+1|k=12N∑j=12Nμj,k+1|k,P^k+1|k=12N∑j=12Nμj,k+1|kμj,k+1|kT−X^k+1|kX^k+1|kT+Qk.Step 6:
The second update.Factorize(21)P^k+1|k=Sk+1Sk+1T.Generate the cubature points(22)ξj,k+1=Ak+1ξj+X^k+1|k,j=1,…,2L.Calculate the propagated cubature points:(23)πj,k+1|k∗=hk+1ξj,k+1,j=1,…,2L.The second predicted measurement(24)Z^k+1=12L∑j=12Lπj,k+1|k∗.The second covariance matrix of the innovations(25)P^k+1zz=12L∑j=12Lπj,k+1|k∗πj,k+1|k∗T−Z^k+1Zk+1T+Rk+1,P^k+1zz=12L∑j=12Lπj,k+1|k∗πj,k+1|k∗T−Z^k+1Zk+1T+Rk+1.Step 7:
Compute gain matrix, estimate state, and its covariance matrix secondly:(26)Kk+1=P^k+1xzP^k+1zz−1,X^k+1=X^k+1|k+Kk+1Zk+1−Z^k+1,P^k+1=P^k+1|k+Kk+1P^k+1zzKk+1T.Step 8:
Repeat Steps 1 to 7 for the next sample.


## 3.1. State Model and Measurement Models of a Linear System

For linear systems, the state transfer model is [37](3)Xt=FtXt+Wt,where F can be established from system transfer models; W is the system input noise, assumed to be white with zero mean. The state vector is defined asX=δLδλδhδVEδVNδVUϕEϕNϕU∇bx∇by∇bzεbxεbyεbzMxMyMzTwith δL, δλ, and δh denoting the latitude, longitude, and height position error in ENU axes, respectively; δVE, δVN, and δVU are the east, north, and upward velocity errors in ENU axes, respectively; ϕE, ϕN, and ϕU represent the heading, pitch, and roll errors in ENU axes, respectively; ∇bx, ∇by, and ∇bz set as the biases of the accelerometer projections onto the ENU axes; εbx, εby, and εbz for the gyroscope drift projections onto the ENU axes, respectively; Mx, My, and Mz are the measured magnetic field vector in the ENU axes, respectively.The linear measurement model is defined as(4)Zt=VEINS−VEDRVNINS−VNDRVUINS−VUDRφGyro−φMagθGyro−θAcceγGyro−γAcce=HtXt+Vt,where VEINS, VNINS, and VUINS are the estimated velocities by DR (Dead Reckoning) along the east, north, and upward direction, respectively; VEDR, VNDR, and VUDR represent the measurement velocity of INS (Inertial Navigation System) along the east, north, and upward direction, respectively. φGyro and φMag are the heading measured by gyroscopes and magnetometers, respectively; θGyro and θAcce denote the pitch measured by gyroscopes and accelerometers, respectively; γGyro and γAcce describe the roll measured by gyroscopes and accelerometers, respectively; H is the observation matrix; V is a white Gaussian measurement noise with zero mean value.


## 3.2. CUKF Models

Generally, for underwater navigation, the state and measurement models of multisensor fusion are nonlinear [38, 39]. How to derive a nonlinear sequential state estimation from equations (3) and (4) is the main task. One can draw the nonlinear state and measurement models as follows:(5)Xk+1=fXk+Wk,(6)Zk=hXk+Vk,where Xk and Zk represent the state vector and measurement at time step k, respectively. f⋅ is n-dimensional nonlinear function for state transition and h⋅ is the m-dimensional nonlinear measurement function. Wk and Vk are zero mean process noise and measurement noise with covariance matrix Qk and Rk, respectively. Through discretization, it is straightforward to convert the process and measurement equations in (12) and (13) to the nonlinear process function f⋅ and measurement function h⋅ in (5) and (6).The degree of nonlinearity in the practical underwater navigation is much bigger than the theoretical analysis, especially for MEMS-grade sensors [40, 41]. It may not achieve the desired effect for traditional UKF, and this paper proposes a new algorithm of cubature unscented Kalman filter (CUKF) considering the trade-off among different aspects to improve further accuracy, real-time, and stability.The procedure for implementing the CUKF can be summarized as follows:Step 1:
Sample. Then-dimensional random variable Xk with mean X^k and covariance P^k is approximated by sigma points selected using the following equations:(7)χi,k−1=X^k−1,i=0,χi,k−1=X^k−1+aLP^k−1i,i=1,…,L,χi,k−1=X^k−1−aLP^k−1i−L,i=L+1,…,2L,wherea∈R is a turning parameter denoting the spread of the sigma points around X^k−1 and is often set to a small positive value. This parameter only affects errors caused by more than second-order matrices. The LP^k−1i is the ithcolumn of the matrix square root of LP^k−1 which is real symmetric positive definite matrix obtained through Cholesky resolution for LP^k−1.The sample points are composed of sigma points setχi,i=0,…,2L, and the corresponding weight with these sample points is described as ω0=1−1/a2, ωi=1/2La2,i=1,…,2L, ωi is the weight of ithsigma point, and ∑i=02nωi=1.Step 2:
The first prediction. Each point is instantiated through the process model to yield a set of transformed samples(8)χi,k|k−1=fk−1χi,k−1.The predicted mean and covariance are computed as(9)X^k|k−1=∑i=02Lωiχi,k|k−1,P^k|k−1=∑i=02Lωiχi,k|k−1−X^k|k−1χi,k|k−1−X^k|k−1T+Qk.Step 3:
The first update, with the process of measurement update, is as follows:The measurement vector is(10)Zk=hk−1χi,k|k−1.The first predicted measurement vector is(11)Z^k=∑i=02nωiZi,k|k−1.The first covariance matrix of the innovations is(12)P^kzz=∑i=02nωiZi,k|k−1−Z^kZi,kk−1−Z^kT+Rk.The first cross covariance matrix between the predicted state estimate errors and innovations is(13)P^kxz=∑i=02nωiχi,k|k−1−X^k|k−1Zi,k|k−1−Z^kT.The gain matrix is(14)Kk=P^kxzP^kzz−1.The updated state estimates are(15)X^k=X^k|k−1+KkZk−Z^k.The covariance matrix of errors in the updated state estimates is(16)P^k=P^k|k−1+KkP^z,kKkT.Step 4:
Cubature points generationUpdate the covariance matrix of error(17)P^k=SkSkT.Generate the cubature points(18)ξj,k=ξj,k1Tξj,k2T=Akξj+X^k,j=1,…,N.Calculate the propagated cubature points(19)μj,k+1|k=fkξj,k1,j=1,…,N,πj,k+1|k=hkξj,k1,j=1,…,N,Step 5:
The second prediction.The predicted mean and covariance are calculated as(20)X^k+1|k=12N∑j=12Nμj,k+1|k,P^k+1|k=12N∑j=12Nμj,k+1|kμj,k+1|kT−X^k+1|kX^k+1|kT+Qk.Step 6:
The second update.Factorize(21)P^k+1|k=Sk+1Sk+1T.Generate the cubature points(22)ξj,k+1=Ak+1ξj+X^k+1|k,j=1,…,2L.Calculate the propagated cubature points:(23)πj,k+1|k∗=hk+1ξj,k+1,j=1,…,2L.The second predicted measurement(24)Z^k+1=12L∑j=12Lπj,k+1|k∗.The second covariance matrix of the innovations(25)P^k+1zz=12L∑j=12Lπj,k+1|k∗πj,k+1|k∗T−Z^k+1Zk+1T+Rk+1,P^k+1zz=12L∑j=12Lπj,k+1|k∗πj,k+1|k∗T−Z^k+1Zk+1T+Rk+1.Step 7:
Compute gain matrix, estimate state, and its covariance matrix secondly:(26)Kk+1=P^k+1xzP^k+1zz−1,X^k+1=X^k+1|k+Kk+1Zk+1−Z^k+1,P^k+1=P^k+1|k+Kk+1P^k+1zzKk+1T.Step 8:
Repeat Steps 1 to 7 for the next sample.


## 4. Quadratic Interpolation-Based Variational Bayesian Filter

### 4.1. Lagrange Interpolation Polynomial

When the measurement information is lost, the Lagrange interpolation polynomial (LIP) can be used to calculate the predicted observed vectorY⌢k. The QI method is used because the Runge phenomenon needs to be avoided [42, 43].Assumption 1.
(a) The measurement information is lost at the timek. (b) The measurement information is observed from the time 1 to k-1, as the method function in Figure 1.
The QI polynomial is described as(27)L2t=Yk−3lk−3t+Yk−2lk−2t+Yk−1lk−1t,where lk−3t=t−tk−2t−tk−1/tk−3−tk−2tk−3−tk−1, lk−2t=t−tk−3t−tk−1/tk−2−tk−3tk−2−tk−1, and lk−1t=t−tk−3t−tk−2/tk−1−tk−3tk−1−tk−2.
Hence,Y⌢kcan be factored as follows:(28)Y⌢k=Yk−3lk−3tk+Yk−2lk−2tk+Yk−1lk−1tk=Yk−3tk−tk−2tk−tk−1tk−3−tk−2tk−3−tk−1+Yk−2tk−tk−3tk−tk−1tk−2−tk−3tk−2−tk−1+Yk−1tk−tk−3tk−tk−2tk−1−tk−3tk−1−tk−2,where Y⌢kis the predicted observed vector at the time k and tk means the time of k.Figure 1
QI method from the time sequence 1 to (k)−1. (a) QI basis function at the time sequence k−3. (b) QI basis function at the time sequence k−2. (c) QI basis function at the time k−1.
(a)(b)(c)


### 4.2. The Variational Bayesian Approach

In order to estimate the state vectorXk|k with the measurement information lost, the joint PDF PDFXk|k,Pk|k−1,Rk,Y1:k−1,Y⌢k is considered to calculate the optimal solutions of Xk|k, Pk|k−1 and Rk. However, this PDF cannot be obtained, so the VB method is used to find some independent PDFs q⋅, which are used to approximate PDFXk|k,Pk|k−1,Rk,Y1:k−1,Y⌢k.SetΘ=Xk||k,Pk|k−1,Rk. Then, the optimal solutions of Θ, which is satisfied, are as follows:(29)logqκ=EΘ−κlogPDFΘ,Y1:k−1,Y⌢k+Cκ,where EΘ−κlogPDFΘ,Y1:k−1,Y⌢k is the expectation logPDFΘ,Y1:k−1,Y⌢k of the Θ except for κ. Cκ is a constant about κ. In Bayesian statistics theory, the covariance matrix is defined to follow the inverse Wishart (IW) distribution.The distributions ofPk|k−1and Rk are set in(30)qPk|k−1=IWPk|k−1;mk|k−1,Mk|k−1,qRk=IWRk;uk|k−1,Uk|k−1,where IWPk|k−1;mk|k−1,Mk|k−1 is the variable Pk|k−1 that follows IW distribution with mk|k−1 degree of freedom (DOF) and Mk|k−1 inverse scale matrix. IWRk;uk|k−1,Uk|k−1 is the variable Rk that follows IW distribution with uk|k−1 DOF and Uk|k−1 inverse scale matrix;logPDFΘ,Y1:k−1,Y⌢k can be rewritten as follows:(31)PDFΘ,Y1:k−1,Y⌢k=PDFY⌢k|Xk|k,RkPDFXk|k|Y1:k−1,Pk|k−1×IWPk|k−1;mk|k−1,Mk|k−1IWRk;uk|k−1,Uk|k−1PDFY1:k−1.Hence,PDFΘ,Y1:k−1,Y⌢k is expended in(32)PDFΘ,Y1:k−1,Y⌢k=NY⌢k;hkXk|k,RkNXk|k;Xk|k−1,Pk|k−1×IWPk|k−1;mk|k−1,Mk|k−1IWRk;uk|k−1,Uk|k−1ProY1:k−1.The optimal solution is defined as follows:(33)logPDFΘ,Y1:k−1,Y⌢k=−12Y⌢k−ΗkXk|klTRk−1Y⌢k−ΗkXk|kl−12Xk|k−Xk|k−1TPk|k−1−1Xk|k−Xk|k−1−12nY⌢+uk|k−1+2log|Rk−12trUk|k−1Rk−1−12nX+mk|k−1+2logPk|k−1−12trMk|k−1Pk|k−1−1+Cκ.When choosingκ=Pk|k−1, the parameters of Pk|k−1 are given by using(34)mk|k−1l+1=mk|k−1l+1,Mk|k−1l+1=Akl+Mk|k−1l,where Akl=Pk|k−1+Xk|kl−Xk|k−1Xk|kl−Xk|k−1T.When choosingκ=Rk, the parameters of Rk are given by using(35)uk|k−1l+1=uk|k−1l+1,Uk|k−1l+1=Bkl+Uk|k−1l,where Bkl is calculated as follows:(36)Bkl=ElY⌢k−ΗkXk|klY⌢k−ΗkXk|klT.Then,Pk|k−1 and Rk at l+1th step are calculated as follows, respectively:(37)El+1Pk|k−1=Mk|k−1l+1mk|k−1l+1−nX−1,(38)El+1Rk=Uk|k−1l+1uk|k−1l+1−nY⌢−1.When choosingκ=Xk|k, qXk|k is modelled as follows:(39)qXk|k=NXk|k;Xk|kl+1,Pk|kl+1,where NXk|k;Xk|kl+1,Pk|kl+1 denotes the variable Xk|k that follows Gaussian distribution with Xk|kl+1 mean vector and Pk|kl+1 error covariance matrix, and the measurement update is written as follows:(40)Xk|kl+1=Xk|kl+Kkl+1Y⌢k−ΗkXk|kl,Pk|kl+1=Pk|k−1l+1−Kkl+1ΗkPk|k−1l+1,where the Kalman gain Kkl+1 at the l+1th step is calculated by using (41) at time k.(41)Kkl+1=Pk|k−1l+1HkTHkPk|k−1l+1HkT+Rkl+1−1.After fixed-point iterationNVB, the VB approximate of PDF is derived by(42)qPk|k−1≈qNVBPk|k−1=IWPk|k−1;mk|k−1NVB,Mk|k−1NVB,qRk≈qNVBRk=IWRk;uk|k−1NVB,Uk|k−1NVB,qXk|k≈qNVBXk|k=NXk|k;Xk|kNVB,Pk|kNVB.The pseudocode is presented in Table1. If the measurement information vector is lost, the LIP method is used to calculate the predicted observed vector Y⌢k. Then, the predicted state vector Xk|k−1 and predicted error covariance matrix Pk|k−1 are calculated by using the state vector Xk|k, error covariance matrix Pk|k and process noise matrix Qk. Then, Xk|k0, Pk|k−10, Mk|k−10, mk|k−10, Uk|k−10 and uk|k−10 are initialized for the subsequent variational Bayesian measurement estimation. The predicted error covariance matrix Pk|k−1l+1 and the measurement noise matrix Rkl+1 at the l+1 th step are calculated in order to estimate the state vector Xk|kl+1 and error covariance matrix Pk|kl+1 at the l+1 th step, respectively. Finally, after NVB th iteration, the accurate state vector Xk|k and error covariance matrix Pk|k are determined at the time k.Table 1
The pseudocode of the proposed algorithm.
PseudocodeInput:X0|0, P0|0, Q0LIP method:Y⌢k=Yk−3lk−3tk+Yk−2lk−2tk+Yk−1lk−1tk=Yk−3tk−tk−2tk−tk−1/tk−3−tk−2tk−3−tk−1+Yk−2tk−tk−3tk−tk−1/tk−2−tk−3tk−2−tk−1+Yk−1tk−tk−3tk−tk−2/tk−1−tk−3tk−1−tk−2Time update:Xk|k−1=fk−1Xk−1|k−1Pk|k−1=Xk|k−1−Xk−1|k−1Xk|k−1−Xk−1|k−1T+Qk−1Variational Bayesian measurement update initialization:Xk|k0=Xk|k−1, Pk|k−10=Pk|k−1, Mk|k−10, mk|k−10, Uk|k−10, uk|k−10.Variational Bayesian measurement estimation: forl = 0: NVB-1The parameters ofPk|k−1:mk|k−1l+1=mk|k−1l+1Mk|k−1l+1=Akl+Mk|k−1lThe parameters ofRk:uk|k−1l+1=uk|k−1l+1Uk|k−1l+1=Bkl+Uk|k−1lThe expectations ofPk|k−1 and Rk are calculated:El+1Pk|k−1=Mk|k−1l+1/mk|k−1l+1−nX−1El+1Rk=Uk|k−1l+1/uk|k−1l+1−nY⌢−1The state vectorXk|kl+1 and error covariance matrix Pk|kl+1:Xk|kl+1=Xk|kl+Kkl+1Y⌢k−ΗkXk|klPk|kl+1=Pk|k−1l+1−Kkl+1ΗkPk|k−1l+1endOutput:Xk|k, Pk|k.


## 4.1. Lagrange Interpolation Polynomial

When the measurement information is lost, the Lagrange interpolation polynomial (LIP) can be used to calculate the predicted observed vectorY⌢k. The QI method is used because the Runge phenomenon needs to be avoided [42, 43].Assumption 1.
(a) The measurement information is lost at the timek. (b) The measurement information is observed from the time 1 to k-1, as the method function in Figure 1.
The QI polynomial is described as(27)L2t=Yk−3lk−3t+Yk−2lk−2t+Yk−1lk−1t,where lk−3t=t−tk−2t−tk−1/tk−3−tk−2tk−3−tk−1, lk−2t=t−tk−3t−tk−1/tk−2−tk−3tk−2−tk−1, and lk−1t=t−tk−3t−tk−2/tk−1−tk−3tk−1−tk−2.
Hence,Y⌢kcan be factored as follows:(28)Y⌢k=Yk−3lk−3tk+Yk−2lk−2tk+Yk−1lk−1tk=Yk−3tk−tk−2tk−tk−1tk−3−tk−2tk−3−tk−1+Yk−2tk−tk−3tk−tk−1tk−2−tk−3tk−2−tk−1+Yk−1tk−tk−3tk−tk−2tk−1−tk−3tk−1−tk−2,where Y⌢kis the predicted observed vector at the time k and tk means the time of k.Figure 1
QI method from the time sequence 1 to (k)−1. (a) QI basis function at the time sequence k−3. (b) QI basis function at the time sequence k−2. (c) QI basis function at the time k−1.
(a)(b)(c)


## 4.2. The Variational Bayesian Approach

In order to estimate the state vectorXk|k with the measurement information lost, the joint PDF PDFXk|k,Pk|k−1,Rk,Y1:k−1,Y⌢k is considered to calculate the optimal solutions of Xk|k, Pk|k−1 and Rk. However, this PDF cannot be obtained, so the VB method is used to find some independent PDFs q⋅, which are used to approximate PDFXk|k,Pk|k−1,Rk,Y1:k−1,Y⌢k.SetΘ=Xk||k,Pk|k−1,Rk. Then, the optimal solutions of Θ, which is satisfied, are as follows:(29)logqκ=EΘ−κlogPDFΘ,Y1:k−1,Y⌢k+Cκ,where EΘ−κlogPDFΘ,Y1:k−1,Y⌢k is the expectation logPDFΘ,Y1:k−1,Y⌢k of the Θ except for κ. Cκ is a constant about κ. In Bayesian statistics theory, the covariance matrix is defined to follow the inverse Wishart (IW) distribution.The distributions ofPk|k−1and Rk are set in(30)qPk|k−1=IWPk|k−1;mk|k−1,Mk|k−1,qRk=IWRk;uk|k−1,Uk|k−1,where IWPk|k−1;mk|k−1,Mk|k−1 is the variable Pk|k−1 that follows IW distribution with mk|k−1 degree of freedom (DOF) and Mk|k−1 inverse scale matrix. IWRk;uk|k−1,Uk|k−1 is the variable Rk that follows IW distribution with uk|k−1 DOF and Uk|k−1 inverse scale matrix;logPDFΘ,Y1:k−1,Y⌢k can be rewritten as follows:(31)PDFΘ,Y1:k−1,Y⌢k=PDFY⌢k|Xk|k,RkPDFXk|k|Y1:k−1,Pk|k−1×IWPk|k−1;mk|k−1,Mk|k−1IWRk;uk|k−1,Uk|k−1PDFY1:k−1.Hence,PDFΘ,Y1:k−1,Y⌢k is expended in(32)PDFΘ,Y1:k−1,Y⌢k=NY⌢k;hkXk|k,RkNXk|k;Xk|k−1,Pk|k−1×IWPk|k−1;mk|k−1,Mk|k−1IWRk;uk|k−1,Uk|k−1ProY1:k−1.The optimal solution is defined as follows:(33)logPDFΘ,Y1:k−1,Y⌢k=−12Y⌢k−ΗkXk|klTRk−1Y⌢k−ΗkXk|kl−12Xk|k−Xk|k−1TPk|k−1−1Xk|k−Xk|k−1−12nY⌢+uk|k−1+2log|Rk−12trUk|k−1Rk−1−12nX+mk|k−1+2logPk|k−1−12trMk|k−1Pk|k−1−1+Cκ.When choosingκ=Pk|k−1, the parameters of Pk|k−1 are given by using(34)mk|k−1l+1=mk|k−1l+1,Mk|k−1l+1=Akl+Mk|k−1l,where Akl=Pk|k−1+Xk|kl−Xk|k−1Xk|kl−Xk|k−1T.When choosingκ=Rk, the parameters of Rk are given by using(35)uk|k−1l+1=uk|k−1l+1,Uk|k−1l+1=Bkl+Uk|k−1l,where Bkl is calculated as follows:(36)Bkl=ElY⌢k−ΗkXk|klY⌢k−ΗkXk|klT.Then,Pk|k−1 and Rk at l+1th step are calculated as follows, respectively:(37)El+1Pk|k−1=Mk|k−1l+1mk|k−1l+1−nX−1,(38)El+1Rk=Uk|k−1l+1uk|k−1l+1−nY⌢−1.When choosingκ=Xk|k, qXk|k is modelled as follows:(39)qXk|k=NXk|k;Xk|kl+1,Pk|kl+1,where NXk|k;Xk|kl+1,Pk|kl+1 denotes the variable Xk|k that follows Gaussian distribution with Xk|kl+1 mean vector and Pk|kl+1 error covariance matrix, and the measurement update is written as follows:(40)Xk|kl+1=Xk|kl+Kkl+1Y⌢k−ΗkXk|kl,Pk|kl+1=Pk|k−1l+1−Kkl+1ΗkPk|k−1l+1,where the Kalman gain Kkl+1 at the l+1th step is calculated by using (41) at time k.(41)Kkl+1=Pk|k−1l+1HkTHkPk|k−1l+1HkT+Rkl+1−1.After fixed-point iterationNVB, the VB approximate of PDF is derived by(42)qPk|k−1≈qNVBPk|k−1=IWPk|k−1;mk|k−1NVB,Mk|k−1NVB,qRk≈qNVBRk=IWRk;uk|k−1NVB,Uk|k−1NVB,qXk|k≈qNVBXk|k=NXk|k;Xk|kNVB,Pk|kNVB.The pseudocode is presented in Table1. If the measurement information vector is lost, the LIP method is used to calculate the predicted observed vector Y⌢k. Then, the predicted state vector Xk|k−1 and predicted error covariance matrix Pk|k−1 are calculated by using the state vector Xk|k, error covariance matrix Pk|k and process noise matrix Qk. Then, Xk|k0, Pk|k−10, Mk|k−10, mk|k−10, Uk|k−10 and uk|k−10 are initialized for the subsequent variational Bayesian measurement estimation. The predicted error covariance matrix Pk|k−1l+1 and the measurement noise matrix Rkl+1 at the l+1 th step are calculated in order to estimate the state vector Xk|kl+1 and error covariance matrix Pk|kl+1 at the l+1 th step, respectively. Finally, after NVB th iteration, the accurate state vector Xk|k and error covariance matrix Pk|k are determined at the time k.Table 1
The pseudocode of the proposed algorithm.
PseudocodeInput:X0|0, P0|0, Q0LIP method:Y⌢k=Yk−3lk−3tk+Yk−2lk−2tk+Yk−1lk−1tk=Yk−3tk−tk−2tk−tk−1/tk−3−tk−2tk−3−tk−1+Yk−2tk−tk−3tk−tk−1/tk−2−tk−3tk−2−tk−1+Yk−1tk−tk−3tk−tk−2/tk−1−tk−3tk−1−tk−2Time update:Xk|k−1=fk−1Xk−1|k−1Pk|k−1=Xk|k−1−Xk−1|k−1Xk|k−1−Xk−1|k−1T+Qk−1Variational Bayesian measurement update initialization:Xk|k0=Xk|k−1, Pk|k−10=Pk|k−1, Mk|k−10, mk|k−10, Uk|k−10, uk|k−10.Variational Bayesian measurement estimation: forl = 0: NVB-1The parameters ofPk|k−1:mk|k−1l+1=mk|k−1l+1Mk|k−1l+1=Akl+Mk|k−1lThe parameters ofRk:uk|k−1l+1=uk|k−1l+1Uk|k−1l+1=Bkl+Uk|k−1lThe expectations ofPk|k−1 and Rk are calculated:El+1Pk|k−1=Mk|k−1l+1/mk|k−1l+1−nX−1El+1Rk=Uk|k−1l+1/uk|k−1l+1−nY⌢−1The state vectorXk|kl+1 and error covariance matrix Pk|kl+1:Xk|kl+1=Xk|kl+Kkl+1Y⌢k−ΗkXk|klPk|kl+1=Pk|k−1l+1−Kkl+1ΗkPk|k−1l+1endOutput:Xk|k, Pk|k.


## 5. Simulations and Results

The proposed QIVBF is compared with the traditional KF in a simulation with measurement missing scenarios, and the root mean squared error (RMSE) is defined to compare the performances of these algorithms.(43)RMSEk=Px,k−Xkt2+Py,k−Ykt2,where Px,k,Py,k and Xkt,Ykt are the predicted position and reference position, respectively. The parameters are set in Table 2.Table 2
The simulation parameters.
ParametersValuesP0|0eye4Qk−110−3×diag1,0.5,1,0.5Rkeye4According to the navigation system, the accuracy of the methods is compared, and the above position estimation errors are analyzed. From Figures2 and 3, the proposed QIVBF performs closer to the reference than the traditional KF. The proposed QIVBF results in a much better estimation accuracy than the existing filter on handling the problems of underwater navigation in the scenarios of measurement information missing. In particular, KF can easily lose tracking of the reference values by reason that the accumulated errors of iteration. On the other hand, the quadratic interpolation improves the observation vector for the precision and stability of estimation iterations when the environments change, or the measurement information is lost. Therefore, the results demonstrate that the proposed QIVBF outperforms the traditional KF in the condition of measurement information missing.Figure 2
The positions inx direction of the different algorithms.Figure 3
The positions iny direction of the different algorithms.The navigation accuracy and anti-interference ability of the KF and QIVBF are compared by calculating the root-mean-square error (RMSE) of the position estimation. The RMSE has a good reflection of the measurement precision. It can be seen from Figure4 that the QIVBF performs better than the traditional KF. When the measurement information is missing, the traditional KF cannot estimate the accurate Kalman gain, which leads to the filter divergence. However, the proposed QIVBF uses the QI method to estimate the lost observed vector, which can help estimate the accurate information about cross error covariance matrix. Then, the VB method can help calculate the accurate state vector and error covariance matrix. The RMSE of QIVBF is lower than KF. Hence, the performance of QIVBF is better than KF.Figure 4
RMSE of the KF and QIVBF algorithms.


## 6. Conclusions

The paper proposes a quadratic interpolation-based variational Bayesian filter for underwater navigation when measurement information is lost, where the quadratic interpolation method is used to calculate the observed vector when the measurement information is lost. Then, the state vector together with the predicted error covariance matrix and the measurement noise matrix is estimated based on the VB method. Simulation results demonstrated the effectiveness of the proposed QIVBF algorithm as compared with the traditional algorithm in the case of measurement information missing. The idea of the proposed QIVBF can be extended to design a nonlinear filter for the non-Gaussian noises with measurement information lost. In future work, we plan to apply multiple datasets of underwater navigation measurements, so that the position and attitude can be estimated based on the fusion of various data sources for better accuracy.



---

*Source: 6666411-2020-12-23.xml*