# Maximum Likelihood-Based Measurement Noise Covariance Estimation Using Sequential Quadratic Programming for Cubature Kalman Filter Applied in INS/BDS Integration

Authors: Bingbing Gao; Gaoge Hu; Wenmin Li; Yan Zhao; Yongmin Zhong
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

With the completion of the Beidou-3 system (BDS) in China, INS/BDS integration will become a promising navigation and positioning strategy. However, due to the nonlinear propagation characteristic of INS error and inevitable involvement of inaccurate measurement noise statistics, it is difficult to achieve the optimal solution through the INS/BDS integration. This paper proposes a method of cubature Kalman filter (CKF) with the measurement noise covariance estimation by using the maximum likelihood principle to solve the abovementioned problem. It establishes an estimation model for measurement noise covariance according to the maximum likelihood principle, and then, its estimation is calculated by utilizing the sequential quadratic programming. The estimated measurement noise covariance will be fed back to the procedure of CKF to improve its adaptability. Simulation and comparison analysis verify that the proposed method can accurately estimate measurement noise covariance to effectively restrain its influence on navigation solution, leading to improved navigation performance for the INS/BDS integration.


---

## Body

## 1. Introduction

Nowadays, the inertial navigation system (INS) plays an important role in the vehicle navigation community due to its autonomy and comprehensive navigation information [1, 2]. However, its navigation error will increase unboundedly with time because the drift is involved in the inertial measurement units [2, 3]. By contrast, the global navigation satellite system (GNSS) is a satellite-based system, providing the highly accurate information on velocity and position of a vehicle in a long time [3, 4]. However, it is difficult to achieve the continuous localization by the GNSS since satellite signals may be lost and corrupted in the environmental conditions such as high buildings, tunnels, and mountains [4, 5]. Recently, the Beidou-3 system (BDS) of China has been successfully built and become a typical GNSS. It will be the backbone of global positioning and navigation [6, 7]. However, it still suffers from the problems of commonly used GNSS [6].The INS is commonly integrated with the GNSS or BDS to take advantages of the two techniques [8, 9]. It overcomes the limitations of two standalone systems, that is, long-time drift of the INS and easy to inference for the GNSS. It also sufficiently exploits the advantages of both systems, such as the consistently high accuracy of the GNSS and the short-term stability of the INS [9, 10]. Consequently, INS/GNSS integration is a promising solution to improve the navigation and positioning accuracy by utilizing the GNSS information to compensate the error of the INS. Due to these reasons, this integration has been widely used in many real applications, such as aircraft landing, autonomous underwater vehicle, air-space-ground integrated network, unmanned aerial vehicle, and so on [8–10].The state estimation problem is of importance for the INS/GNSS or INS/BDS integration. It is still one of the most significant open research problems and attracted wide interest in the research community [11, 12]. The Kalman filter is an optimal filter in the sense of the minimum mean square error criterion for the linear system [12]. However, the INS error propagation always reflects nonlinear characteristics, thus adopting the nonlinear INS error model which can better describe the propagation characteristic of INS error [12, 13]. The commonly used linear system model is only a theoretical approximation to the actual nonlinear system with limited approximation accuracy, leading to decreased navigation performance for INS/GNSS integration [13, 14].The unscented Kalman Filter (UKF) has received increased attention for INS/GNSS integration due to the ability of dealing with strongly nonlinear systems [12–14]. This filter uses a set of deterministically selected sigma points to approximate the probability distribution of system state and further propagates them directly through nonlinear system functions. It avoids system model linearization and has higher-order approximation accuracy than the traditional extended Kalman filter (EKF) [14, 15]. However, the UKF is considered to be unstable, especially for high-dimensional (more than 3) nonlinear systems [15, 16].As an emerging nonlinear filtering technology, the cubature Kalman filter (CKF) is an improvement to the traditional nonlinear filters such as the EKF and UKF [16, 17]. It uses a third-degree spherical-radical cubature rule to approximate the involved Gaussian-weighted integrals in a nonlinear Gaussian filter framework and can approximate the posterior mean and covariance of system state vector for any nonlinear system in second-order accuracy, which is higher than the EKF in first-order accuracy and has better numerical stability than the UKF for high-dimensional nonlinear systems such as a multisensor integrated navigation system [16–18]. Furthermore, the CKF only contains 2n cubature points, which is smaller than the UKF with 2n + 1 sigma points. Thus, the CKF has a better computational real-time performance than the UKF [19]. However, all the abovementioned nonlinear filters are dependent on the accurate prior statistical information for process and measurement noise [3, 11]. If the noise statistics are inaccurate to be used, the performance will be deteriorated. In the INS/GNSS integration, the precision of the system state equation can be ensured by using laser or optical inertial measurement units of ultrahigh accuracy [2, 20]. Nevertheless, the GNSS and BDS are always affected by the external environment such as urban canyons, tunnels, and foliage conditions, leading to biased measurement noise statistics and further failure to provide superior navigation result [4, 20].Various methods were reported to handle the inaccurate measurement noise statistics for the CKF state estimation. Zhang et al. developed an H-infinity strategy-based robust CKF by minimizing the estimation error in the worst case [17]. However, it may break down in the presence of randomly occurring inaccurate noise statistics [21]. For the non-Gaussian measurement noise, Liu et al. proposed a maximum correntropy-based square-root CKF (MCSCKF) [12]. However, the construction of the estimation error covariance matrix in this method is not based on theoretical analysis [22], making the improvement of this method questionable. The M-estimation can also be combined with the CKF to curb the influence of inaccurate measurement noise statistics on system state estimation [18]. Nevertheless, the achievement of the robustness for this method is based on the cost of accuracy of the nonlinear transformation itself [23].The maximum likelihood principle provides a promising solution to address the parameter estimation problem for a statistical model [24, 25]. It can estimate the measurement noise statistics by maximizing the likelihood function of unknown measurement noise statistics based on a set of known measurements. The maximum likelihood principle can achieve the unique and converged solution in the probabilistic sense [25, 26]. Thus, this method has been gradually used for the noise statistics estimation of a dynamic system in recent years [24, 26]. However, the most existing maximum likelihood method is only for the Kalman filter in a linear system, there has been very limited research focusing on the use of the maximum likelihood principle for system noise statistic estimation in a nonlinear CKF.This paper presents a method of CKF with the measurement noise covariance estimation by using the maximum likelihood principle to improve the adaptability of INS/BDS integration. The proposed method establishes a theory of an estimation model for the measurement noise covariance based on the maximum likelihood principle. Subsequently, the sequential quadratic programming is used to calculate the estimation of measurement noise covariance, and it is fed back to the CKF procedure to improve the adaptability of the filter. Simulations and comparison analysis have been conducted to comprehensively evaluate the performance of the proposed method for INS/BDS integration.


## 2. System Model for INS/BDS Integration

The INS/BDS integration used for vehicle navigation in this paper is to utilize the accurate velocity and position by the BDS to correct the navigation error of the INS. An error model with nonlinear characteristics is established based on the additive quaternion, and the differences of velocity and position information between INS and BDS are taken as the measurement of the integrated system. Thus, the system model for INS/BDS integration is developed and described in the following.


