Journal of Sensors
Volume 2019 (2019), Article ID 1216838, 19 pages
https://doi.org/10.1155/2019/1216838

Research Article

Randomly Weighted CKF for Multisensor Integrated Systems

Hua Zong,¹ Zhaohui Gao,² Wenhui Wei,² Yongmin Zhong,³ and Chengfan Gu⁴

¹National Key Laboratory of Science and Technology on Aerospace Intelligent Control, Beijing 100854, China
²School of Automatics, Northwestern Polytechnical University, Xi’an 710072, China
³School of Engineering, RMIT University, Bundoora, VIC 3083, Australia
⁴Laboratory of Excellence on Design of Alloy Metals for low-mAss Structures (DAMAS), Université de Lorraine, France

Correspondence should be addressed to Zhaohui Gao; alexandergao@mail.nwpu.edu.cn

Received 3 November 2018; Revised 27 January 2019; Accepted 4 August 2019; Published 11 November 2019

Academic Editor: Jesus Corres

Copyright © 2019 Hua Zong et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract.

The cubature Kalman filter (CKF) is an estimation method for nonlinear Gaussian systems. However, its filtering solution is affected by system error, leading to biased or diverged system state estimation. This paper proposes a randomly weighted CKF (RWCKF) to handle the CKF limitation. This method incorporates random weights in CKF to restrain system error’s influence on system state estimation by dynamic modification of cubature point weights. Randomly weighted theories are established to estimate predicted system state and system measurement as well as their covariances. Simulation and experimental results as well as comparison analyses demonstrate the presented RWCKF conquers the CKF problem, leading to enhanced accuracy for system state estimation.

1. Introduction

Nonlinear system state estimation is an importance research topic in various fields such as multisensor data fusion and integrated navigation, weapon guidance and control, intelligent transportation, system identification, and target tracking. The extended Kalman filter is a typical estimation method for nonlinear system state estimation [1, 2]. This method approximates the nonlinear system equation using first-order Taylor expansion and then uses the conventional Kalman filter for system state estimation. However, the linear approximation of the nonlinear system equation involves error in estimating the mean and covariance of system state, resulting in divergent solutions [3, 4]. Further, the calculation of the Jacobian matrix involved in EKF is an intricate and burdensome process [5].

The unscented Kalman filter (UKF) is an improved method of EKF. This method incorporates unscented transformation in the Kalman filter to estimate system state for nonlinear systems [6–9]. It is simple in computation and has higher accuracy than EKF [10]. However, due to the use of the same structure of linear update as the Kalman filter, UKF requires accurate system models and exact noise statistics [11]. If the system models involve error, the UKF solution will be degraded or diverged. The adaptive unscented Kalman filter (AUKF) is an approach to restraining system error’s interference on system state estimation by introducing an adaptive factor [12]. However, as the adaptive factor is determined empirically, this method cannot be adapted to the uncertain nature of system error, leading to suboptimal or biased filtering solutions. The stochastic integration filter (SIF) is a local filter based on stochastic integration [13]. This filter provides the asymptotically exact evaluation of an integral. Its convergence speed is also faster than that of Monte Carlo integration. However, similar to UKF, the SIF filtering solution is still affected by system error.

The particle filter (PF) is a method to conduct recursive Bayesian estimation based on Monte Carlo simulation [14, 15]. This method is suitable for systems with strong non-Gaussian and nonlinear properties, as it represents the posterior distribution with a set of particles. However, the problem of particle degradation occurs frequently in the recursive process, leading to poor filtering accuracy [16, 17]. Further, PF also requires a large number of sample points to achieve reliable estimation, leading to a large computational cost [18–20]. The unscented particle filter (UPF) reduces the PF computational load using ( is the system state dimension) sample points to improve the importance of sampling density via unscented transformation [21]. However, it still suffers from the problem of particle degeneracy. Wei et al. studied an improved UPF by combining adaptive filtering with square-root filtering into UPF to restrain process model noise’s interference in the filtering process, thus preventing particles from degeneracy [22]. However, this improvement is achieved by sacrificing the computational performance.

The cubature Kalman filter (CKF) is a method of state estimation for nonlinear Gaussian systems. Based on the cubature criterion, this method approximates the Gaussian integral by generating a set of sample points which are named the cubature points, according to nonlinear system functions [23–25]. It is derived from the Bayesian theory and Spherical-Radial criterion via rigorous mathematical analysis. Comparing to EKF, CKF reduces the linearization error of EKF, leading to third-order accuracy for system state estimation. It also has various advantages over UKF. First, the cubature points in CKF are obtained by integral operations, while the sigma points in UKF are resulted from unscented transformation, leading to the selection of weights and sample points at random. In addition, CKF adopts 2n sample points under a common weight, while UKF selects sample points under different weights. Further, in terms of filtering stability, the common weight of cubature points in CKF is 1/2n. This means that no matter how large the system dimension is, CKF guarantees that the weight of cubature points is always positive. However, the free adjustment parameter in UKF is random. If , the weight . This will make the covariance matrix of a high dimensional system nonpositive, thus leading to deteriorated or diverged filtering solutions. For strongly nonlinear and Gaussian systems, CKF also outperforms UKF in terms of accuracy and stability [26, 27].

