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

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.


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]. e 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 shortterm 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][9][10].
e 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]. e 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]. e 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]. e unscented Kalman Filter (UKF) has received increased attention for INS/GNSS integration due to the ability of dealing with strongly nonlinear systems [12][13][14].
is 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][17][18]. Furthermore, the CKF only contains 2n cubature points, which is smaller than the UKF with 2n + 1 sigma points. us, 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. e 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]. e 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. e maximum likelihood principle can achieve the unique and converged solution in the probabilistic sense [25,26].
us, 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.
is 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. e 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.

System Model for INS/BDS Integration
e 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. us, the system model for INS/BDS integration is developed and described in the following.

System State Equation.
We denote the navigation frame (n-frame) as the E-N-U (East-North-Up) geography frame (g-frame). e 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] δ _ Q where δQ n b � δq 0 δq 1 δq 2 δq 3 T and Q n b � q 0 q 1 q 2 q 3 are the attitude error quaternion and attitude quaternion; 2 Mathematical Problems in Engineering T denotes the body angular rate measured by three gyros in the b-frame; δω b ib is the gyro error; ω n in � ω n ie + ω n en � ω n inx ω n iny ω n inz T represents the angular rate of the n-frame with respect to the i-frame and expressed in the n-frame; δω n in � δω n ie + δω n en is the corresponding error; the meanings of δω n ie , δω n en , δω n ie , and δω n en are given in (3); and the parameters e INS's velocity error equation is [27] δ is the velocity error and T is the velocity in east, north, and up; δC n b is an attitude error coupling term, which has strongly nonlinear characteristics about δQ n b as described in [28] T is the angular rate of the n-frame with respect to the e-frame; where R M and R N denote the meridian and transverse radii of curvature and L and h represent the latitude and altitude of the vehicle; and δω n ie and δω n en are the corresponding errors.
e INS's position error is described as [28] δ _ P n � MδV n + NδP n , where δP n � δL δλ δh T is the position error in latitude, longitude, and altitude. M and N can be formulated as e IMU error includes the errors of the gyro and accelerometer. e gyro error and accelerometer error both can be described as where represent the constant drift of the gyro and the zero bias of the accelerometer; w g � w gx w gy w gz T and w a � w ax w ay w az T are the white noises for the gyro and accelerometer. e system state variable is where f(·) is the continuous-time nonlinear function describing the system state equation and W(t) is the system state noise.
To use the discrete-time CKF, (11) should be discretized via the improved Euler method [29].
where f(·) is the discrete-time nonlinear function describing the system state equation and W k is the discretetime system state noise.

Measurement Equation.
Taking the differences of velocity and position information between the INS and BDS as the system measurement vector, where where is the measurement noise vector, which obeys the zero-mean Gaussian distribution with covariance R.

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.
where X k represents the system state and the Z k is the system measurement; f(·) is the nonlinear function describing the system state equation and H k is the measurement matrix; and W k and V k are the process noise and measurement noise, respectively. ey are uncorrelated zero-mean Gaussian white noises, and their covariances are Based on this nonlinear system, the procedure of state estimation using the CKF is given as follows.
Step 1. Initialization: the state estimate X 0 and its error covariance P 0 are initially set as Step 2. Cubature point calculation: suppose that the previous time's state estimate X k− 1 and its error covariance matrix P k− 1 are given. en, the cubature points are calculated as where e i 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.
en, the state mean and covariance are predicted as Step 4. Measurement update: the measurement update is performed same as the Kalman filter due to the linear measurement equation.
Step 5. We repeat Steps 2 and 4 for all the samples. It can be seen that when the measurement noise covariance R is incorrect, the innovation covariance P Z k/k− 1 will be worsened and further making the filter gain K k is biased. us, 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.