### 2.1. System State Equation

We denote the navigation frame (n-frame) as the E-N-U (East-North-Up) geography frame (g-frame). The body frame, inertial frame, and earth frame are abbreviated as b-frame, i-frame, and e-frame, respectively. By using the attitude error quaternion, the INS’s attitude error equation can be described as [27](1)δQ˙bn=12ΩuωibbδQbn−12ΩdωinnδQbn+12UQbnδωibb−12YQbnδωinn,where δQbn=δq0δq1δq2δq3T and Qbn=q0q1q2q3 are the attitude error quaternion and attitude quaternion; ωibb=ωibxbωibybωibzbT denotes the body angular rate measured by three gyros in the b-frame; δωibb is the gyro error; ωinn=ωien+ωenn=ωinxnωinynωinznT represents the angular rate of the n-frame with respect to the i-frame and expressed in the n-frame; δωinn=δωien+δωenn is the corresponding error; the meanings of δωien, δωenn, δωien, and δωenn are given in (3); and the parameters Ωuωibb, Ωdωinn, UQbn, and YQbn are computed as(2)Ωuωibb=0−ωibxb−ωibyb−ωibzbωibxb0ωibzb−ωibybωibyb−ωibzb0ωibxbωibzbωibyb−ωibxb0,Ωdωinb=0−ωinxn−ωinyn−ωinznωinxn0−ωinznωinynωinynωinzn0−ωinxnωinzn−ωinynωinxn0,UQbn=−q1−q2−q3q0−q3q2q3q0−q1−q2q1q0,YQbn=−q1−q2−q3q0q3−q2−q3q0q1q2−q1q0.The INS’s velocity error equation is [27](3)δV˙n=δCbnfb−2ωien+ωenn×δVn+2δωien+δωenn×Vn+Cbnδfb,where δV˙n=δvEnδvNnδvUnT is the velocity error and Vn=vEvNvUT is the velocity in east, north, and up; δCbn is an attitude error coupling term, which has strongly nonlinear characteristics about δQbn as described in [28]; δQbn is the rotation matrix; fb is the specific force, δfb is the accelerometer error; ωien=0−δLωiesinLωiesinLT is the projection of the Earth rotation rate ωie; ωenn=−vN/RM+hvE/RN+hvEtanL/RN+hT is the angular rate of the n-frame with respect to the e-frame; where RM and RN denote the meridian and transverse radii of curvature and L and h represent the latitude and altitude of the vehicle; and δωien and δωenn are the corresponding errors.The INS’s position error is described as [28](4)δP˙n=MδVn+NδPn,where δPn=δLδλδhT is the position error in latitude, longitude, and altitude. M and N can be formulated as(5)M=01RM+h0secLRN+h00001,N=00−vNRM+h2vEtanLsecLRN+h0−vEsecLRN+h2000.The IMU error includes the errors of the gyro and accelerometer. The gyro error and accelerometer error both can be described as(6)δωibb=εb+wg,(7)ε˙b=0,(8)δfb=∇b+wa,(9)∇˙b=0,where εb=εbxεbyεbzT and ∇˙b=∇bx∇by∇bzT represent the constant drift of the gyro and the zero bias of the accelerometer; wg=wgxwgywgzT and wa=waxwaywazT are the white noises for the gyro and accelerometer.The system state variable is(10)Xt=δQbnTδVnTδPnTεbT∇bTT.Figure1 describes the constitute component of the system state equation for INS/BDS integration. As described in Figure 1, combining (1), (3), (4), (6), and (8), the system state equation of the INS/BDS integration can be obtained as(11)X˙t=f¯Xt+Wt,where f¯⋅ is the continuous-time nonlinear function describing the system state equation and Wt is the system state noise.Figure 1
Constitute component of system state equation for INS/BDS integration.To use the discrete-time CKF, (11) should be discretized via the improved Euler method [29].(12)Xk=fXk−1+Wk,where f⋅ is the discrete-time nonlinear function describing the system state equation and Wk is the discrete-time system state noise.


### 2.2. Measurement Equation

Taking the differences of velocity and position information between the INS and BDS as the system measurement vector,(13)Zk=vEI−vEBvNI−vNBvUI−vUBλI−λBLI−LBhI−hBT,where vEIvNIvUIT and λILIhIT or vEBvNBvUBT and λBLBhBT are the vehicle’s velocity and position information from the INS or BDS.Then, the measurement equation for the INS/BDS integration can be established by the measurement vector(14)Zk=HkXk+Vk,where Hk=HvHp is the measurement matrix, Hv=03×4diag1,1,103×9, Hp=03×7diagRM,RNcosL,103×9, and Vk=VvVp is the measurement noise vector, which obeys the zero-mean Gaussian distribution with covariance R.


## 2.1. System State Equation

We denote the navigation frame (n-frame) as the E-N-U (East-North-Up) geography frame (g-frame). The body frame, inertial frame, and earth frame are abbreviated as b-frame, i-frame, and e-frame, respectively. By using the attitude error quaternion, the INS’s attitude error equation can be described as [27](1)δQ˙bn=12ΩuωibbδQbn−12ΩdωinnδQbn+12UQbnδωibb−12YQbnδωinn,where δQbn=δq0δq1δq2δq3T and Qbn=q0q1q2q3 are the attitude error quaternion and attitude quaternion; ωibb=ωibxbωibybωibzbT denotes the body angular rate measured by three gyros in the b-frame; δωibb is the gyro error; ωinn=ωien+ωenn=ωinxnωinynωinznT represents the angular rate of the n-frame with respect to the i-frame and expressed in the n-frame; δωinn=δωien+δωenn is the corresponding error; the meanings of δωien, δωenn, δωien, and δωenn are given in (3); and the parameters Ωuωibb, Ωdωinn, UQbn, and YQbn are computed as(2)Ωuωibb=0−ωibxb−ωibyb−ωibzbωibxb0ωibzb−ωibybωibyb−ωibzb0ωibxbωibzbωibyb−ωibxb0,Ωdωinb=0−ωinxn−ωinyn−ωinznωinxn0−ωinznωinynωinynωinzn0−ωinxnωinzn−ωinynωinxn0,UQbn=−q1−q2−q3q0−q3q2q3q0−q1−q2q1q0,YQbn=−q1−q2−q3q0q3−q2−q3q0q1q2−q1q0.The INS’s velocity error equation is [27](3)δV˙n=δCbnfb−2ωien+ωenn×δVn+2δωien+δωenn×Vn+Cbnδfb,where δV˙n=δvEnδvNnδvUnT is the velocity error and Vn=vEvNvUT is the velocity in east, north, and up; δCbn is an attitude error coupling term, which has strongly nonlinear characteristics about δQbn as described in [28]; δQbn is the rotation matrix; fb is the specific force, δfb is the accelerometer error; ωien=0−δLωiesinLωiesinLT is the projection of the Earth rotation rate ωie; ωenn=−vN/RM+hvE/RN+hvEtanL/RN+hT is the angular rate of the n-frame with respect to the e-frame; where RM and RN denote the meridian and transverse radii of curvature and L and h represent the latitude and altitude of the vehicle; and δωien and δωenn are the corresponding errors.The INS’s position error is described as [28](4)δP˙n=MδVn+NδPn,where δPn=δLδλδhT is the position error in latitude, longitude, and altitude. M and N can be formulated as(5)M=01RM+h0secLRN+h00001,N=00−vNRM+h2vEtanLsecLRN+h0−vEsecLRN+h2000.The IMU error includes the errors of the gyro and accelerometer. The gyro error and accelerometer error both can be described as(6)δωibb=εb+wg,(7)ε˙b=0,(8)δfb=∇b+wa,(9)∇˙b=0,where εb=εbxεbyεbzT and ∇˙b=∇bx∇by∇bzT represent the constant drift of the gyro and the zero bias of the accelerometer; wg=wgxwgywgzT and wa=waxwaywazT are the white noises for the gyro and accelerometer.The system state variable is(10)Xt=δQbnTδVnTδPnTεbT∇bTT.Figure1 describes the constitute component of the system state equation for INS/BDS integration. As described in Figure 1, combining (1), (3), (4), (6), and (8), the system state equation of the INS/BDS integration can be obtained as(11)X˙t=f¯Xt+Wt,where f¯⋅ is the continuous-time nonlinear function describing the system state equation and Wt is the system state noise.Figure 1
Constitute component of system state equation for INS/BDS integration.To use the discrete-time CKF, (11) should be discretized via the improved Euler method [29].(12)Xk=fXk−1+Wk,where f⋅ is the discrete-time nonlinear function describing the system state equation and Wk is the discrete-time system state noise.