However, the state estimation of CKF is disturbed by system error which may be generated from various sources such as manufacturing error, installation error, calibration error, and environmental error. In CKF, the predictions of state and measurement and error covariances are calculated by arithmetic mean with a common weight. Since the predictions and error covariances at all cubature points have the same contribution to the evaluations of prediction errors, the prediction errors acquired from all cubature points cannot reflect the actual statistical characteristics of system error, thus leading to distorted or diverged solutions. The adaptive CKF is an approach to restraining system error’s disturbance on system state estimation by introducing an adaptive factor [28]. However, since the adaptive factor is determined empirically, this method cannot adapt to the uncertain nature of system error, resulting in suboptimal or biased solutions. Further, it still suffers from the problem due to the use of arithmetic mean for state and measurement predictions.

Randomly weighted estimation is a relatively new statistical method [29]. This method is extensively used in different areas, including system identification and target tracking, navigation, guidance and control, error characteristic estimation, and estimation and analysis of empirical distribution function [30]. It provides an unbiased estimation at a low computation cost. It also suits large sample computation without requirement of the accurate distribution of model parameters [31]. However, it is a difficult task to combine randomly weighted estimation into CKF to improve the filtering performance. Just recently, the research group reported a randomly weighted method to estimate system noise statistics and further restrain their influence on system state estimation [32]. However, this method focuses on the estimation of system noise statistics, rather than system model error. Further, it is a loose integration of randomly weighted estimation with CKF, where randomly weighted estimation is applied to estimate system noise statistics which are further plugged into the CKF filtering process, rather than directly applied to modify and improve the CKF filtering process itself.

In this paper, a new randomly weighted CKF (RWCKF) is presented to enhance the estimation precision of CKF. Different from our previous work [32], this method directly modifies cubature point weights based on prediction errors, rather than estimates system noise statistics, to restrain system error’s influence on system state estimation. It establishes randomly weighted theories to estimate predicted system state and system measurement together with their error covariances. Consequent upon these theories, the proposed RWCKF dynamically modifies cubature point weights as per prediction errors to restrain system error’s interference on system state estimation to improve the filtering accuracy. Simulations, experiments, and comparisons with UKF, CKF, SIF, and AUKF were performed to comprehensively examine the efficacy of the presented RWCKF.

2. CKF

In order to improve CKF, let us briefly discuss and analyze the CKF procedure at first.

The mathematical models of a nonlinear system can be generally expressed as

where is the system state vector at time point , is the measurement vector, and are the additive process noise and measurement noise, and and are the nonlinear functions describing the process and measurement models.

Assuming process noise and measurement noise are the independent and time-varying Gaussian white noises with nonzero means, satisfying

where both and are a symmetric matrix, and is the function.

The CKF algorithm involves the following steps:

Step 1. Set the initial values for the system state estimation and error covariance

Step 2. Time update and computing cubature points.
Suppose the posterior density function at time point obeys the normal distribution , i.e., . Then, the error covariance at time point can be expressed as

Accordingly, the cubature points can be obtained by

where () represents the system state vector at time point calculated from the ith cubature point, denotes the matrix obtained by the Cholesky decomposition [23, 25] at time point , and denotes the ith cubature point, in which represents the quantity of the cubature points and is commonly chosen as large as the system state dimension by a factor of two, represents a set of symmetric points in the system state space, and is the ith point of . A set of cubature points can be simply determined as and , where is the system state dimension (see Appendix A for computation of the cubature points).

Then, the ith cubature point from time point to is estimated by

Accordingly, the state prediction and error covariance from time point to at the ith cubature point is described by

Step 3. Measurement update.
By Cholesky decomposition, can be expressed by

Calculate the system state vector from time point to at the ith cubature point

Then, the measurement vector from time point to at the ith cubature point is given by

Accordingly, the measurement prediction together with its associated autocovariance and cross-covariance matrices from time point to at the ith cubature point is represented by

Calculate the filtering gain at time point

Calculate the estimated state at time point

According to the mean-square error of minimum variance estimation, we have

where both and are a symmetric matrix, i.e., and .

The covariance matrix of the state error at time point is represented by

If the nonlinear system model involves error, (1) will become

where and represent process model error and measurement model error, respectively.

Accordingly, the predicted state and predicted measurement will become

It is evident from (19) to (21) that if the nonlinear system model involves error, the state prediction given by (7) and the measurement prediction given by (12) will become biased and further make the filtering gain biased, thus deteriorating the system state estimation obtained from (16). Accordingly, if there exists a system model error, the CKF estimation will be deteriorated.

