Abstract

This paper presents a new adaptive random weighting cubature Kalman filtering method for nonlinear state estimation. This method adopts the concept of random weighting to address the problem that the cubature Kalman filter (CKF) performance is sensitive to system noise. It establishes random weighting theories to estimate system noise statistics and predicted state and measurement together with their associated covariances. Subsequently, it adaptively adjusts the weights of cubature points based on the random weighting estimations to improve the prediction accuracy, thus restraining the disturbances of system noises on state estimation. Simulations and comparison analysis demonstrate the improved performance of the proposed method for nonlinear state estimation.

1. Introduction

Since navigation systems generally involve nonlinear characteristics, nonlinear filtering is a popular research topic in navigation and positioning. So far, various nonlinear filtering methods have been studied for spacecraft navigation. The extended Kalman filter (EKF) is a traditional nonlinear filtering method based on the first-order Taylor term of nonlinear functions [1, 2]. It maintains the computational efficiency of the linear Kalman filter. However, since it neglects higher-order terms of the nonlinear system model, EKF suffers from the degradation of estimation accuracy [3, 4]. The unscented Kalman filter (UKF) conducts nonlinear state estimation in the context of control theory, where a finite number of sigma points are used to propagate the probability of state distribution through nonlinear system dynamics [58]. In addition to the improved accuracy comparing to EKF, UKF has the merits such as simplicity in realization, high filtering precision, and good convergence [912]. However, UKF requires the a priori statistical characteristics of system noise to be precisely known. In practical applications, due to uncertainties in the dynamic environment, it is difficult to accurately describe noise statistics, leading to biased or even divergent filtering solutions [1315].

In the recent ten years, the cubature Kalman filter (CKF) has received a great attention in nonlinear filtering [16, 17]. Based on the cubature rules, this method adopts a set of cubature points evaluated via nonlinear functions to approximate the Gaussian integration [18, 19]. Similar to UKF, CKF does not require the linearization of the nonlinear model and the calculation of Jacobian matrix, thus overcoming the problems caused by linear truncation such as poor positioning accuracy and error divergence. Different from UKF, CKF has a stronger nonlinearity due to the use of cubature points. However, CKF requires exact information on the statistical characteristics of system noise [20]. If the statistical characteristics of system noise are not known exactly, the CKF filtering solution will be deteriorated.

The random weighting is a statistical method with high estimation accuracy and low computational burden [21, 22]. This method can handle the calculation of large sample size without requiring the accurate distributions of model parameters. It has been used to solve many problems, for example, multisensor data fusion [2325], parameter estimation [26], M-test in linear models [27], analysis of asymptotic properties of function distribution [28], and dynamic navigation and positioning [29]. However, there has been very limited research on utilizing the random weighting method with CKF for spacecraft navigation.

This paper presents a new adaptive random weighting cubature Kalman filter (ARWCKF) by adopting the concept of random weighting to address the limitation that the CKF performance is sensitive to system noise. This method constructs random weighting estimations for system noise statistics, as well as predicted state and measurement vectors and their associated covariances. Based on the random weighting estimations, it adaptively adjusts the weights of cubature points to inhibit the disturbances of system noises on state estimation, leading to improved filtering robustness. Simulations and comparisons with CKF have been conducted to evaluate the performance of the proposed ARWCKF.

2. Concept of Random Weighting Methods

Denote as a series of random variables in independent and identical distribution, and the common distribution function as . Then, the corresponding empirical distribution function can be defined aswhere is the indicator function.

The random weighted estimation of can be defined aswhere represents the random weighted vector subject to Dirichlet distribution , i.e., , and the joint density function of is with and .

3. Nonlinear System Models

Consider the following nonlinear discrete-time system:where is the state vector at epoch , is the measurement vector, is the system process noise, is the measurement noise, and are the Gaussian white noises, is the nonlinear system function, and is the nonlinear measurement function.

System process noise and measurement noise are assumed to be the uncorrelated Gaussian white noises of constant statistical properties; i.e.,where is the covariance function, and are the noise intensity matrices, and is the function.

Let

Substituting (5) into (3), the nonlinear system model can be expressed aswhere and are the Gaussian white noises with zero means and variances and , respectively.

4. Analysis of Cubature Kalman Filter

In order to improve the filtering performance of CKF, let us briefly review the filtering procedure of CKF, which is given as follows.