## 2.2. Measurement Equation

Taking the differences of velocity and position information between the INS and BDS as the system measurement vector,(13)Zk=vEI−vEBvNI−vNBvUI−vUBλI−λBLI−LBhI−hBT,where vEIvNIvUIT and λILIhIT or vEBvNBvUBT and λBLBhBT are the vehicle’s velocity and position information from the INS or BDS.Then, the measurement equation for the INS/BDS integration can be established by the measurement vector(14)Zk=HkXk+Vk,where Hk=HvHp is the measurement matrix, Hv=03×4diag1,1,103×9, Hp=03×7diagRM,RNcosL,103×9, and Vk=VvVp is the measurement noise vector, which obeys the zero-mean Gaussian distribution with covariance R.


## 3. Maximum Likelihood-Based Measurement Noise Covariance Estimation Using Sequential Quadratic Programming for the Cubature Kalman Filter

In this section, a method of maximum likelihood-based measurement noise covariance estimation is developed via the sequential quadratic programming for the nonlinear CKF applied in INS/BDS integration to hinder the influence of inaccurate measurement noise statistics on the navigation solution.


### 3.1. Cubature Kalman Filter

The nonlinear discrete-time system is commonly described as (12) and (14).(15)Xk=fXk−1+Wk,Zk=HkXk+Vk,where Xk represents the system state and the Zk is the system measurement; f⋅ is the nonlinear function describing the system state equation and Hk is the measurement matrix; and Wk and Vk are the process noise and measurement noise, respectively. They are uncorrelated zero-mean Gaussian white noises, and their covariances are EWkWkT=Q and EVkVkT=R.Based on this nonlinear system, the procedure of state estimation using the CKF is given as follows.Step 1.
Initialization: the state estimateX^0 and its error covariance P0 are initially set as(16)X^0=EX0,P0=EX0−X^0X0−X^0T.Step 2.
Cubature point calculation: suppose that the previous time’s state estimateX^k−1 and its error covariance matrix Pk−1 are given. Then, the cubature points are calculated as(17)Pk−1=Sk−1Sk−1T,(18)χi,k−1=X^k−1+Sk−1ξi,where Sk−1=Pk−1; ξi is chosen as(19)ξi=nei,i=1,2,…,n,−nei−n,i=n+1,n+2,…,2n,where ei represents the ith column of the n×n identity matrix.Step 3.
Time update: the selection of cubature points is propagated by the system state equation.(20)χi,k/k−1=fχi,k−1i=1,2,…,2n
Then, the state mean and covariance are predicted as(21)X^k/k−1=12n∑i=12nχi,k/k−1,(22)Pk/k−1=12n∑i=12nχi,k/k−1χi,k/k−1T−X^k/k−1X^k/k−1T+Q.Step 4.
Measurement update: the measurement update is performed same as the Kalman filter due to the linear measurement equation.(23)Z^k/k−1=HkX^k/k−1,(24)PZ^k/k−1=HkPk/k−1HkT+R,(25)PX^k/k−1Z^k/k−1=Pk/k−1HkT,(26)Kk=PX^k/k−1Z^k/k−1PZ^k/k−1−1,(27)X^k=X^k/k−1+KkZk−Z^k/k−1,(28)Pk=Pk/k−1−KkPZ^k/k−1KkT.Step 5.
We repeat Steps2 and 4 for all the samples.
It can be seen that when the measurement noise covarianceR is incorrect, the innovation covariance PZ^k/k−1 will be worsened and further making the filter gain Kk is biased. Thus, the state estimation accuracy will be decreased. Due to this reason, studying a way to estimate the measurement noise covariance and improve the system’s state estimation accuracy is necessary and urgent.


### 3.2. Maximum Likelihood-Based Measurement Noise Covariance Estimation

Assuming that the measurement noise covarianceR is unknown, we denote the R as a parameter. According to the maximum likelihood framework, the estimation model for the parameter R based on the measurement data can be established as [24, 26](29)R^ML=argmaxRlnLR|Z1:k,where LR|Z1:k is the likelihood function of parameter R with respect to the measurement data Z1:k.From the definition of likelihood function, we have(30)LR|Z1:k=pZ1:k|R,where pZ1:k|R represents a suitable conditional probability density function (pdf) defined using the data Z1:k.We denote the innovation of the CKF as(31)Z˜j=Zj−Z^j/j−1=Zj−HjX^j/j−1,j=1,2,…,k.For the nonlinear Gaussian system described by (15), the innovation z˜j obeys the multivariate Gaussian distribution N0,PZ^j/j−1, where(32)PZ^j/j−1=EZ˜jZ˜jT=HjPj/j−1HjT+R.The innovations form of the likelihood function for the parameterR can be written as(33)LR|Z1:k=pZ1:k|R≈∏j=1kpZ˜j|R,=∏j=1k12πn/2PZ^j/j−11/2exp−Z˜jTPZ^j/j−1−1Z˜j2.Taking the logarithm of both sides of (33) and ignoring the constant term, it can be obtained that(34)lnLR|Z1:k≈−12∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j.Substituting (34) into (29), the estimation model of parameter R based on maximum likelihood principle can be achieved.(35)R^ML=argminR∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j.To reduce the computational burden of noise statistic estimation,R is usually taken as a diagonal matrix, i.e.,(36)R=diagr1,r2,…,rm,considering the following condition:(37)R>0.Therefore, the parameter estimation model based on the maximum likelihood principle can be established by combining (35)–(37):(38)R^ML=argminR∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j,R=diagr1,r2,…,rm,R>0.Obviously, the measurement noise covariance estimate has been converted to a typical nonlinear programming problem with inequality constraint. It will be solved by using the sequential quadratic programming method to obtain the estimation of measurement noise covariance.