It is also evident from the above that the predicted state and measurement as well as their covariances are computed by arithmetic mean via the same weight . Therefore, the predictions and their covariances at all cubature points have the same contribution to the evaluations of prediction errors, and thus, the prediction errors calculated from all cubature points cannot precisely characterize the actual statistical properties of system error. Accordingly, the CKF estimation may be biased or diverged.

3. Randomly Weighted CKF

3.1. Randomly Weighted Estimation

Assume that are the random variables which are independent of each other and identically distributed as well as share a distribution function , and the associated empirical distribution function of these variables is represented as

where denotes the indicative function.

Accordingly, the randomly weighted estimate of is expressed by

where is the randomly weighted factor, which is subject to Dirichlet distribution , that is, and the united probability density function of is , where denotes the Gamma function, and .

Comparing (22) with (23), we can see that the randomly weighted method utilizes randomly weighted factors for estimation, while the arithmetic mean method utilizes one commonly weighted factor for estimation. There are various strategies to determine the randomly weighted factors according to Dirichlet distribution [30, 31]. A straightforward way is as follows.

Generate an independent and identically distributed sample sequence of the uniform distribution and assume this sample sequence is . Let be the order statistic of , where and . Then, , and the joint density function for is the Dirichlet distribution .

3.2. Randomly Weighted CKF

The proposed RWCKF adopts the randomly weighted estimation to address the CKF limitation caused by arithmetic mean estimation.

State Prediction. According to the randomly weighted concept described by (22) and (23), based on (7), the randomly weighted estimate of the predicted state is obtained as

where represents the randomly weighted factor.

In a similar way, from (8), the randomly weighted estimate of predicted state’s covariance matrix is expressed by

Measurement Prediction. From (12), the randomly weighted estimate of the predicted measurement is obtained as

From (13), the randomly weighted estimate of predicted measurement’s autocovariance matrix is represented by

By (14), the randomly weighted estimate of predicted measurement’s cross-covariance matrix is obtained as

Update of System State. Calculate the filtering gain at time point

Compute the state estimation

where is the predicted state which is expressed by

where represents the posterior density function at time point .

Compute the estimation of state error covariance

It should be noted that in practice, it cannot guarantee that the state error covariance matrix obtained from (32) is always positive. If this covariance matrix is negative, the filtering process will be diverged. This problem can be overcome by either selecting an appropriate initial state value [10] or using the matrix decomposition technique such as singular value decomposition [33] and square root decomposition [34].

From the above equations, we can see that the proposed RWCKF adopts the randomly weighted concept in CKF. Thus, we can adaptively select appropriate random weights based on the prediction residuals to improve the estimation accuracy for state prediction and measurement prediction together with their covariances and cross-covariance, leading to enhanced accuracy for state estimation in comparison with CKF. Figure 1 illustrates the procedure of the RWCKF algorithm.

Figure 1: RWCKF algorithm.

3.3. Comparison Analysis between RWCKF and CKF

If the nonlinear system model involves error, based on (20) and (21) which describe the predicted state and predicted measurement in CKF, the predicted state and predicted measurement in RWCKF can be readily obtained as

In CKF, the residual vectors of the predicted state and measurement are expressed as

In RWCKF, the residual vectors of the predicted state and measurement are expressed as

If the nonlinear system model involves error, the CKF residual vectors and given by (34) and (35) will be biased, and their magnitudes will also increase. However, as shown in (36) and (37), RWCKF enables us to reduce the magnitudes of the residual vectors and by adjusting randomly weighted factors to restrain system model error’s disturbance, leading to increased estimation accuracy.

4. Performance Evaluation and Discussion

Simulations and experiments were conducted to assess the effectiveness of the presented RWCKF. Comparison analyses with CKF, UKF, SIF [13], and AUKF [6] are also discussed in this section.

4.1. Target Tracking

A simulation test on tracking a dynamic target with a two-dimensional radar was designed for performance evaluation. The dynamic equation of the target is expressed as

where denotes the state, (, ) the target position, and (, ) the target velocity. denotes the process noise and is considered as a Gaussian white noise having zero mean and covariance

where denotes the time step of the continuous measurement vector, and is the scale constant of the process noise. The value of is determined by the reliability of the system state model. The larger the is, the more unreliable the prediction information is. In the simulation test, the scale constant was set to by test computation.

The transition matrices and can be expressed as

is the systematic error function, which is expressed as

The measurement equation of the target is linear and can be described as

where

is the measurement noise and is considered as a Gaussian white noise having zero mean and covariance

The process noise is independent of the measurement noise.

The simulation time was 100 s, and the time step was 1 s. Initially, the target was in a Gaussian random state having mean [0.48, 1.95, 0.48, 1.95] and covariance .

4.1.1. Filtering Accuracy

For the purpose of comparison, trials were carried out under the same circumstances by CKF, UKF, SIF, AUKF, and RWCKF to estimate the target position, where Monte Carlo simulation was conducted 100 times. Figures 2 and 3 show the position errors and position RMSEs (root-mean-square errors) of these five filters. In terms of estimation consistency, Figure 4 shows the average normalized estimation errors squared (ANEESs) of the filters.