Maximum Likelihood-Based Measurement Noise Covariance Estimation.
Assuming that the measurement noise covariance R 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] where L(R|Z 1: k ) is the likelihood function of parameter R with respect to the measurement data Z 1: k . From the definition of likelihood function, we have where p(Z 1: k |R) represents a suitable conditional probability density function (pdf ) defined using the data Z 1: k . We denote the innovation of the CKF as For the nonlinear Gaussian system described by (15), the innovation z j obeys the multivariate Gaussian distribution e innovations form of the likelihood function for the parameter R can be written as Taking the logarithm of both sides of (33) and ignoring the constant term, it can be obtained that Substituting (34) into (29), the estimation model of parameter R based on maximum likelihood principle can be achieved.
To reduce the computational burden of noise statistic estimation, R is usually taken as a diagonal matrix, i.e., considering the following condition: erefore, the parameter estimation model based on the maximum likelihood principle can be established by combining (35)-(37): Obviously, the measurement noise covariance estimate has been converted to a typical nonlinear programming problem with inequality constraint. It will be solved by using Mathematical Problems in Engineering the sequential quadratic programming method to obtain the estimation of measurement noise covariance.

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.

(39)
In the tth iteration, the constraints in (39) are linearized at the iteration point x (t) , and the following quadratic programming subproblem is obtained: where d is the iterative direction; ∇g(x (t) ) and ∇c l (x (t) ) are the gradients of functions g(x) and c l (x) at x (t) ; W t is the positive definite quasi-Newton approximation matrix of Lagrange function La(x, μ (t) ) � g(x) + e l�1 μ (t) l c l (x)+ q l�e+1 μ (t) l c l (x) at x (t) . For the nonlinear constrained programming problem described in (39), the computing steps of SQP can be summarized as follows: Step 1: when t � 1, we firstly select the initial iteration point x (1) and symmetric positive definite matrix B 1 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 d (t) and the corresponding Lagrangian multiplier μ (t) .
If d (t) � 0, x (t+1) � x (t) + d (t) 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 point x (t) , the objective function is constructed along the direction d (t) . Here, A one-dimensional search [31] is conducted to φ(α) in (41) to determine the iterative step size α t .
Step 6: let t � t + 1, and we return to Step 2.

Proposed
Method. For the nonlinear system (15), Figure 2 gives the flowchart of the proposed method, and the core procedure is shown as follows: Step 1: the previous time's system state estimation X k− 1 and its error covariance P k− 1 are given Step 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) e estimation model (38) for measurement noise covariance based on the maximum likelihood principle is constructed (iii) e 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 P k/k− 1 Step 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 Step 5: we repeat Steps 1-4 for all the samples

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. e UAV and its main constitute parameters can been seen in the Figure 3. Comparison analysis of the proposed MLCKF with CKF, H-infinity strategy-based robust CKF (HRCKF), and MCSCKF is also discussed in this section. 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 Figure 4. e 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.

Mathematical Problems in Engineering
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.

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) us, the actual measurement noise covariance used for the filters can be written as 16 × R, k ∈ (500 s, 700 s).

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). Figure 5 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 m 2 . 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. is 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. e 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.

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. e overall error is defined by [10] ‖Δx‖ � where Δx E , Δx N , and Δx U are the error components of x in east, north, and up, respectively. Figures 7-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. e 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. e 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. is is because the MLCKF can accurately estimate the measurement noise covariance by using the maximum likelihood principle. us, 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.

Non-Gaussian
Noise. e nominal Gaussian distribution of the measurement noise is perturbed by another distribution; i.e., the actual probability density function is 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. Figures 13-15 describe the overall attitude errors, overall velocity errors, and overall position errors by the CKF, MCSCKF, and proposed MLCKF for the UAV navigation. e CKF's attitude, velocity, and position estimation error of UAV are around 1.21′, 0.67 m/s, and 20.65 m. e influence of the non-Gaussian noise is quite serious for the CKF. e 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. e proposed MLCKF also can curb the non-Gaussian noise to some extent through the covariance estimation, leading to the attitude, velocity, and position    e 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.

Conclusions
is paper presents a method of the CKF with measurement noise covariance estimation for the INS/BDS integration. e 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.
Data Availability e processed data required to reproduce these findings cannot be shared at this time as the data also form part of an ongoing study.

Conflicts of Interest
e authors declare that they have no conflicts of interest.