# Randomly Weighted CKF for Multisensor Integrated Systems

Authors: Hua Zong; Zhaohui Gao; Wenhui Wei; Yongmin Zhong; Chengfan Gu
Journal: Journal of Sensors
 (2019)

---

## 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.


---

## Body

## 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 2n+1 (n 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 2n+1 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 κ<0, the weight w0<0. 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(1)xk=fxk−1+Wk−1,zk=hxk+Vk,where xk∈Rn is the system state vector at time point k, zk∈Rm is the measurement vector, Wk∈Rn and Vk∈Rm are the additive process noise and measurement noise, and f⋅ and h⋅ are the nonlinear functions describing the process and measurement models.Assuming process noiseWk and measurement noise Vk are the independent and time-varying Gaussian white noises with nonzero means, satisfying
(2)EWk=qk,covWk,Wj=Qkδkj,EVk=rk,covVk,Vj=Rkδkj,covWk,Vj=0,where both Qk≥0 and Rk>0 are a symmetric matrix, and δkj is the Kronecker‐δ function.The CKF algorithm involves the following steps:Step 1.
Set the initial values for the system state estimation and error covariance(3)x^0=Ex0,P0=Ex0−x^0x0−x^0T.Step 2.
Time update and computing cubature points.
Suppose the posterior density functionpxk−1 at time point k−1 obeys the normal distribution N⋅, i.e., pxk−1=Nx^k−1k−1,Pk−1k−1. Then, the error covariance Pk−1k−1 at time point k−1 can be expressed as
(4)Pk−1k−1=Ak−1k−1Ak−1k−1T.Accordingly, the cubature points can be obtained by(5)xi,k−1k−1=Ak−1k−1ζi+x^k−1k−1,where xi,k−1k−1 (i=1,2,⋯,m) represents the system state vector at time point k−1 calculated from the ith cubature point, Ak−1k−1 denotes the matrix obtained by the Cholesky decomposition [23, 25] at time point k−1, and ζi=m/21i denotes the ith cubature point, in which m represents the quantity of the cubature points and is commonly chosen as large as the system state dimension by a factor of two, 1∈Rn represents a set of symmetric points in the system state space, and 1i is the ith point of 1. A set of cubature points ζi,ηi can be simply determined as ζi=m/21i and ηi=1/m,i=1,2,⋯,m.m=2n, where n is the system state dimension (see Appendix A for computation of the cubature points).Then, theith cubature point from time point k−1 to k is estimated by
(6)xi,k∣k−1∗=fxi,k−1k−1.Accordingly, the state prediction and error covariance from time pointk−1 to k at the ith cubature point is described by
(7)x^k∣k−1=1m∑i=1mxi,k∣k−1∗,(8)Pk∣k−1=1m∑i=1mxi,k∣k−1∗xi,k∣k−1∗T−x^k∣k−1x^Tk∣k−1+Qk−1.Step 3.
Measurement update.
By Cholesky decomposition,Pkk−1 can be expressed by
(9)Pk∣k−1=Ak∣k−1Ak∣k−1T.Calculate the system state vector from time pointk−1 to k at the ith cubature point
(10)xi,k∣k−1=Ak∣k−1ζi+x^k∣k−1.Then, the measurement vector from time pointk−1 to k at the ith cubature point is given by
(11)zi∗,k∣k−1=hxi,k∣k−1.Accordingly, the measurement prediction together with its associated autocovariance and cross-covariance matrices from time pointk−1 to k at the ith cubature point is represented by
(12)z^k∣k−1=1m∑i=1mzi∗,k∣k−1,(13)Pzz,k∣k−1=1m∑i=1mzi∗,k∣k−1zi∗T,k∣k−1−z^k∣k−1z^Tk∣k−1+Rk,(14)Pxz,k∣k−1=1m∑i=1mxi,k∣k−1zi∗T,k∣k−1−x^k∣k−1z^k∣k−1T.Calculate the filtering gain at time pointk
(15)Kk=Pxz,k∣k−1Pzz,k∣k−1−1.Calculate the estimated state at time pointk
(16)x^kk=x^k∣k−1+Kkzk−z^k∣k−1.According to the mean-square error of minimum variance estimation, we have(17)Pkk=Pk∣k−1−Pxz,k∣k−1Pzz,k∣k−1−1Pxz,k∣k−1,where both Pzz,k∣k−1 and Pxz,k∣k−1 are a symmetric matrix, i.e., Pzz,k∣k−1=Pzz,k∣k−1T and Pxz,k∣k−1=Pxz,k∣k−1T.The covariance matrix of the state error at time pointk is represented by
(18)Pkk=Pk∣k−1−Pxz,k∣k−1Pzz,k∣k−1−1Pzz,k∣k−1Pzz,k∣k−1−1Pxz,k∣k−1=Pk∣k−1−KkPzz,k∣k−1KkT.If the nonlinear system model involves error, (1) will become
(19)xk=fxk−1+Δxk+Wk−1,zk=hxk+Δzk+Vk,where Δxk and Δzk represent process model error and measurement model error, respectively.Accordingly, the predicted statex^k∣k−1 and predicted measurement z^k∣k−1 will become
(20)x^k∣k−1=1m∑i=1mxi,k∣k−1∗+Δxk,(21)z^k∣k−1=1m∑i=1mzi∗,k∣k−1+Δzk.It is evident from (19) to (21) that if the nonlinear system model involves error, the state prediction x^k∣k−1 given by (7) and the measurement prediction z^k∣k−1 given by (12) will become biased and further make the filtering gain Kk 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 weight1/m. 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 thatY1,Y2,⋯,Yn are the random variables which are independent of each other and identically distributed as well as share a distribution function Ψy, and the associated empirical distribution function of these variables is represented as
(22)Ψny=1n∑j=1nBYj≤y,where BYj≤y denotes the indicative function.Accordingly, the randomly weighted estimate ofΨny is expressed by
(23)Hny=∑j=1nβjBYj≤y,where βjj=1,2,⋯,n is the randomly weighted factor, which is subject to Dirichlet distribution D1,1,⋯,1, that is, ∑j=1nβj=1 and the united probability density function of β1,β2,⋯,βm−1 is fβ1,β2,⋯,βm−1=Γm, where Γ denotes the Gamma function, β1,β2,⋯,βn−1∈Dn−1 and Dn−1=β1,β2,⋯,βn−1:βj≥0,j=1,2,⋯,n−1,∑j=1n−1β1≤1.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 D1,1,⋯,1 [30, 31]. A straightforward way is as follows.Generate an independent and identically distributed sample sequence of the uniform distributionU0,1 and assume this sample sequence is U1,U2,⋯,Un−1. Let U1,U2,⋯,Un−1 be the order statistic of U1,U2,⋯,Un−1, where U0=0 and Un=1. Then, βj=Uj−Uj−1,j=1,2,⋯,n, and the joint density function for βj=Uj−Uj−1 is the Dirichlet distribution D1,1,⋯,1.