### 3.3. Solution Method

Sequential quadratic programming (SQP) is one of the most effective algorithms in the world for solving nonlinear constrained programming problems [30, 31]. SQP transforms a complex nonlinear constrained programming problem into a series of quadratic programming subproblems. By solving these quadratic programming subproblems, the optimal solution of the original problem is obtained.Considering the general nonlinear constrained programming problem,(39)mingxs.t.clx=0,l=1,2,…,e,clx≤0,l=e+1,e+2,…,q.In thetth iteration, the constraints in (39) are linearized at the iteration point xt, and the following quadratic programming subproblem is obtained:(40)minGd=12dTWtd+∇gxtTd,s.t.∇clxtTd+clxt=0,l=1,2,…,e,∇clxtTd+clxt≤0,l=e+1,e+2,…,q,where d is the iterative direction; ∇gxt and ∇clxt are the gradients of functions gx and clx at xt; Wt is the positive definite quasi-Newton approximation matrix of Lagrange function Lax,μt=gx+∑l=1eμltclx+∑l=e+1qμltclx at xt.For the nonlinear constrained programming problem described in (39), the computing steps of SQP can be summarized as follows:Step 1: whent=1, we firstly select the initial iteration point x1 and symmetric positive definite matrix B1 are selected, and then, the iteration precision is set ξ>0.Step 2: the active set method [31] is used to solve the quadratic programming subproblem (40) to obtain the optimal solution dt and the corresponding Lagrangian multiplier μt.Ifdt=0, xt+1=xt+dt is the optimal solution of the nonlinear programming problem (39), and the iterative calculation is stopped. Else, we go to Step 3.Step 3: starting from the iteration pointxt, the objective function is constructed along the direction dt.(41)φα=wxt+αdt,λ˜t.Here,(42)λ˜t=λ˜1t,λ˜2t,…,λ˜qt,λ˜lt=μlt,l=1,2,…,q,t=1,λ˜lt=maxμlt,12λ˜lt−1+μlt,l=1,2,…,q,t>1,wx,λ˜=gx+∑l=1eλ˜lclx+∑l=e+1qλ˜lmax0,clx.A one-dimensional search [31] is conducted to φα in (41) to determine the iterative step size αt.Step 4: letxt+1=xt+αtdt. If xt+1−xt<ξ, xt+1 will be the approximate optimal solution for the nonlinear programming problem (39), and the iterative calculation is stopped. Else, we go to Step 5.Step 5: by using the BFGS (Broyden–Fletcher–Goldfarb–Shanno) formulation [31], Wt can be revised as Wt+1:(43)Wt+1=Wt−WtststTWtstTWtst+ηtηtTηtTst.Here,(44)st=xt+1−xt,ηt=ϖet+1−ϖWtst,et=∇Laxt+1,μt−∇Laxt,μt,ϖ=1,etTst≥0.2stTWtst,0.8stTWtststTWtst−etTst,Others.Step 6: lett=t+1, and we return to Step 2.


### 3.4. Proposed Method

For the nonlinear system (15), Figure 2 gives the flowchart of the proposed method, and the core procedure is shown as follows:Figure 2
Flowchart of the proposed method.Step 1: the previous time’s system state estimationX^k−1 and its error covariance Pk−1 are givenStep 2: measurement noise covariance estimation:(i)
According to (31) and (32), the innovation vector and its error covariance matrix from time Step 1 to time step k are obtained(ii)
The estimation model (38) for measurement noise covariance based on the maximum likelihood principle is constructed(iii)
The SQP method is used to iteratively solve the (38) to obtain the estimated measurement noise covariance R^Step 3: time update: CKF steps (Equations (17)–(22)) are performed to calculate the state prediction X^k/k−1 and its error covariance matrix Pk/k−1Step 4: Measurement update: we perform CKF steps (Equations (23)–(28)) to update the system state estimation and its error covariance matrix, where (24) is replaced by(45)PZ^k/k−1=HkPk/k−1HkT+R^Step 5: we repeat Steps 1–4 for all the samples


## 3.1. Cubature Kalman Filter

The nonlinear discrete-time system is commonly described as (12) and (14).(15)Xk=fXk−1+Wk,Zk=HkXk+Vk,where Xk represents the system state and the Zk is the system measurement; f⋅ is the nonlinear function describing the system state equation and Hk is the measurement matrix; and Wk and Vk are the process noise and measurement noise, respectively. They are uncorrelated zero-mean Gaussian white noises, and their covariances are EWkWkT=Q and EVkVkT=R.Based on this nonlinear system, the procedure of state estimation using the CKF is given as follows.Step 1.
Initialization: the state estimateX^0 and its error covariance P0 are initially set as(16)X^0=EX0,P0=EX0−X^0X0−X^0T.Step 2.
Cubature point calculation: suppose that the previous time’s state estimateX^k−1 and its error covariance matrix Pk−1 are given. Then, the cubature points are calculated as(17)Pk−1=Sk−1Sk−1T,(18)χi,k−1=X^k−1+Sk−1ξi,where Sk−1=Pk−1; ξi is chosen as(19)ξi=nei,i=1,2,…,n,−nei−n,i=n+1,n+2,…,2n,where ei represents the ith column of the n×n identity matrix.Step 3.
Time update: the selection of cubature points is propagated by the system state equation.(20)χi,k/k−1=fχi,k−1i=1,2,…,2n
Then, the state mean and covariance are predicted as(21)X^k/k−1=12n∑i=12nχi,k/k−1,(22)Pk/k−1=12n∑i=12nχi,k/k−1χi,k/k−1T−X^k/k−1X^k/k−1T+Q.Step 4.
Measurement update: the measurement update is performed same as the Kalman filter due to the linear measurement equation.(23)Z^k/k−1=HkX^k/k−1,(24)PZ^k/k−1=HkPk/k−1HkT+R,(25)PX^k/k−1Z^k/k−1=Pk/k−1HkT,(26)Kk=PX^k/k−1Z^k/k−1PZ^k/k−1−1,(27)X^k=X^k/k−1+KkZk−Z^k/k−1,(28)Pk=Pk/k−1−KkPZ^k/k−1KkT.Step 5.
We repeat Steps2 and 4 for all the samples.
It can be seen that when the measurement noise covarianceR is incorrect, the innovation covariance PZ^k/k−1 will be worsened and further making the filter gain Kk is biased. Thus, the state estimation accuracy will be decreased. Due to this reason, studying a way to estimate the measurement noise covariance and improve the system’s state estimation accuracy is necessary and urgent.


