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.
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 (-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]where and are the attitude error quaternion and attitude quaternion; denotes the body angular rate measured by three gyros in the b-frame; is the gyro error; represents the angular rate of the n-frame with respect to the i-frame and expressed in the n-frame; is the corresponding error; the meanings of , , , and are given in (3); and the parameters , , , and are computed as
The INS’s velocity error equation is [27]where is the velocity error and is the velocity in east, north, and up; is an attitude error coupling term, which has strongly nonlinear characteristics about as described in [28]; is the rotation matrix; is the specific force, is the accelerometer error; is the projection of the Earth rotation rate ; is the angular rate of the n-frame with respect to the e-frame; where and denote the meridian and transverse radii of curvature and and represent the latitude and altitude of the vehicle; and and are the corresponding errors.
The INS’s position error is described as [28]where is the position error in latitude, longitude, and altitude. and can be formulated as
The IMU error includes the errors of the gyro and accelerometer. The gyro error and accelerometer error both can be described aswhere and represent the constant drift of the gyro and the zero bias of the accelerometer; and are the white noises for the gyro and accelerometer.
The system state variable is
Figure 1 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 aswhere is the continuous-time nonlinear function describing the system state equation and is the system state noise.
To use the discrete-time CKF, (11) should be discretized via the improved Euler method [29].where is the discrete-time nonlinear function describing the system state equation and 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,where and or and 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 vectorwhere is the measurement matrix, , , and is the measurement noise vector, which obeys the zero-mean Gaussian distribution with covariance .
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).where represents the system state and the is the system measurement; is the nonlinear function describing the system state equation and is the measurement matrix; and and are the process noise and measurement noise, respectively. They are uncorrelated zero-mean Gaussian white noises, and their covariances are and .
Based on this nonlinear system, the procedure of state estimation using the CKF is given as follows.
Step 1. Initialization: the state estimate and its error covariance are initially set as
Step 2. Cubature point calculation: suppose that the previous time’s state estimate and its error covariance matrix are given. Then, the cubature points are calculated aswhere ; is chosen aswhere represents the ith column of the identity matrix.
Step 3. Time update: the selection of cubature points is propagated by the system state equation.Then, 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 is incorrect, the innovation covariance will be worsened and further making the filter gain 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 covariance is unknown, we denote the as a parameter. According to the maximum likelihood framework, the estimation model for the parameter based on the measurement data can be established as [24, 26]where is the likelihood function of parameter with respect to the measurement data .
From the definition of likelihood function, we havewhere represents a suitable conditional probability density function (pdf) defined using the data .
We denote the innovation of the CKF as
For the nonlinear Gaussian system described by (15), the innovation obeys the multivariate Gaussian distribution , where
The innovations form of the likelihood function for the parameter 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 based on maximum likelihood principle can be achieved.
To reduce the computational burden of noise statistic estimation, is usually taken as a diagonal matrix, i.e.,considering the following condition:
Therefore, 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 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,
In the tth iteration, the constraints in (39) are linearized at the iteration point , and the following quadratic programming subproblem is obtained:where is the iterative direction; and are the gradients of functions and at ; is the positive definite quasi-Newton approximation matrix of Lagrange function at .
For the nonlinear constrained programming problem described in (39), the computing steps of SQP can be summarized as follows: Step 1: when , we firstly select the initial iteration point and symmetric positive definite matrix are selected, and then, the iteration precision is set . Step 2: the active set method [31] is used to solve the quadratic programming subproblem (40) to obtain the optimal solution and the corresponding Lagrangian multiplier . If , 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 , the objective function is constructed along the direction . Here, A one-dimensional search [31] is conducted to in (41) to determine the iterative step size . Step 4: let . If , 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], can be revised as : Here, Step 6: let , 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:
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 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. 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 ; the zero-bias and random walk coefficient of the accelerometer are 1 × 10−3 g and ; 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 , , and in pitch, roll, and yaw components; and the filter period is set as 1 s.
According to the chosen of simulation parameter, the measurement noise covariance for the INS/BDS integration should be
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 during the time interval (500 s, 700 s), i.e.,
Thus, the actual measurement noise covariance used for the filters can be written as
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).
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 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.
(a)
(b)
(a)
(b)
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]where , , and are the error components of 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. 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.
Figures 10–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.
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 iswhere represents the ratio of the perturbing distribution. If 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. 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.
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.
Data Availability
The 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
The authors declare that they have no conflicts of interest.
Acknowledgments
The work in this paper was supported by the National Natural Science Foundation of China (Project Nos. 41904028, 61703424, and 42004021), the Fundamental Research Funds for the Central Universities (Project No. 3102019ZDHQD09), the Natural Science Basic Research Plan in Shaanxi Province of China (Project Nos. 2020JQ-150 and 2020JQ-234), and the Aerospace Science and Technology Fund (Project No. 2020-HT-XG).