### 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
(24)x^kk−1∗=∑i=1mβixi,k∣k−1∗,where βii=1,2,⋯,m represents the randomly weighted factor.In a similar way, from (8), the randomly weighted estimate of predicted state’s covariance matrix is expressed by
(25)Pkk−1∗=∑i=1mβixi,k∣k−1∗xi,k∣k−1∗T−x^∗k∣k−1x^k∣k−1∗T+Qk−1.Measurement Prediction. From (12), the randomly weighted estimate of the predicted measurement is obtained as
(26)z^kk−1∗=∑i=1mβizi∗,k∣k−1.From (13), the randomly weighted estimate of predicted measurement’s autocovariance matrix is represented by
(27)Pzz,kk−1∗=∑i=1mβizi,k∣k−1∗zi,k∣k−1∗T−z^∗k∣k−1z^k∣k−1∗T+Rk.By (14), the randomly weighted estimate of predicted measurement’s cross-covariance matrix is obtained as
(28)Pxz,kk−1∗=∑i=1mβixi,k∣k−1∗zi,k∣k−1∗T−x^∗k∣k−1z^k∣k−1∗T.Update of System State. Calculate the filtering gain at time point k
(29)Kk∗=Pxz∗,k∣k−1Pzz∗,k∣k−1−1.Compute the state estimation(30)x^kk=x^kk−1∗+Kk∗zk−z^kk−1∗,where x^∗k∣k−1 is the predicted state which is expressed by
(31)x^kk−1∗=Efxk−1=∫Rnfxk−1pxk−1dxk−1,where pxk−1 represents the posterior density function at time point k−1.Compute the estimation of state error covariance(32)Pkk∗=Pkk−1∗−Kk∗Pzz,kk−1∗Kk∗T.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. Figure1 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 x^k∣k−1 and predicted measurement z^k∣k−1 in CKF, the predicted state x^∗k∣k−1 and predicted measurement z^∗k∣k−1 in RWCKF can be readily obtained as
(33)x^kk−1∗=∑i=1mβixi,k∣k−1∗+Δxk,z^kk−1∗=∑i=1mβizi∗,k∣k−1+Δzk.In CKF, the residual vectors of the predicted state and measurement are expressed as(34)ek=x^kk−x^kk−1=x^kk−1m∑i=1mxi,k∣k−1∗−Δxk,(35)εk=zk−z^kk−1=zk−1m∑i=1mzi∗,k∣k−1−Δzk.In RWCKF, the residual vectors of the predicted state and measurement are expressed as(36)ek∗=x^kk−x^kk−1∗=x^kk−∑i=1mβixi,k∣k−1∗−Δxk,(37)εk∗=zk−z^kk−1∗=zk−∑i=1mβizi∗,k∣k−1−Δzk.If the nonlinear system model involves error, the CKF residual vectorsek and εk 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 ek∗ and εk∗ by adjusting randomly weighted factors βii=1,2,⋯,m to restrain system model error’s disturbance, leading to increased estimation accuracy.