Figure 2: Errors of position by CKF, UKF, SIF, AUKF, and RWCKF.

Figure 3: RMSEs of position by CKF, UKF, SIF, AUKF, and RWCKF.

Figure 4: ANEESs of CKF, UKF, SIF, and AUKF.

As shown in Figure 2, the error curves of both CKF and UKF involve large oscillations, and their position errors are approximately [, ]. This is because the UKF performance is sensitive to system error, while CKF is unable to reflect the real characteristics of system error due to the use of arithmetic mean for prediction errors. The error curves of both SIF and AUKF also involve oscillations. This is because the SIF performance is also sensitive to system error, while AUKF is unable to adapt the dynamically changing characteristics of system error due to the adaptive factor determined empirically. However, the error curve of RWCKF involves small oscillations, and its position error is approximately [, ], which is 15 times smaller than those of UKF and CKF. This is because RWCKF adopts random weights to inhibit system error’s disturbance on the system state estimation, leading to the enhanced precision. Figure 3 clearly shows that the position RMSE of RWCKF is much smaller than those of CKF, UKF, SIF, and AUKF. The position RMSE of RWCKF is approximately [0.15 m, 0.21 m], while those of UKF and CKF are approximately [0.50 m, 0.62 m] and those of SIF and AUKF are approximately [0.45 m, 0.51 m] and [0.39 m, 0.42 m].

We can see from Figure 4 that there are obvious oscillations involved in the ANEES curves of CKF, UKF, SIF, and AUKF, while the ANEES values of RWCKF are around 1 as shown in Figure 5. This demonstrates the covariance matrices obtained by RWCKF are much more reliable than those of CKF, UKF, SIF, and AUKF.

Figure 5: ANEES of RWCKF.

From the above results, it is evident that RWCKF outperforms the other filters in terms of accuracy.

4.1.2. Computational Performance

The above simulations were carried out in the Matlab program on a Core i7 PC with 3.6 GHz CPU and 4 GB memory. The average computational times for the 100 Monte Carlo simulations of CKF, UKF, SIF, AUKF, and RWCKF are shown in Table 1. We can see from Table 1 that the average computational time of the proposed RWCKF is close to those of CKF and UKF and much smaller than those of SIF and AUKF.

Table 1: The average computational times of CKF, UKF, SIF, AUKF, and RWCKF.

Filtering method
Average computational time (s)

CKF
0.1533

UKF
0.1747

SIF
0.1945

AUKF
0.2589

RWCKF
0.1631

4.2. SINS/XPN Integrated Navigation System

Simulation experiments were also carried out to verify the effectiveness of the reported RWCKF in terms of SINS/XPN (Strapdown Inertial Navigation System/X-ray Pulsar Navigation) integrated navigation.

4.2.1. System Models

Since the position and velocity of XPN are of high accuracy, they are used to correct the position and velocity errors of SINS. In order to decrease the state vector size for SINS/XPN integration, both position error and velocity error of XPN are considered as a Gaussian white noise. Therefore, only the errors of SINS are used to represent the system state for SINS/XPN integration.

The inertial coordinate system is adopted as the navigation frame of the SINS/XPN integrated navigation system. Its state vector is defined by

where (), (), and () denote the position error, velocity error, and attitude error, () represents gyro’s constant drift, and () represents accelerometer’s zero bias.

As per (45) and the SINS error model, the state equation of the SINS/XPN integrated system can be represented by

where denotes the nonlinear system function, which is described by

where denotes the inverse matrix of the specific force , and denotes the transfer matrix from the body to the inertial coordinate system.

where , and is the gravitational constant.

denotes the system noise and is represented by

where is the gravitational vector error, denotes the measurement error resulted from gyro’s constant drift, and denotes the measurement error resulted from accelerometer’s zero bias.

In the SINS/XPN integrated navigation system, the position and velocity can be obtained independently by SINS and XPN. By subtracting the XPN measurement from the SINS measurement, the measurement model for SINS/XPN integrated navigation is expressed as

where and denote the position and velocity output by SINS, and denote the position and velocity output by XNAV, is the measurement matrix, and is the measurement noise, which is considered as a Gaussian white noise with zero mean and covariance .

4.2.2. Simulation and Analysis

The rocket is supposed to orbit the earth with the parameters provided in Table 2. The obtained flight path is shown in Figure 6.

Table 2: Rocket orbital parameters.

Parameters
Values

Semimajor axis
6976.023529 km

Orbital eccentricity
0.001159

Orbital inclination
28.948°

RAAN
340.61°

Argument of periapsis
342.274°

True anomaly
230.52°

Orbit time period
49866 s

Figure 6: Rocket flight path.