## 3.2. Maximum Likelihood-Based Measurement Noise Covariance Estimation

Assuming that the measurement noise covarianceR is unknown, we denote the R as a parameter. According to the maximum likelihood framework, the estimation model for the parameter R based on the measurement data can be established as [24, 26](29)R^ML=argmaxRlnLR|Z1:k,where LR|Z1:k is the likelihood function of parameter R with respect to the measurement data Z1:k.From the definition of likelihood function, we have(30)LR|Z1:k=pZ1:k|R,where pZ1:k|R represents a suitable conditional probability density function (pdf) defined using the data Z1:k.We denote the innovation of the CKF as(31)Z˜j=Zj−Z^j/j−1=Zj−HjX^j/j−1,j=1,2,…,k.For the nonlinear Gaussian system described by (15), the innovation z˜j obeys the multivariate Gaussian distribution N0,PZ^j/j−1, where(32)PZ^j/j−1=EZ˜jZ˜jT=HjPj/j−1HjT+R.The innovations form of the likelihood function for the parameterR can be written as(33)LR|Z1:k=pZ1:k|R≈∏j=1kpZ˜j|R,=∏j=1k12πn/2PZ^j/j−11/2exp−Z˜jTPZ^j/j−1−1Z˜j2.Taking the logarithm of both sides of (33) and ignoring the constant term, it can be obtained that(34)lnLR|Z1:k≈−12∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j.Substituting (34) into (29), the estimation model of parameter R based on maximum likelihood principle can be achieved.(35)R^ML=argminR∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j.To reduce the computational burden of noise statistic estimation,R is usually taken as a diagonal matrix, i.e.,(36)R=diagr1,r2,…,rm,considering the following condition:(37)R>0.Therefore, the parameter estimation model based on the maximum likelihood principle can be established by combining (35)–(37):(38)R^ML=argminR∑j=1klnPZ^j/j−1+Z˜jTPZ^j/j−1−1Z˜j,R=diagr1,r2,…,rm,R>0.Obviously, the measurement noise covariance estimate has been converted to a typical nonlinear programming problem with inequality constraint. It will be solved by using the sequential quadratic programming method to obtain the estimation of measurement noise covariance.


## 3.3. Solution Method

Sequential quadratic programming (SQP) is one of the most effective algorithms in the world for solving nonlinear constrained programming problems [30, 31]. SQP transforms a complex nonlinear constrained programming problem into a series of quadratic programming subproblems. By solving these quadratic programming subproblems, the optimal solution of the original problem is obtained.Considering the general nonlinear constrained programming problem,(39)mingxs.t.clx=0,l=1,2,…,e,clx≤0,l=e+1,e+2,…,q.In thetth iteration, the constraints in (39) are linearized at the iteration point xt, and the following quadratic programming subproblem is obtained:(40)minGd=12dTWtd+∇gxtTd,s.t.∇clxtTd+clxt=0,l=1,2,…,e,∇clxtTd+clxt≤0,l=e+1,e+2,…,q,where d is the iterative direction; ∇gxt and ∇clxt are the gradients of functions gx and clx at xt; Wt is the positive definite quasi-Newton approximation matrix of Lagrange function Lax,μt=gx+∑l=1eμltclx+∑l=e+1qμltclx at xt.For the nonlinear constrained programming problem described in (39), the computing steps of SQP can be summarized as follows:Step 1: whent=1, we firstly select the initial iteration point x1 and symmetric positive definite matrix B1 are selected, and then, the iteration precision is set ξ>0.Step 2: the active set method [31] is used to solve the quadratic programming subproblem (40) to obtain the optimal solution dt and the corresponding Lagrangian multiplier μt.Ifdt=0, xt+1=xt+dt is the optimal solution of the nonlinear programming problem (39), and the iterative calculation is stopped. Else, we go to Step 3.Step 3: starting from the iteration pointxt, the objective function is constructed along the direction dt.(41)φα=wxt+αdt,λ˜t.Here,(42)λ˜t=λ˜1t,λ˜2t,…,λ˜qt,λ˜lt=μlt,l=1,2,…,q,t=1,λ˜lt=maxμlt,12λ˜lt−1+μlt,l=1,2,…,q,t>1,wx,λ˜=gx+∑l=1eλ˜lclx+∑l=e+1qλ˜lmax0,clx.A one-dimensional search [31] is conducted to φα in (41) to determine the iterative step size αt.Step 4: letxt+1=xt+αtdt. If xt+1−xt<ξ, xt+1 will be the approximate optimal solution for the nonlinear programming problem (39), and the iterative calculation is stopped. Else, we go to Step 5.Step 5: by using the BFGS (Broyden–Fletcher–Goldfarb–Shanno) formulation [31], Wt can be revised as Wt+1:(43)Wt+1=Wt−WtststTWtstTWtst+ηtηtTηtTst.Here,(44)st=xt+1−xt,ηt=ϖet+1−ϖWtst,et=∇Laxt+1,μt−∇Laxt,μt,ϖ=1,etTst≥0.2stTWtst,0.8stTWtststTWtst−etTst,Others.Step 6: lett=t+1, and we return to Step 2.


## 3.4. Proposed Method

For the nonlinear system (15), Figure 2 gives the flowchart of the proposed method, and the core procedure is shown as follows:Figure 2
Flowchart of the proposed method.Step 1: the previous time’s system state estimationX^k−1 and its error covariance Pk−1 are givenStep 2: measurement noise covariance estimation:(i)
According to (31) and (32), the innovation vector and its error covariance matrix from time Step 1 to time step k are obtained(ii)
The estimation model (38) for measurement noise covariance based on the maximum likelihood principle is constructed(iii)
The SQP method is used to iteratively solve the (38) to obtain the estimated measurement noise covariance R^Step 3: time update: CKF steps (Equations (17)–(22)) are performed to calculate the state prediction X^k/k−1 and its error covariance matrix Pk/k−1Step 4: Measurement update: we perform CKF steps (Equations (23)–(28)) to update the system state estimation and its error covariance matrix, where (24) is replaced by(45)PZ^k/k−1=HkPk/k−1HkT+R^Step 5: we repeat Steps 1–4 for all the samples


## 4. Simulation Analysis and Discussions