## 3.1. Randomly Weighted Estimation

Assume thatY1,Y2,⋯,Yn are the random variables which are independent of each other and identically distributed as well as share a distribution function Ψy, and the associated empirical distribution function of these variables is represented as
(22)Ψny=1n∑j=1nBYj≤y,where BYj≤y denotes the indicative function.Accordingly, the randomly weighted estimate ofΨny is expressed by
(23)Hny=∑j=1nβjBYj≤y,where βjj=1,2,⋯,n is the randomly weighted factor, which is subject to Dirichlet distribution D1,1,⋯,1, that is, ∑j=1nβj=1 and the united probability density function of β1,β2,⋯,βm−1 is fβ1,β2,⋯,βm−1=Γm, where Γ denotes the Gamma function, β1,β2,⋯,βn−1∈Dn−1 and Dn−1=β1,β2,⋯,βn−1:βj≥0,j=1,2,⋯,n−1,∑j=1n−1β1≤1.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 D1,1,⋯,1 [30, 31]. A straightforward way is as follows.Generate an independent and identically distributed sample sequence of the uniform distributionU0,1 and assume this sample sequence is U1,U2,⋯,Un−1. Let U1,U2,⋯,Un−1 be the order statistic of U1,U2,⋯,Un−1, where U0=0 and Un=1. Then, βj=Uj−Uj−1,j=1,2,⋯,n, and the joint density function for βj=Uj−Uj−1 is the Dirichlet distribution D1,1,⋯,1.