(i) Initialize State Estimate and Its Associated Error Covariance where obeys the Gaussian distribution.

(ii) Calculation of Cubature Points. Assume the posterior density function at epoch is . By Cholesky decomposition, the error covariance at epoch can be written as , where is a diagonal matrix at epoch . The cubature points can be calculated by , where ) is the system state of the th cubature point at epoch .

Cubature points set can be represented aswhere is the number of the cubature points and is set to twice of the state dimension, is a set of points, and is the -th point of .

(iii) State Prediction. The estimated state of the th cubature point from epoch to is described as

Based on (9), the predicted state of the ith cubature point from epoch to isand its covariance is .

Letandwhere is the posterior mean of the state estimation , which is propagated by the nonlinear system function .

Then

(iv) Observation Update. The estimated observation of the th cubature point from epoch to is represented by

Based on (14), the predicted observation of the th cubature point from epoch to epoch can be represented byand its autocovariance and cross-covariance matrices are and .

Letandwhere is the posterior mean of the state prediction , which is propagated by the nonlinear measurement function , and .

Then

(v) State UpdateIt can be seen from (11), (13), (16), and (18) that CKF requires the statistical characteristics of the system and measured noises to be known. However, in engineering practice, it is difficult to accurately know the statistical characteristics of the system and measurement noises, thus degrading the filtering accuracy. In the following, we shall discuss how to adaptively estimate the system noise and measurement noise via the concept of random weighting.

5. Adaptive Random Weighting Estimation of Noise Statistics

5.1. Estimation of Noise Statistics

Denote the maximum posterior estimates of , , , and by , , , and , respectively, and the posterior estimate of state   by  . Assuming is a joint probability density function, we have [30]where can be obtained from prior information, and thus it is considered as a constant.

Consider the conditional density functionwhere and .

Consequently, the maximum posterior estimates , , , and can be obtained by solving for the conditional density function . It is evident that the maximum posterior estimates , , , and are not related to the denominator in (21). Therefore, the problem of calculating , , , and can be equivalently obtained by maximizing the following density function in (21).

According to the multiplication theorem of conditional probability, we can obtain [30]where is the state dimension, is a constant, and is the determinant of .

Assume that the measurements are known and are independent of each other. Then,where is the measurement dimension and is a constant.

Substituting (22) and (23) into (20) yieldswhere .

Taking the logarithm on both sides of (24) yieldsLetThe maximum posterior estimates and can be obtained as (see Appendices A and B for details)where is the posterior mean of state estimate , which is propagated through nonlinear system function .

Similarly, letThe maximum posterior estimates and can be obtained as (see Appendices C and D for details)where is the posterior mean of one-step state prediction which is propagated through nonlinear measurement function .

In (27) and (29), replacing the filtering estimations and with the estimations and and replacing with state prediction , we havewhere and .

Accordingly, the random weighted estimations of , , , and can be represented aswhere () are the random weighting factors.

5.2. Determination of Random Weighting Factors

Suppose the estimation and prediction of the state at epoch are and , respectively. The residual vector of the prediction is assumed to be , which is expressed asThe residual vector of the measurement is expressed aswhere .

When the statistics of the system process noise are changed, the contribution of the state prediction to the state estimate will decrease, leading the prediction to be biased. As a result, the magnitude of the residual vector of the state prediction will increase. Similarly, when the statistics of the measurement noise are changed, the measurement residual will be biased and its magnitude will increase.

In order to capture the changes of the system noises, the random weighting factors are required to satisfywhere , , and the symbol “” indicates the proportional operation.

Equation (34) implies that the larger the value of is, the larger the weighting factor is.

Therefore, the random weighting factors can be determined as follows.

LetNormalizing (), the random weighting factors are obtained asThe random weighted estimation of noise statistics enables adaptively adjusting the weights of the cubature points to improve the accuracy of the state prediction and measurement prediction and further inhibit the interference of system process noise and measurement noise on state estimation, leading to improved filtering accuracy and reliability.

6. Adaptive Random Weighting Cubature Kalman Filter

Assume the posterior density function at epoch is . The ARWCKF algorithm for the nonlinear system defined by (6) can be described as follows.

(i) Initialize State Estimate and Error Covariance

(ii) Time Update and Calculation of Cubature Points. Denote the covariance of state prediction at time by . By Cholesky decomposition [16, 17], can be written asand the cubature points can be calculated bywhere () is the system state of the th cubature point at epoch , and is a diagonal matrix at time .