Simulations have been conducted to comprehensively evaluate the performance of the proposed CKF, which is abbreviated as the MLCKF (Maximum Likelihood-based Cubature Kalman Filter), for an unmanned aerial vehicle (UAV) navigation using INS/BDS integration in terms of inaccurate measurement noise statistics. The UAV and its main constitute parameters can been seen in the Figure3. Comparison analysis of the proposed MLCKF with CKF, H-infinity strategy-based robust CKF (HRCKF), and MCSCKF is also discussed in this section.Figure 3
UAV and its main constitute parameters.A flight trajectory is designed for the UAV dynamic flight considering the complex maneuvers such as climbing, descending, turning, uniform linear motion, and accelerated linear motion. It can be seen in Figure4. The initial position for the UAV is 108.997°, 34.246°, and 1000 m in longitude, latitude, and altitude components; the initial velocity is 0 m/s, 150 m/s, and 0 m/s in east, north, and up components; the initial attitude is 0°, 0°, and 0° in pitch, roll, and yaw components; and the simulation time is 1000 s. The parameters for the navigation sensors are as follows: the constant drift and random walk coefficient of the gyro are 0.1°/h and 0.05 ∘/h; the zero-bias and random walk coefficient of the accelerometer are 1 × 10−3 g and 1×10−4g⋅s; the BDS’s accuracy in horizontal position, altitude and velocity are 5 m, 8 m, and 0.05 m/s in the sense of root mean square error, respectively; and the sampling rates for the INS and BDS are 25 Hz and 1 Hz. For the navigation filter, the initial position error is set as 8 m, 8 m, and 15 m in longitude, latitude, and altitude components; the initial velocity error is 0.5 m/s, 0.5 m/s, and 0.5 m/s in east, north, and up components; the initial attitude error is ε, 1′, and 1.5′ in pitch, roll, and yaw components; and the filter period is set as 1 s.Figure 4
Flight trajectory of the UAV.According to the chosen of simulation parameter, the measurement noise covariance for the INS/BDS integration should be(46)R=diag0.05m/s2I3×3,5m2,5m2,8m2.In the simulation, to evaluate the performance of proposed MLCKF in terms of inaccurate measurement noise covariance, two cases are considered: (i) Gaussian noise with inaccurate covariance; (ii) non-Gaussian noise.


### 4.1. Gaussian Noise with Inaccurate Covariance

In this case, the measurement noise covarianceε is enlarged to 16×R during the time interval (500 s, 700 s), i.e.,(47)R¯=diag0.2m/s2I3×3,20m2,20m2,32m2.Thus, the actual measurement noise covariance used for the filters can be written as(48)Rk=R,Others,16×R,k∈500s,700s.


#### 4.1.1. Measurement Noise Covariance Estimation Evaluation

In this section, the measurement noise covariance estimation of the proposed MLCKF will be evaluated by comparing with the method using the genetic algorithm [32] to solve the estimation model (38).Figure5 gives the estimated measurement noise covariance by the proposed MLCKF using SQP for the time interval (500 s, 700 s). It can be seen that the estimated measurement noise covariance by the MLCKF can converge to its actual value at a fast speed. When the proposed MLCKF tends to be stable, the estimation error for measurement noise covariance in velocity is within 0.0052 (m/s)2, and the estimation error for measurement noise covariance in position is within 7.41 m2. In contrast, Figure 6 provides the estimated measurement noise covariance by the method using the genetic algorithm. It can be seen that its convergence speed is significantly slower than that of the proposed MLCKF using SQP, and its estimation for measurement noise covariance is also larger than that of the proposed MLCKF using SQP when it tends to be stable. This is because the convergence of the genetic algorithm cannot be guaranteed in theory and the premature convergence may be occurred [32]. Table 1 lists the mean estimations for the measurement noise covariance during the time interval (500 s, 700 s). By comparing the mean estimations with their actual value in Table 1, the ability of the proposed MLCKF to adaptively estimate measurement noise covariance has been verified. The abovementioned analysis shows that the proposed MLCKF can effectively estimate the measurement noise covariance and provide accurate measurement noise statistics information for the filter when the measurement noise statistics is inaccurate.Figure 5
Estimation of the measurement noise covariance using SQP. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Figure 6
Estimation of the measurement noise covariance using genetic algorithm. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Table 1
The actual value, initial value, and mean estimation by the proposed method using SQP and genetic algorithm for the measurement noise covariance.
Measurement noise covarianceΔxN (500 s, 700 s)Actual valuediag0.05m/s2I3×3,5m2,5m2,8m2Initial valuediag0.2m/s2I3×3,20m2,20m2,32m2Mean estimation by SQPdiag0.0031m/s2,0.0032m/s2,0.0036m/s2,24.7801m2,25.0178m2,64.8577m2Mean estimation by genetic algorithmdiag0.0054m/s2,0.0056m/s2,0.0053m/s2,27.2759m2,27.9192m2,68.8187m2


#### 4.1.2. Navigation Accuracy Evaluation

Under the same simulation conditions, the CKF, HRCKF, and MLCKF are, respectively, used as the navigation filter, and the overall errors of attitude, velocity, and position for the UAV by the abovementioned three methods are compared. The overall error is defined by [10](49)Δx=ΔxE2+ΔxN2+ΔxU2,where ΔxE, ΔxN, and ΔxU are the error components of x in east, north, and up, respectively.Figures7–9 depict the overall attitude errors, overall velocity errors, and overall position errors by the CKF, HRCKF, and proposed MLCKF for the UAV navigation. Analyzing Figures 7–9, we can achieve the following conclusions:(i)
When the measurement noise statistics is accurately known, the CKF, HRCKF, and MLCKF can converge rapidly to estimate the UAV attitude, velocity, and position with a quite high accuracy.(ii)
During the time interval (500 s, 700 s), both the CKF and HRCKF are affected by the inaccurate initial measurement noise covariance, and the attitude, velocity, and position estimation errors all increase obviously. The attitude, velocity, and position estimation errors of UAV are around 0.82', 0.53 m/s, and 19.17 m for the CKF and 0.71′, 0.41 m/s, and 16.31 m for the HRCKF, respectively. The navigation accuracy of the HRCKF is relatively superior to that of the CKF due to its ability to curb inaccurate measurement noise covariance. Compared to the abovementioned two methods, the attitude, velocity, and position estimation errors of UAV for the proposed MLCKF are around 0.55′, 0.29 m/s, and 13.29 m, respectively, which are much smaller than the CKF and HRCKF. This is because the MLCKF can accurately estimate the measurement noise covariance by using the maximum likelihood principle. Thus, the MLCKF can achieve a higher navigation accuracy compared to the other two methods via the estimation of measurement noise covariance for the case of inaccurate measurement noise statistics.Figure 7
Attitude errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 8
Velocity errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 9
Position errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figures10–12 describe the intuitive comparison of the CKF, HRCKF, and MLCKF in terms of mean estimation errors of attitude, velocity, and position for the UAV during the time interval (500 s, 700 s), which also validate the abovementioned conclusions.Figure 10
Mean attitude errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 11
Mean velocity errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 12
Mean position errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.


### 4.2. Non-Gaussian Noise