## 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
(24)x^kk−1∗=∑i=1mβixi,k∣k−1∗,where βii=1,2,⋯,m represents the randomly weighted factor.In a similar way, from (8), the randomly weighted estimate of predicted state’s covariance matrix is expressed by
(25)Pkk−1∗=∑i=1mβixi,k∣k−1∗xi,k∣k−1∗T−x^∗k∣k−1x^k∣k−1∗T+Qk−1.Measurement Prediction. From (12), the randomly weighted estimate of the predicted measurement is obtained as
(26)z^kk−1∗=∑i=1mβizi∗,k∣k−1.From (13), the randomly weighted estimate of predicted measurement’s autocovariance matrix is represented by
(27)Pzz,kk−1∗=∑i=1mβizi,k∣k−1∗zi,k∣k−1∗T−z^∗k∣k−1z^k∣k−1∗T+Rk.By (14), the randomly weighted estimate of predicted measurement’s cross-covariance matrix is obtained as
(28)Pxz,kk−1∗=∑i=1mβixi,k∣k−1∗zi,k∣k−1∗T−x^∗k∣k−1z^k∣k−1∗T.Update of System State. Calculate the filtering gain at time point k
(29)Kk∗=Pxz∗,k∣k−1Pzz∗,k∣k−1−1.Compute the state estimation(30)x^kk=x^kk−1∗+Kk∗zk−z^kk−1∗,where x^∗k∣k−1 is the predicted state which is expressed by
(31)x^kk−1∗=Efxk−1=∫Rnfxk−1pxk−1dxk−1,where pxk−1 represents the posterior density function at time point k−1.Compute the estimation of state error covariance(32)Pkk∗=Pkk−1∗−Kk∗Pzz,kk−1∗Kk∗T.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. Figure1 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 x^k∣k−1 and predicted measurement z^k∣k−1 in CKF, the predicted state x^∗k∣k−1 and predicted measurement z^∗k∣k−1 in RWCKF can be readily obtained as
(33)x^kk−1∗=∑i=1mβixi,k∣k−1∗+Δxk,z^kk−1∗=∑i=1mβizi∗,k∣k−1+Δzk.In CKF, the residual vectors of the predicted state and measurement are expressed as(34)ek=x^kk−x^kk−1=x^kk−1m∑i=1mxi,k∣k−1∗−Δxk,(35)εk=zk−z^kk−1=zk−1m∑i=1mzi∗,k∣k−1−Δzk.In RWCKF, the residual vectors of the predicted state and measurement are expressed as(36)ek∗=x^kk−x^kk−1∗=x^kk−∑i=1mβixi,k∣k−1∗−Δxk,(37)εk∗=zk−z^kk−1∗=zk−∑i=1mβizi∗,k∣k−1−Δzk.If the nonlinear system model involves error, the CKF residual vectorsek and εk 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 ek∗ and εk∗ by adjusting randomly weighted factors βii=1,2,⋯,m 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(38)xk=Φxk−1+Fgxk+Wk,where xk=xk,ẋk,yk,ẏkT denotes the state, (xk, yk) the target position, and (ẋk, ẏk) the target velocity. Wk denotes the process noise and is considered as a Gaussian white noise having zero mean and covariance
(39)Qk=λM33M2200M22M0000M33M2200M22M,where M denotes the time step of the continuous measurement vector, and λ>1 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 λ=2 by test computation.The transition matricesΦ and F can be expressed as
(40)Φ=1M000100001M0001,F=M220M00M220M.g⋅ is the systematic error function, which is expressed as
(41)gxk=−0.5ẋ2k+ẏk21/2ẋkẏk.The measurement equation of the target is linear and can be described as(42)zk=Hxk+Vk,where
(43)zk=z1kz2kT,H=10000010.Vk is the measurement noise and is considered as a Gaussian white noise having zero mean and covariance
(44)Rk=1001.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 covariancediag0.0052,0.022,0.0052,0.022.