During the simulation, the gyro drift was , the gyro random walk was , the zero bias of the accelerometer was , and the random drift of the accelerometer was . The SINS initial error of alignment was . For the rocket, the initial errors in position, velocity, and attitude were (15 m, 15 m, 15 m), (0.1 m/s, 0.1 m/s, 0.1 m/s), and , respectively. The covariance of the initial state error was

The process noise covariance and measurement noise covariance were

The data update rate of SINS was 10 Hz, the measurement cycle of XPN was 1 s, and the simulation time was 150000 s (3 orbital cycles). The position error and velocity error of XPN were 25 m and 0.3 m/s.

In order to analyze the influence of system error upon state estimation, a straightforward way is to add a constant error in the system to make the system error salient [35]. Accordingly, the two variance errors, i.e., (, , ) and (, , ), were added to the predicted measurement covariances of position and velocity, respectively, in the second orbital cycle of the rocket flight, i.e., the time period (49866 s, 99732 s).

The position error and velocity error are expressed as

For comparison and analysis, simulation tests were carried out under the same circumstances by CKF, UKF, SIF, AUKF, and RWCKF, respectively. Monte Carlo simulation was conducted 100 times. Figures 7–11 show the position errors and velocity errors obtained by these filters. Figures 12 and 13 show the ANEES values of the filters.

Figure 7: Position error and velocity error by CKF.

Figure 8: Position error and velocity error by UKF.

Figure 9: Position error and velocity error by SIF.

Figure 10: Position error and velocity error by AUKF.

Figure 11: Position error and velocity error by RWCKF.

Figure 12: ANEESs of CKF, UKF, SIF, AUKF, and RWCKF for position error.

Figure 13: ANEESs of CKF, UKF, SIF, AUKF, and RWCKF for velocity error.

As shown in Figures 7 and 8, the position error and velocity error of CKF are similar to those of UKF, which are around 120 m and 1.2 m/s. In particular, similar to UKF, during the time period with the added variance errors, the estimation accuracy of CKF is also deteriorated significantly. The reason for UKF to cause the large position and velocity errors is that its filtering performance is sensitive to system error. For CKF, it uses arithmetic mean to estimate prediction errors, incapable of identifying the actual characteristics of system error. Thus, the CKF solution is affected by system error, leading to the limited accuracy.

As shown in Figures 9 and 10, comparing to CKF and UKF, the improvement of SIF is not as significant as that of AUKF, especially for the time period with the added variance errors. This is because AUKF uses the adaptive factor to resist system error’s disturbance on the filtering solution, while SIF still lacks the capability of resisting system error’s disturbance on the state estimation. However, as the adaptive factor of AUKF is determined empirically, AUKF cannot be adapted to the dynamically changing conditions of system error, leading its improvement to be limited.

In contrast, as shown in Figure 11, the accuracy of RWCKF is superior to that of CKF, UKF, SIF, and AUKF. The position error and velocity error of RWCKF are around 63 m and 0.62 m/s, which are much smaller than those of the other filters, especially for the time period where the constant errors were added. This is because in order to restrain system error’s influence on system state estimate, RWCKF assigns different weights to cubature points to estimate state prediction and measurement prediction together with error covariances, leading to the increased filtering accuracy. Table 3 summaries the position RMSEs and velocity RMSEs of UKF, CKF, SIF, AUKF, and RWCKF for both time periods with and without the added errors.

Table 3: Position and velocity RMSEs of the filtering algorithms.

Filtering algorithm
Position RMSEs (m)
Velocity RMSEs (m/s)

With the added errors
Without the added errors
Difference value
With the added errors
Without the added errors
Difference value

UKF
54.354
41.513
12.841
0.546
0.426
0.120

CKF
53.978
41.606
12.372
0.528
0.412
0.116

SIF
51.244
39.402
11.842
0.519
0.407
0.112

AUKF
43.857
34.596
9.261
0.456
0.352
0.104

RWCKF
26.799
23.432
3.364
0.246
0.203
0.043

It can be seen from Figures 12 and 13 that there are obvious oscillations involved in the ANEES curves of CKF, UKF, SIF, and AUKF. Particularly for the time period with the added errors, more unreliable error covariance matrices are appeared and their associated ANEES values become larger. However, the ANEES values of RWCKF are still around 1, which means the estimation performance of RWCKF is consistent and not affected by the added errors. This demonstrates the covariance matrices obtained by RWCKF are much more reliable than those by CKF, UKF, SIF, and AUKF.

The above results demonstrate that RWCKF can effectively restrain system error’s disturbance on system state estimate, resulting in increased navigation accuracy comparing to CKF, UKF, SIF, and AUKF.

4.3. Experimental Analysis

Experimental tests on monitoring the position of a ground vehicle were also performed to verify the effectiveness of the proposed RWCKF. The ground vehicle used a SINS/GPS integrated navigation system for dynamic navigation. As shown in Figure 14, the SINS/GPS integrated navigation system and its associated DC power supply were fixed on the vehicle. The vehicle also carried an ampere-voltage meter and an industrial personal computer (IPC) to monitor and process the signals from the SINS/GPS integrated system. Table 4 lists the parameters of the SINS/GPS integrated navigation system.