Then, the estimated state of the th cubature point from epoch to can be written as

(iii) State Prediction. Based on (40), the predicted state of the th cubature point from epoch to is

Accordingly, the random weighted estimation of can be written as

The covariance of the state prediction is described by

Accordingly, the random weighted estimation of can be written as

(iv) Measurement Prediction. The estimated measurement of the th cubature point from epoch to can be written as

The predicted measurement of the th cubature point from epoch to can be expressed as

Accordingly, the random weighted estimation of can be written as

The autocovariance of the measurement prediction is described by

Accordingly, the random weighting estimation for the autocovariance of the measurement prediction is

The cross-covariance of the measurement prediction is

Accordingly, the random weighting estimation for the cross-covariance of the measurement prediction is

(v) State Update. The filter gain at epoch is

The state estimation at epoch is

The covariance estimation of the state error at epoch is

From the above, it can be seen that the proposed ARWCKF incorporates random weighting factors in CKF, enabling us to adaptively refrain from the disturbances of system noises by online adjusting random weights to improve the accuracy of state estimation. The ARWCKF algorithm is shown in Figure 1.

7. Performance Evaluation and Discussion

Simulation trials were conducted to evaluate the effectiveness of the proposed ARWCKF for a SINS/SRS (strap-down inertial navigation system/spectrum red-shift) integrated navigation system. The comparison analysis with CKF was also conducted to demonstrate the improved performance of the proposed ARWCKF for the SINS/SRS integrated navigation system.

7.1. System State Model

Due to the high accuracy in the velocity of SRS, the velocity error of SRS is much smaller than that of SINS. Therefore, the velocity error of SRS is assumed as a Gaussian white noise to reduce the dimension of the SINS/SRS integrated navigation system. Accordingly, the state vector of the SINS/SRS integrated navigation system consists of SINS errors only.

The SINS/SRS integrated navigation system adopts the inertial coordinate frame as the navigation frame. Its system state vector is defined aswhere , , and are the errors in velocity, position, and attitude from SINS; is the constant bias of the gyros; and is the zero bias of the accelerometers.

The system state equation of the SINS/SRS integration is described bywhere is the nonlinear function given by (57), is the system state vector, and is the system noise.where is the Euler platform error angle matrix; is the transformation matrix from the navigation () to computer () frames in terms of attitude; is the transformation matrix from the body () to computer () frames in terms of attitude; and are the projections of the actual and ideal angular velocities from the Earth coordinate system () to the inertial coordinate system () into the navigation frame (); and are the calculation errors of and ; and are the real force and its associated error of the accelerometer; and are the real velocity and its associated error; is the error of gravity acceleration; and are the latitude and altitude values; and and are the radii of curvatures of local meridian and prime vertical.

The noise coefficient matrix is defined asThe system noise is described by , where and are the gyro and accelerometer random noise, respectively.

7.2. Measurement Model

In order to overcome the drawback that the velocity error of SINS is accumulated in time series, the velocity information from SRS is used to correct the velocity error of SINS. Further, a radar altimeter is used to provide the altitude information to correct the altitude channel of SINS.

The measurement of the SINS/SRS integrated navigation system can be chosen as the difference in velocity between SINS and SRS as well as the difference in altitude between the radar altimeter and SINS.

Suppose the velocities obtained by SRS and SINS are and . The difference in velocity between SRS and SINS is defined aswhere is the velocity measurement noise matrix, and is the measurement matrix.

Further, the difference in altitude between the radar altimeter and SINS can be described aswhere and are the altitudes obtained by SINS and the radar altimeter; is the measurement noise matrix in altitude; and is the measurement matrix.

Combining (59) with (60), the measurement equation of the SINS/SRS integration subsystem is established as

7.3. Simulation and Analysis

Assume the spacecraft orbits the earth, where the orbit parameters are described in Table 1. A flight period of 4000s was selected for the simulation test, where the initial position was “6359800m, 3076100m, -6268900m,” and the end position was “4550800m, 5222600m, -4356700.” The flight trajectory is shown in Figure 2.