#### 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. Figures2 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 Figure2, the error curves of both CKF and UKF involve large oscillations, and their position errors are approximately [−1.5m, 2.1m]. 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 [−0.1m, 0.1m], 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 Figure4 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 Table1. 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(45)xt=δPxδPyδPzδVxδVyδVzφxφyφzεxεyεz∇x∇y∇zT,where (δPx,δPy,δPz), (δVx,δVy,δVz), and (φx,φy,φz) denote the position error, velocity error, and attitude error, (εx,εy,εz) represents gyro’s constant drift, and (∇x,∇y,∇z) 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
(46)ẋt=lx,t+Wt,where lx,t denotes the nonlinear system function, which is described by
(47)lx,t=δVi−Fiϕ+NiδP+Cbi∇Cbiε03×103×1,where Fi denotes the inverse matrix of the specific force fi, and Cbi denotes the transfer matrix from the body to the inertial coordinate system.
(48)Ni=μr3−1+3x2r23xyr23xzr23xyr2−1+3y2r23yzr23xzr23yzr2−1+3z2r2,where r=x2+y2+z21/2, and μ is the gravitational constant.Wt denotes the system noise and is represented by
(49)Wt=0δg0wgwa,where δg is the gravitational vector error, wg denotes the measurement error resulted from gyro’s constant drift, and wa 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(50)zt=PSPXVSVX=Hxt+Vt,where PS and VS denote the position and velocity output by SINS, PX and VX denote the position and velocity output by XNAV, H=I6×606×9 is the measurement matrix, and Vt is the measurement noise, which is considered as a Gaussian white noise with zero mean and covariance Rk>0.


#### 4.2.2. Simulation and Analysis

The rocket is supposed to orbit the earth with the parameters provided in Table2. 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 sFigure 6
Rocket flight path.During the simulation, the gyro drift was0.1°/h, the gyro random walk was 0.01°/h, the zero bias of the accelerometer was 10−3g, and the random drift of the accelerometer was 10−4g/s. The SINS initial error of alignment was 0′. 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 0.4∘,0.4∘,0.4∘, respectively. The covariance of the initial state error was
(51)P0=diag152,152,152,0.12,0.12,0.12,0.42,0.42,0.42,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2.The process noise covariance and measurement noise covariance were(52)Qk=diag01×3,0.0012,0.0012,0.0.0012,01×3,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2,Rk=diag252,252,252,0.32,0.32,0.32.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., (3m2, 3m2, 3m2) and (0.04m/s2, 0.04m/s2, 0.04m/s2), 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(53)δP=δPx2+δPy2+δPz2,δV=δVx2+δVy2+δVz2.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. Figures7–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 Figures7 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 Figures9 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 Figure11, 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.043It can be seen from Figures12 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 Figure14, 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
−30°C~+60°C
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°46′05.89″, N34°01′41.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°49′04.61″, N34°03′10.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 vectorxt is given by
(54)xt=ϕE,ϕN,ϕU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,∇x,∇y,∇zT,where ϕE,ϕN,ϕU and δVE,δVN,δVU represent the errors in attitude and velocity, and δL,δλ,δh, εx,εy,εx, and ∇x,∇y,∇z represent the position error, gyroscope’s random drift, and accelerometer’s constant bias.The system state equation is represented by(55)ẋt=φx,t+Λtwt,where φx,t denotes the nonlinear system function, wt=wgx,wgy,wgz,wax,way,wazTrepresents the system noise, which is defined by the Gaussian white noises of the gyro and accelerometer, and Λt represents the system noise’s coefficient matrix.The measurement equation is represented by(56)zk=Hkxk+Mk+vk,where Hk is the measurement matrix, vk is the measurement noise, and Mk 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. Figures17 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 −18.3017m,18.9691m and −19.1417m,13.7780m. 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 −9.6504m,13.5516m and −12.8917m,10.1254m, respectively.Figure 17
Longitude errors obtained by CKF and RWCKF.Figure 18
Latitude errors obtained by CKF and RWCKF.Table5 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.6235It 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.