Figure 14: Experimental setup.

Table 4: Parameters for SINS/GPS integration.

Parameters
Values

Refresh frequency
SINS 125 Hz, GPS 5 Hz

Starting time period
<1 s

Thermal Reading of environment

SINS gyros
Measurement limit
±200°/s

Zero bias
10.0°/hr (1σ)

Scale factor
0.1% (1σ)

Random walk coefficient
1.0°/hr1/2 (1σ)

SINS accelerometers
Measurement limit
±20 g

Zero bias
2 mg (1σ)

Scale factor
0.1% (1σ)

Random walk coefficient
0.005 m/s/hr1/2 (1σ)

GPS measurement
L1/L2
Horizontal accuracy 1.8 m, vertical accuracy 2.5 m (1σ)

DGPS
Horizontal accuracy 0.5 m, vertical accuracy 1.0 m (1σ)

Velocity accuracy
0.02 m/s (1σ)

The vehicle initially stopped at (E108°4605.89, N34°0141.24). After the SINS/GPS integrated system was initialized for one minute, the vehicle started to move along the Huanshan Road in the Northeast direction to the Fengyu Kou roundabout at (E108°4904.61, N34°0310.28). At the Fengyu Kou roundabout, the vehicle turned over and then moved back to the initial position. The vehicle trajectory and its corresponding position coordinates are shown in Figures 15 and 16. The travelling distance and time of the vehicle were 12.38 km and 19 minutes, and the average vehicle speed was 39.1 km/h. During the vehicle travelling process, the GPS receiver functioned well. Meanwhile, the vehicle position was also acquired by a high-precision differential GPS and used as the reference to calculate the positioning errors of the different filters adopted in the SINS/GPS integration system.

Figure 15: The trajectory of the vehicle.

Figure 16: The position coordinates for the trajectory of the vehicle.

The SINS/GPS integrated system adopts the ENU (East North Up) coordinate system as its navigation frame. Its state vector is given by

where and represent the errors in attitude and velocity, and , , and represent the position error, gyroscope’s random drift, and accelerometer’s constant bias.

The system state equation is represented by

where denotes the nonlinear system function, represents the system noise, which is defined by the Gaussian white noises of the gyro and accelerometer, and represents the system noise’s coefficient matrix.

The measurement equation is represented by

where is the measurement matrix, is the measurement noise, and represents the quadratic term and higher terms in the pseudorange difference of GPS satellites.

Experiments were conducted by adopting CKF and RWCKF in the SINS/GPS integrated navigation system to observe the vehicle position, where the sampling time was 1000 s. Figures 17 and 18 show the longitude and latitude errors of the vehicle obtained by CKF and RWCKF, respectively. Due to the existence of system error caused by dynamic disturbance during the vehicle travelling, the accuracy of CKF is seriously affected, and its positioning result involves a large deviation. The longitude and latitude errors of CKF are within and . This is because CKF uses arithmetic mean to estimate prediction errors, making the algorithm vulnerable to system error because of the unreasonable weight allocation to cubature points. Contrarily, RWCKF assigns different weights to cubature points for estimating state prediction and measurement prediction together with error covariances, leading to the increased filtering accuracy. Therefore, the positioning accuracy of RWCKF is significantly higher than that of CKF, and the resultant errors in longitude and latitude are within and , respectively.

Figure 17: Longitude errors obtained by CKF and RWCKF.

Figure 18: Latitude errors obtained by CKF and RWCKF.

Table 5 shows the MAEs (mean-absolute errors) and RMSEs for CKF and RWCKF. It can be clearly seen that the MAEs and RMSEs obtained by RWCKF are also smaller than those of CKF.

Table 5: MAEs and RMSEs of CKF and RWCKF.

Method
Error type
Longitude errors
Latitude errors

CKF
MAE (m)
6.2985
4.9886

RMSE (m)
9.3567
8.3168

RWCKF
MAE (m)
3.1482
2.3516

RMSE (m)
4.9667
3.6235

It is evident from the above simulation and experimental results and analyses that RWCKF is capable of restraining system error’s disturbance on system state estimate, resulting in enhanced navigation accuracy comparing to CKF.

5. Conclusions

In this paper, a RWCKF is presented to address the inherent problem of CKF by incorporating random weights in CKF. The presented method dynamically adjusts random weights on cubature points based on prediction errors to improve the accuracy of system state estimation. Randomly weighted estimations are established for predicted state and measurement as well as their covariances. Simulation and experimental results together with comparison analyses show that the proposed RWCKF is capable of inhibiting system error’s disturbance on system state estimate by dynamically modifying cubature point weights according to prediction errors, resulting in improved filtering accuracy comparing to CKF, UKF, SIF, and AUKF. Further, the results of ANEES analysis show that the covariance matrices obtained by RWCKF are much more reliable than those by CKF, UKF, SIF, and AUKF. Future research work will concentrate on extending the proposed RWCKF to the cases with corrupted measurement data.