In the simulation, the SINS gyro drift is 0.05(°)/h, the random walk of the gyro is 0.005(°), and the zero bias and random drift of the accelerator are and . The SINS initial alignment error is . The initial position and velocity errors of the spacecraft are 10m, 10m, and 10m and 0.2m/s, 0.2m/s, and 0.2m/s. The sampling period of SINS is 0.01s, the data measurement frequency of SRS is 1Hz, and the velocity error of SRS is 0.05m/s. The noises of the system state model and measurement model are set as 0.01 and 0.02, respectively. The unscented transformation parameters are and .

The position error is defined as

The velocity error is defined as

For comparison analysis, 100 Monte Carlo simulation trials were conducted under the same conditions by CKF and ARWCKF, respectively.

Figure 3 shows the estimation errors in position and velocity by both CKF and ARWCKF. It can be seen that the position and velocity errors are within “-165m, 145m” and “-1.72m/s, 1.68m/s.” This poor estimation accuracy is because CKF does not have the capability to inhibit the disturbance of system noises. In contrast, as shown in Figure 4, ARWCKF can effectively estimate the noise statistics of the system state and measurement models to inhibit the disturbances of system noises, leading to the significantly improved accuracy. As shown in Figure 3, the resultant position and velocity errors by ARWCKF are within “-30m, 32m” and “-0.31m/s, 0.35m/s.” Table 2 summaries the means of the root mean square errors (RMSEs) in terms of position and velocity by both methods as well as the RMSEs of the estimated noises by the proposed ARWCKF for the SINS/SRS integrated navigation system.

The performance evaluation was further analyzed in terms of average normalized estimation error squared (ANEES). Figure 5 shows the ANEES values in terms of position and velocity errors for both CKF and ARWCKF. It can be seen that there are significant oscillations involved in the ANEES curves of CKF. This means that a considerable portion of the error covariance matrices obtained by CKF are unreliable due to the existence of system noises. However, the ANEES values of ARWCKF are small during the entire simulation test, showing the estimation consistency of ARWCKF is not affected by system noises. Table 3 summaries the ANEES values of both CKF and ARWCKF. The results in terms of ANEES demonstrate the covariance matrices obtained by ARWCKF are much more reliable than those of CKF.

The above results demonstrate that the proposed ARWCKF can effectively inhibit the disturbances of system noises for spacecraft navigation, leading to the improved accuracy and robustness in comparison with CKF.

8. Conclusions

This paper presents a new ARWCKF for state estimation in nonlinear systems. It adopts the concept of random weighting for the first time to address the problem that the CKF performance is sensitive to system noise. The proposed method incorporates random weights in CKF to estimate system noise statistics as well as predicted state and measurement together with their associated covariances. Based on this, it adaptively adjusts random weights of cubature points to inhibit the disturbances of system noises on state estimation, leading to improved filtering accuracy and robustness. Simulations and comparison analysis demonstrate that the proposed ARWCKF is superior to CKF for spacecraft navigation.

Future work will focus on improvement of the proposed ARWCKF. By combining the proposed ARWCKF with advanced artificial intelligence techniques such as deep learning neural networks, swarming algorithms, and fuzzy logic, it is expected to develop an intelligent filtering algorithm for automatic estimation of system noise statistics from various error sources in practical engineering applications.

Appendix

A. Proof of in (27)

From (24), we readily have

Taking the partial derivatives of on both sides of (A.1) yields

Let

Since , we havei.e.,

Thus,

The proof of is completed.

B. Proof of in (27)

Suppose we have the following conditions: and are symmetric matrices; i.e., and

where is the determinant of .

Taking the partial derivatives of on both sides of (A.1) yields

Equation (B.1) can be rewritten as

Multiplying both sides of (B.2) by yields

Substituting into (B.3), we have

The proof of is completed.

C. Proof of in (29)

Taking the partial derivatives of on both sides of (A.1) yieldsLet

Since , we havei.e.,Thus,The proof of is completed.

D. Proof of in (29)

Assume we have the same conditions as in Appendix B. Taking the partial derivatives of on both sides of (A.1) yieldsAccording to the second equal sign in the above formula, we can getMultiplying both sides of the above formula by yieldsSubstituting into the above formula yieldsThe proof of is completed.

Data Availability

The data used to support the findings of this study are included within the article.

Conflicts of Interest

The authors declare that there is no conflict of interest regarding the publication of this paper.

Acknowledgments

The work of this paper was supported by the Doctoral Dissertation Innovation Foundation of Northwestern Polytechnical University, China (Project no. CX201834), and the Training Foundation for Excellent Doctoral Students of Northwestern Polytechnical University, China.