## 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(38)xk=Φxk−1+Fgxk+Wk,where xk=xk,ẋk,yk,ẏkT denotes the state, (xk, yk) the target position, and (ẋk, ẏk) the target velocity. Wk denotes the process noise and is considered as a Gaussian white noise having zero mean and covariance
(39)Qk=λM33M2200M22M0000M33M2200M22M,where M denotes the time step of the continuous measurement vector, and λ>1 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 λ=2 by test computation.The transition matricesΦ and F can be expressed as
(40)Φ=1M000100001M0001,F=M220M00M220M.g⋅ is the systematic error function, which is expressed as
(41)gxk=−0.5ẋ2k+ẏk21/2ẋkẏk.The measurement equation of the target is linear and can be described as(42)zk=Hxk+Vk,where
(43)zk=z1kz2kT,H=10000010.Vk is the measurement noise and is considered as a Gaussian white noise having zero mean and covariance
(44)Rk=1001.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 covariancediag0.0052,0.022,0.0052,0.022.


### 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. Figures2 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 Figure2, the error curves of both CKF and UKF involve large oscillations, and their position errors are approximately [−1.5m, 2.1m]. 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 [−0.1m, 0.1m], 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 Figure4 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 Table1. 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.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. Figures2 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 Figure2, the error curves of both CKF and UKF involve large oscillations, and their position errors are approximately [−1.5m, 2.1m]. 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 [−0.1m, 0.1m], 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 Figure4 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 Table1. 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(45)xt=δPxδPyδPzδVxδVyδVzφxφyφzεxεyεz∇x∇y∇zT,where (δPx,δPy,δPz), (δVx,δVy,δVz), and (φx,φy,φz) denote the position error, velocity error, and attitude error, (εx,εy,εz) represents gyro’s constant drift, and (∇x,∇y,∇z) 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
(46)ẋt=lx,t+Wt,where lx,t denotes the nonlinear system function, which is described by
(47)lx,t=δVi−Fiϕ+NiδP+Cbi∇Cbiε03×103×1,where Fi denotes the inverse matrix of the specific force fi, and Cbi denotes the transfer matrix from the body to the inertial coordinate system.
(48)Ni=μr3−1+3x2r23xyr23xzr23xyr2−1+3y2r23yzr23xzr23yzr2−1+3z2r2,where r=x2+y2+z21/2, and μ is the gravitational constant.Wt denotes the system noise and is represented by
(49)Wt=0δg0wgwa,where δg is the gravitational vector error, wg denotes the measurement error resulted from gyro’s constant drift, and wa 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(50)zt=PSPXVSVX=Hxt+Vt,where PS and VS denote the position and velocity output by SINS, PX and VX denote the position and velocity output by XNAV, H=I6×606×9 is the measurement matrix, and Vt is the measurement noise, which is considered as a Gaussian white noise with zero mean and covariance Rk>0.


### 4.2.2. Simulation and Analysis