The nominal Gaussian distribution of the measurement noise is perturbed by another distribution; i.e., the actual probability density function is(50)ρactual=1−ερnominal+ερperturbing,where ε represents the ratio of the perturbing distribution. If ρperturbing also obeys a Gaussian distribution but with a larger standard deviation, the actual distribution is called the Gaussian mixture, which is a non-Gaussian noise. In this study, the standard deviation of the perturbing distribution is assumed to be 8 times larger than that of the nominal distribution, and ε is assumed to be the value of 0.2.Figures13–15 describe the overall attitude errors, overall velocity errors, and overall position errors by the CKF, MCSCKF, and proposed MLCKF for the UAV navigation. The CKF’s attitude, velocity, and position estimation error of UAV are around 1.21′, 0.67 m/s, and 20.65 m. The influence of the non-Gaussian noise is quite serious for the CKF. The MCSCKF can deal with the non-Gaussian noise to some extent. Its attitude, velocity, and position estimation error of UAV are around 1.02′, 0.59 m/s, and 16.97 m, respectively. However, the construction of the estimation error covariance matrix in this method is not based on theoretical analysis, leading to limited improvement for hindering the non-Gaussian noise. The proposed MLCKF also can curb the non-Gaussian noise to some extent through the covariance estimation, leading to the attitude, velocity, and position estimation error of UAV around 0.92′, 0.53 m/s, and 15.66 m. Figures 16–18 also verify the abovementioned results by the intuitive comparison of the CKF, MCSCKF, and MLCKF in terms of mean estimation errors of attitude, velocity, and position for the UAV.Figure 13
Attitude errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 14
Velocity errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 15
Position errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 16
Mean attitude errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 17
Mean velocity errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 18
Mean position errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.The abovementioned simulations and analysis for the INS/BDS integration show that the proposed MLCKF can significantly improve the adaptability ability for the filter and effectively restrain the influence of inaccurate measurement noise covariance on navigation solution, leading to a higher navigation accuracy than the CKF, HRCKF, and MCSCKF for the UAV navigation with INS/BDS integration.


## 4.1. Gaussian Noise with Inaccurate Covariance

In this case, the measurement noise covarianceε is enlarged to 16×R during the time interval (500 s, 700 s), i.e.,(47)R¯=diag0.2m/s2I3×3,20m2,20m2,32m2.Thus, the actual measurement noise covariance used for the filters can be written as(48)Rk=R,Others,16×R,k∈500s,700s.


### 4.1.1. Measurement Noise Covariance Estimation Evaluation

In this section, the measurement noise covariance estimation of the proposed MLCKF will be evaluated by comparing with the method using the genetic algorithm [32] to solve the estimation model (38).Figure5 gives the estimated measurement noise covariance by the proposed MLCKF using SQP for the time interval (500 s, 700 s). It can be seen that the estimated measurement noise covariance by the MLCKF can converge to its actual value at a fast speed. When the proposed MLCKF tends to be stable, the estimation error for measurement noise covariance in velocity is within 0.0052 (m/s)2, and the estimation error for measurement noise covariance in position is within 7.41 m2. In contrast, Figure 6 provides the estimated measurement noise covariance by the method using the genetic algorithm. It can be seen that its convergence speed is significantly slower than that of the proposed MLCKF using SQP, and its estimation for measurement noise covariance is also larger than that of the proposed MLCKF using SQP when it tends to be stable. This is because the convergence of the genetic algorithm cannot be guaranteed in theory and the premature convergence may be occurred [32]. Table 1 lists the mean estimations for the measurement noise covariance during the time interval (500 s, 700 s). By comparing the mean estimations with their actual value in Table 1, the ability of the proposed MLCKF to adaptively estimate measurement noise covariance has been verified. The abovementioned analysis shows that the proposed MLCKF can effectively estimate the measurement noise covariance and provide accurate measurement noise statistics information for the filter when the measurement noise statistics is inaccurate.Figure 5
Estimation of the measurement noise covariance using SQP. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Figure 6
Estimation of the measurement noise covariance using genetic algorithm. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Table 1
The actual value, initial value, and mean estimation by the proposed method using SQP and genetic algorithm for the measurement noise covariance.
Measurement noise covarianceΔxN (500 s, 700 s)Actual valuediag0.05m/s2I3×3,5m2,5m2,8m2Initial valuediag0.2m/s2I3×3,20m2,20m2,32m2Mean estimation by SQPdiag0.0031m/s2,0.0032m/s2,0.0036m/s2,24.7801m2,25.0178m2,64.8577m2Mean estimation by genetic algorithmdiag0.0054m/s2,0.0056m/s2,0.0053m/s2,27.2759m2,27.9192m2,68.8187m2


### 4.1.2. Navigation Accuracy Evaluation

Under the same simulation conditions, the CKF, HRCKF, and MLCKF are, respectively, used as the navigation filter, and the overall errors of attitude, velocity, and position for the UAV by the abovementioned three methods are compared. The overall error is defined by [10](49)Δx=ΔxE2+ΔxN2+ΔxU2,where ΔxE, ΔxN, and ΔxU are the error components of x in east, north, and up, respectively.Figures7–9 depict the overall attitude errors, overall velocity errors, and overall position errors by the CKF, HRCKF, and proposed MLCKF for the UAV navigation. Analyzing Figures 7–9, we can achieve the following conclusions:(i)
When the measurement noise statistics is accurately known, the CKF, HRCKF, and MLCKF can converge rapidly to estimate the UAV attitude, velocity, and position with a quite high accuracy.(ii)
During the time interval (500 s, 700 s), both the CKF and HRCKF are affected by the inaccurate initial measurement noise covariance, and the attitude, velocity, and position estimation errors all increase obviously. The attitude, velocity, and position estimation errors of UAV are around 0.82', 0.53 m/s, and 19.17 m for the CKF and 0.71′, 0.41 m/s, and 16.31 m for the HRCKF, respectively. The navigation accuracy of the HRCKF is relatively superior to that of the CKF due to its ability to curb inaccurate measurement noise covariance. Compared to the abovementioned two methods, the attitude, velocity, and position estimation errors of UAV for the proposed MLCKF are around 0.55′, 0.29 m/s, and 13.29 m, respectively, which are much smaller than the CKF and HRCKF. This is because the MLCKF can accurately estimate the measurement noise covariance by using the maximum likelihood principle. Thus, the MLCKF can achieve a higher navigation accuracy compared to the other two methods via the estimation of measurement noise covariance for the case of inaccurate measurement noise statistics.Figure 7
Attitude errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 8
Velocity errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 9
Position errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figures10–12 describe the intuitive comparison of the CKF, HRCKF, and MLCKF in terms of mean estimation errors of attitude, velocity, and position for the UAV during the time interval (500 s, 700 s), which also validate the abovementioned conclusions.Figure 10
Mean attitude errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 11
Mean velocity errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 12
Mean position errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.


## 4.1.1. Measurement Noise Covariance Estimation Evaluation