Appendix

Computation of Cubature Points

The cubature points are calculated by the following two steps.

(1) Filter Recursion Formula

Since the Gaussian distribution can be represented by mean and variance, the nonlinear system state can be estimated by the Gaussian filter.

For the nonlinear system described by (1), the Gaussian filter can be described as the following structure

where

In (A.2)~(A.6), the calculation of the mathematical expectation involves the multidimensional integral

where is an arbitrary function and is the integral area.

To compute the cubature points, we need to calculate the above integral (A.7), which is based on the Spherical-Radial cubature criterion. In the following, we shall study the Spherical-Radial cubature criterion.

(2) Spherical-Radial Cubature Criterion

Let . In the spherical radial coordinate system, the integral expressed by (A.7) can be written as

where is a -dimensional unit sphere and is an element on .

Integral can be decomposed into the spherical integral and radial integral

A fully symmetrical spherical cubature criterion can be expressed as

where is the weighted value, is located at the intersection between the -dimensional unit sphere and its coordinate axis, , and represents the column of set .

where .

After the spherical radial transformation, the radial integral can be expressed as

Equation (A.12) is the generalized Lagrangian formula. According to the first power Lagrange rule, when or , through (A.13) we can get the accurate integral value

According to the generalized Gauss-Lagrangian rule, (A.13) can be approximated by

From (A.10) and (A.14), the spherical radial cubature criterion can be obtained as

For the cubic power spherical radial cubature criterion, we have . From (A.13) and (A.14), can be obtained. Thus, (A.15) can be rewritten as

For the standard Gaussian distribution, we have

From (A.7) and (A.16), we have

where are the cubature points, and

Then, we have

From (A.18) and (A.19), we readily have the cubature point set . In CKF, the Gaussian integral is calculated according to the Spherical-Radial cubature criterion by selecting 2 equal-weighted cubature points. Subsequently, the system state at the next time point is predicted via the new cubature points transformed by the nonlinear system state equation.

Data Availability

All the data used in the simulation experiments have been described in detail in the manuscript, and all researchers can easily reproduce these experiments based on these descriptions.

Conflicts of Interest

The authors do not have any conflict of interest involved in this manuscript.

Authors’ Contributions

Dr. Hua Zong completed the theoretical design and analysis of the proposed method in the manuscript and drafted the manuscript. Dr. Zhaohui Gao conducted the simulations for the performance verification in the manuscript. Dr. Wenhui Wei conducted the experiments for the performance verification in the manuscript. Prof. Yongmin Zhong designed the simulation and experiment schemes for the performance evaluation. Dr. Chengfan Gu discussed and analyzed the simulation and experimental results and improved the quality and readiness of the manuscript.

References

Z. Hu and B. Gallacher, “Extended Kalman filtering based parameter estimation and drift compensation for a MEMS rate integrating gyroscope,” Sensors and Actuators A: Physical, vol. 250, pp. 96–105, 2016.

T. Lefebvre, H. Bruyninckx, and J. De Schutter, “Kalman filters for non-linear systems: a comparison of performance,” International Journal of Control, vol. 77, no. 7, pp. 639–653, 2004.

M. Gautier and P. Poignet, “Extended Kalman filtering and weighted least squares dynamic identification of robot,” Control Engineering Practice, vol. 9, no. 12, pp. 1361–1372, 2001.

G. L. Plett, “Extended Kalman filtering for battery management systems of LiPB-based HEV battery packs: part 3. state and parameter estimation,” Journal of Power Sources, vol. 134, no. 2, pp. 277–292, 2004.

N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to nonlinear/non-Gaussian Bayesian state estimation,” IEE Proceedings F Radar and Signal Processing, vol. 140, no. 2, pp. 107–113, 1993.

H. E. Soken and C. Hajiyev, “Adaptive fading UKF with Q-adaptation: application to picosatellite attitude estimation,” Journal of Aerospace Engineering, vol. 26, no. 3, pp. 628–636, 2011.

S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004.

Q. Wang, D. Xiao, and W. Pang, “The research and application of adaptive robust UKF on GPS/SINS integrated system,” Journal of Convergence Information Technology, vol. 8, no. 6, pp. 1169–1177, 2013.

G.-G. Hu, S.-S. Gao, Y.-M. Zhong, and B.-B. Gao, “Stochastic stability of the derivative unscented Kalman filter,” Chinese Physics B, vol. 24, no. 7, article 070202, 2015.

B. Gao, S. Gao, G. Hu, Y. Zhong, and C. Gu, “Maximum likelihood principle and moving horizon estimation based adaptive unscented Kalman filter,” Aerospace Science and Technology, vol. 73, pp. 184–196, 2018.

B. Gao, G. Hu, S. Gao, Y. Zhong, and C. Gu, “Multi-sensor optimal data fusion based on the adaptive fading unscented Kalman filter,” Sensors, vol. 18, no. 2, p. 488, 2018.