The rocket is supposed to orbit the earth with the parameters provided in Table2. 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 sFigure 6
Rocket flight path.During the simulation, the gyro drift was0.1°/h, the gyro random walk was 0.01°/h, the zero bias of the accelerometer was 10−3g, and the random drift of the accelerometer was 10−4g/s. The SINS initial error of alignment was 0′. 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 0.4∘,0.4∘,0.4∘, respectively. The covariance of the initial state error was
(51)P0=diag152,152,152,0.12,0.12,0.12,0.42,0.42,0.42,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2.The process noise covariance and measurement noise covariance were(52)Qk=diag01×3,0.0012,0.0012,0.0.0012,01×3,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2,Rk=diag252,252,252,0.32,0.32,0.32.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., (3m2, 3m2, 3m2) and (0.04m/s2, 0.04m/s2, 0.04m/s2), 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(53)δP=δPx2+δPy2+δPz2,δV=δVx2+δVy2+δVz2.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. Figures7–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 Figures7 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 Figures9 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 Figure11, 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.043It can be seen from Figures12 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.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(45)xt=δPxδPyδPzδVxδVyδVzφxφyφzεxεyεz∇x∇y∇zT,where (δPx,δPy,δPz), (δVx,δVy,δVz), and (φx,φy,φz) denote the position error, velocity error, and attitude error, (εx,εy,εz) represents gyro’s constant drift, and (∇x,∇y,∇z) 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
(46)ẋt=lx,t+Wt,where lx,t denotes the nonlinear system function, which is described by
(47)lx,t=δVi−Fiϕ+NiδP+Cbi∇Cbiε03×103×1,where Fi denotes the inverse matrix of the specific force fi, and Cbi denotes the transfer matrix from the body to the inertial coordinate system.
(48)Ni=μr3−1+3x2r23xyr23xzr23xyr2−1+3y2r23yzr23xzr23yzr2−1+3z2r2,where r=x2+y2+z21/2, and μ is the gravitational constant.Wt denotes the system noise and is represented by
(49)Wt=0δg0wgwa,where δg is the gravitational vector error, wg denotes the measurement error resulted from gyro’s constant drift, and wa 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(50)zt=PSPXVSVX=Hxt+Vt,where PS and VS denote the position and velocity output by SINS, PX and VX denote the position and velocity output by XNAV, H=I6×606×9 is the measurement matrix, and Vt is the measurement noise, which is considered as a Gaussian white noise with zero mean and covariance Rk>0.


## 4.2.2. Simulation and Analysis

The rocket is supposed to orbit the earth with the parameters provided in Table2. 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 sFigure 6
Rocket flight path.During the simulation, the gyro drift was0.1°/h, the gyro random walk was 0.01°/h, the zero bias of the accelerometer was 10−3g, and the random drift of the accelerometer was 10−4g/s. The SINS initial error of alignment was 0′. 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 0.4∘,0.4∘,0.4∘, respectively. The covariance of the initial state error was
(51)P0=diag152,152,152,0.12,0.12,0.12,0.42,0.42,0.42,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2.The process noise covariance and measurement noise covariance were(52)Qk=diag01×3,0.0012,0.0012,0.0.0012,01×3,0.12,0.12,0.12,10−3g2,10−3g2,10−3g2,Rk=diag252,252,252,0.32,0.32,0.32.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., (3m2, 3m2, 3m2) and (0.04m/s2, 0.04m/s2, 0.04m/s2), 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(53)δP=δPx2+δPy2+δPz2,δV=δVx2+δVy2+δVz2.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. Figures7–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 Figures7 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 Figures9 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 Figure11, 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.043It can be seen from Figures12 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 Figure14, 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
−30°C~+60°C
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°46′05.89″, N34°01′41.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°49′04.61″, N34°03′10.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 vectorxt is given by
(54)xt=ϕE,ϕN,ϕU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,∇x,∇y,∇zT,where ϕE,ϕN,ϕU and δVE,δVN,δVU represent the errors in attitude and velocity, and δL,δλ,δh, εx,εy,εx, and ∇x,∇y,∇z represent the position error, gyroscope’s random drift, and accelerometer’s constant bias.The system state equation is represented by(55)ẋt=φx,t+Λtwt,where φx,t denotes the nonlinear system function, wt=wgx,wgy,wgz,wax,way,wazTrepresents the system noise, which is defined by the Gaussian white noises of the gyro and accelerometer, and Λt represents the system noise’s coefficient matrix.The measurement equation is represented by(56)zk=Hkxk+Mk+vk,where Hk is the measurement matrix, vk is the measurement noise, and Mk 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. Figures17 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 −18.3017m,18.9691m and −19.1417m,13.7780m. 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 −9.6504m,13.5516m and −12.8917m,10.1254m, respectively.Figure 17
Longitude errors obtained by CKF and RWCKF.Figure 18
Latitude errors obtained by CKF and RWCKF.Table5 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.6235It 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.



---

*Source: 1216838-2019-11-11.xml*