In this section, the measurement noise covariance estimation of the proposed MLCKF will be evaluated by comparing with the method using the genetic algorithm [32] to solve the estimation model (38).Figure5 gives the estimated measurement noise covariance by the proposed MLCKF using SQP for the time interval (500 s, 700 s). It can be seen that the estimated measurement noise covariance by the MLCKF can converge to its actual value at a fast speed. When the proposed MLCKF tends to be stable, the estimation error for measurement noise covariance in velocity is within 0.0052 (m/s)2, and the estimation error for measurement noise covariance in position is within 7.41 m2. In contrast, Figure 6 provides the estimated measurement noise covariance by the method using the genetic algorithm. It can be seen that its convergence speed is significantly slower than that of the proposed MLCKF using SQP, and its estimation for measurement noise covariance is also larger than that of the proposed MLCKF using SQP when it tends to be stable. This is because the convergence of the genetic algorithm cannot be guaranteed in theory and the premature convergence may be occurred [32]. Table 1 lists the mean estimations for the measurement noise covariance during the time interval (500 s, 700 s). By comparing the mean estimations with their actual value in Table 1, the ability of the proposed MLCKF to adaptively estimate measurement noise covariance has been verified. The abovementioned analysis shows that the proposed MLCKF can effectively estimate the measurement noise covariance and provide accurate measurement noise statistics information for the filter when the measurement noise statistics is inaccurate.Figure 5
Estimation of the measurement noise covariance using SQP. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Figure 6
Estimation of the measurement noise covariance using genetic algorithm. (a) Estimation of the measurement noise covariance in velocity. (b) Estimation of the measurement noise covariance in position.
(a)(b)Table 1
The actual value, initial value, and mean estimation by the proposed method using SQP and genetic algorithm for the measurement noise covariance.
Measurement noise covarianceΔxN (500 s, 700 s)Actual valuediag0.05m/s2I3×3,5m2,5m2,8m2Initial valuediag0.2m/s2I3×3,20m2,20m2,32m2Mean estimation by SQPdiag0.0031m/s2,0.0032m/s2,0.0036m/s2,24.7801m2,25.0178m2,64.8577m2Mean estimation by genetic algorithmdiag0.0054m/s2,0.0056m/s2,0.0053m/s2,27.2759m2,27.9192m2,68.8187m2


## 4.1.2. Navigation Accuracy Evaluation

Under the same simulation conditions, the CKF, HRCKF, and MLCKF are, respectively, used as the navigation filter, and the overall errors of attitude, velocity, and position for the UAV by the abovementioned three methods are compared. The overall error is defined by [10](49)Δx=ΔxE2+ΔxN2+ΔxU2,where ΔxE, ΔxN, and ΔxU are the error components of x in east, north, and up, respectively.Figures7–9 depict the overall attitude errors, overall velocity errors, and overall position errors by the CKF, HRCKF, and proposed MLCKF for the UAV navigation. Analyzing Figures 7–9, we can achieve the following conclusions:(i)
When the measurement noise statistics is accurately known, the CKF, HRCKF, and MLCKF can converge rapidly to estimate the UAV attitude, velocity, and position with a quite high accuracy.(ii)
During the time interval (500 s, 700 s), both the CKF and HRCKF are affected by the inaccurate initial measurement noise covariance, and the attitude, velocity, and position estimation errors all increase obviously. The attitude, velocity, and position estimation errors of UAV are around 0.82', 0.53 m/s, and 19.17 m for the CKF and 0.71′, 0.41 m/s, and 16.31 m for the HRCKF, respectively. The navigation accuracy of the HRCKF is relatively superior to that of the CKF due to its ability to curb inaccurate measurement noise covariance. Compared to the abovementioned two methods, the attitude, velocity, and position estimation errors of UAV for the proposed MLCKF are around 0.55′, 0.29 m/s, and 13.29 m, respectively, which are much smaller than the CKF and HRCKF. This is because the MLCKF can accurately estimate the measurement noise covariance by using the maximum likelihood principle. Thus, the MLCKF can achieve a higher navigation accuracy compared to the other two methods via the estimation of measurement noise covariance for the case of inaccurate measurement noise statistics.Figure 7
Attitude errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 8
Velocity errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figure 9
Position errors of the UAV by the CKF, HRCKF, and MLCKF for the case of Gaussian noise with inaccurate covariance.Figures10–12 describe the intuitive comparison of the CKF, HRCKF, and MLCKF in terms of mean estimation errors of attitude, velocity, and position for the UAV during the time interval (500 s, 700 s), which also validate the abovementioned conclusions.Figure 10
Mean attitude errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 11
Mean velocity errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.Figure 12
Mean position errors of the UAV by the CKF, HRCKF, and MLCKF in the time interval (500 s, 700 s) for the case of Gaussian noise with inaccurate covariance.


## 4.2. Non-Gaussian Noise

The nominal Gaussian distribution of the measurement noise is perturbed by another distribution; i.e., the actual probability density function is(50)ρactual=1−ερnominal+ερperturbing,where ε represents the ratio of the perturbing distribution. If ρperturbing also obeys a Gaussian distribution but with a larger standard deviation, the actual distribution is called the Gaussian mixture, which is a non-Gaussian noise. In this study, the standard deviation of the perturbing distribution is assumed to be 8 times larger than that of the nominal distribution, and ε is assumed to be the value of 0.2.Figures13–15 describe the overall attitude errors, overall velocity errors, and overall position errors by the CKF, MCSCKF, and proposed MLCKF for the UAV navigation. The CKF’s attitude, velocity, and position estimation error of UAV are around 1.21′, 0.67 m/s, and 20.65 m. The influence of the non-Gaussian noise is quite serious for the CKF. The MCSCKF can deal with the non-Gaussian noise to some extent. Its attitude, velocity, and position estimation error of UAV are around 1.02′, 0.59 m/s, and 16.97 m, respectively. However, the construction of the estimation error covariance matrix in this method is not based on theoretical analysis, leading to limited improvement for hindering the non-Gaussian noise. The proposed MLCKF also can curb the non-Gaussian noise to some extent through the covariance estimation, leading to the attitude, velocity, and position estimation error of UAV around 0.92′, 0.53 m/s, and 15.66 m. Figures 16–18 also verify the abovementioned results by the intuitive comparison of the CKF, MCSCKF, and MLCKF in terms of mean estimation errors of attitude, velocity, and position for the UAV.Figure 13
Attitude errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 14
Velocity errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 15
Position errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 16
Mean attitude errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 17
Mean velocity errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.Figure 18
Mean position errors of the UAV by the CKF, MCSCKF, and MLCKF for the case of non-Gaussian noise.The abovementioned simulations and analysis for the INS/BDS integration show that the proposed MLCKF can significantly improve the adaptability ability for the filter and effectively restrain the influence of inaccurate measurement noise covariance on navigation solution, leading to a higher navigation accuracy than the CKF, HRCKF, and MCSCKF for the UAV navigation with INS/BDS integration.


## 5. Conclusions

This paper presents a method of the CKF with measurement noise covariance estimation for the INS/BDS integration. The contributions of this paper are as follows: (i) an estimation model for measurement noise covariance is established according to the maximum likelihood principle, and the sequential quadratic programming is applied to calculate the estimation of measurement noise covariance; (ii) the estimated measurement noise covariance will be fed back to the procedure of the CKF to improve its adaptability. Simulation and comparison analysis demonstrate that the proposed method can accurately estimate measurement noise covariance to effectively restrain its influence on navigation solution, leading to higher navigation accuracy than the CKF, HRCKF, and MCSCKF for the INS/BDS integration in presence of inaccurate measurement noise covariance.



---

*Source: 9383678-2021-01-16.xml*