G. Hu, W. Wang, Y. Zhong, B. Gao, and C. Gu, “A new direct filtering approach to INS/GNSS integration,” Aerospace Science and Technology, vol. 77, pp. 755–764, 2018.

J. Dunik, O. Straka, and M. Simandl, “Stochastic integration filter,” IEEE Transactions on Automatic Control, vol. 58, no. 6, pp. 1561–1566, 2013.

J. B. Rawlings and B. R. Bakshi, “Particle filtering and moving horizon estimation,” Computers & Chemical Engineering, vol. 30, no. 10-12, pp. 1529–1541, 2006.

N. Yang, W. F. Tian, Z. H. Jin, and C. B. Zhang, “Particle filter for sensor fusion in a land vehicle navigation system,” Measurement Science and Technology, vol. 16, no. 3, pp. 677–681, 2005.

M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, 2002.

D. Watzenig, M. Brandner, and G. Steiner, “A particle filter approach for tomographic imaging based on different state-space representations,” Measurement Science and Technology, vol. 18, no. 1, pp. 30–40, 2007.

N. Yang, W. Tian, and Z. Jin, “An interacting multiple model particle filter for manoeuvring target location,” Measurement Science and Technology, vol. 17, no. 6, pp. 1307–1311, 2006.

B. Zhang, W. Tian, and Z. Jin, “Head tracking based on the integration of two different particle filters,” Measurement Science and Technology, vol. 17, no. 11, pp. 2877–2883, 2006.

B. Zhang, M. Chen, D. Zhou, and Z. Li, “Particle-filter-based estimation and prediction of chaotic states,” Chaos, Solitons & Fractals, vol. 32, no. 4, pp. 1491–1498, 2007.

J. Carpenter, P. Clifford, and P. Fearnhead, “Improved particle filter for nonlinear problems,” IEE Proceedings - Radar, Sonar and Navigation, vol. 146, no. 1, pp. 2–7, 1999.

W. Wei, S. Gao, Y. Zhong, C. Gu, and G. Hu, “Adaptive square-root unscented particle filtering algorithm for dynamic navigation,” Sensors, vol. 18, no. 7, p. 2337, 2018.

I. Arasaratnam and S. Haykin, “Cubature Kalman filters,” IEEE Transactions on Automatic Control, vol. 54, no. 6, pp. 1254–1269, 2009.

I. Arasaratnam, S. Haykin, and T. R. Hurd, “Cubature Kalman filtering for continuous-discrete systems: theory and simulations,” IEEE Transactions on Signal Processing, vol. 58, no. 10, pp. 4977–4993, 2010.

I. Arasaratnam, “Cubature Kalman Filtering: Theory & Applications [Ph.D. thesis],” Tech. Rep., ECE Department, McMaster University, Ontario, Canada, 2009.

K. P. B. Chandra, D. W. Gu, and I. Postlethwaite, “Cubature Kalman filter based localization and mapping,” IFAC Proceedings Volumes, vol. 44, no. 1, pp. 2121–2125, 2011.

F. Sun and L. Tang, “Estimation precision comparison of cubature Kalman filter and unscented Kalman filter,” Control and Decision, vol. 28, no. 2, pp. 303–308, 2013.

Y. Shi, X. Tang, X. Feng, D. Bian, and X. Zhou, “Hybrid adaptive cubature Kalman filter with unknown variance of measurement noise,” Sensors, vol. 18, no. 12, p. 4335, 2018.

B. Gao, S. Gao, Y. Zhong, and C. Gu, “Random weighting estimation of sampling distributions via importance resampling,” Communications in Statistics - Simulation and Computation, vol. 46, no. 1, pp. 640–654, 2016.

S. Gao, G. Hu, and Y. Zhong, “Windowing and random weighting-based adaptive unscented Kalman filter,” International Journal of Adaptive Control and Signal Processing, vol. 29, no. 2, pp. 201–223, 2015.

Z. Zheng, “Random weighting method,” Acta Mathematicae Applicatae Sinica, vol. 10, no. 2, pp. 247–253, 1987.

Z. Gao, D. Mu, Y. Zhong, C. Gu, and C. Ren, “Adaptively random weighted cubature Kalman filter for nonlinear systems,” Mathematical Problems in Engineering, vol. 2019, Article ID 4160847, 13 pages, 2019.

M. He and J. He, “A real-time H∞ cubature Kalman filter based on SVD and its application to a small unmanned helicopter,” Optik, vol. 140, pp. 96–103, 2017.

X. Liu, H. Qu, J. Zhao, and P. Yue, “Maximum correntropy square-root cubature Kalman filter with application to SINS/GPS integrated systems,” ISA Transactions, vol. 80, pp. 195–202, 2018.

Y. Zhong, S. Gao, W. Wei, C. Gu, and A. Subic, “Random weighting estimation of kinematic model error for dynamic navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 51, no. 3, pp. 2248–2259, 2015.