# Adaptively Random Weighted Cubature Kalman Filter for Nonlinear Systems

Authors: Zhaohui Gao; Dejun Mu; Yongmin Zhong; Chengfan Gu; Chengcai Ren
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

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


---

## Body

## 1. Introduction

Since navigation systems generally involve nonlinear characteristics, nonlinear filtering is a popular research topic in navigation and positioning. So far, various nonlinear filtering methods have been studied for spacecraft navigation. The extended Kalman filter (EKF) is a traditional nonlinear filtering method based on the first-order Taylor term of nonlinear functions [1, 2]. It maintains the computational efficiency of the linear Kalman filter. However, since it neglects higher-order terms of the nonlinear system model, EKF suffers from the degradation of estimation accuracy [3, 4]. The unscented Kalman filter (UKF) conducts nonlinear state estimation in the context of control theory, where a finite number of sigma points are used to propagate the probability of state distribution through nonlinear system dynamics [5–8]. In addition to the improved accuracy comparing to EKF, UKF has the merits such as simplicity in realization, high filtering precision, and good convergence [9–12]. However, UKF requires the a priori statistical characteristics of system noise to be precisely known. In practical applications, due to uncertainties in the dynamic environment, it is difficult to accurately describe noise statistics, leading to biased or even divergent filtering solutions [13–15].In the recent ten years, the cubature Kalman filter (CKF) has received a great attention in nonlinear filtering [16, 17]. Based on the cubature rules, this method adopts a set of cubature points evaluated via nonlinear functions to approximate the Gaussian integration [18, 19]. Similar to UKF, CKF does not require the linearization of the nonlinear model and the calculation of Jacobian matrix, thus overcoming the problems caused by linear truncation such as poor positioning accuracy and error divergence. Different from UKF, CKF has a stronger nonlinearity due to the use of cubature points. However, CKF requires exact information on the statistical characteristics of system noise [20]. If the statistical characteristics of system noise are not known exactly, the CKF filtering solution will be deteriorated.The random weighting is a statistical method with high estimation accuracy and low computational burden [21, 22]. This method can handle the calculation of large sample size without requiring the accurate distributions of model parameters. It has been used to solve many problems, for example, multisensor data fusion [23–25], parameter estimation [26], M-test in linear models [27], analysis of asymptotic properties of function distribution [28], and dynamic navigation and positioning [29]. However, there has been very limited research on utilizing the random weighting method with CKF for spacecraft navigation.This paper presents a new adaptive random weighting cubature Kalman filter (ARWCKF) by adopting the concept of random weighting to address the limitation that the CKF performance is sensitive to system noise. This method constructs random weighting estimations for system noise statistics, as well as predicted state and measurement vectors and their associated covariances. Based on the random weighting estimations, it adaptively adjusts the weights of cubature points to inhibit the disturbances of system noises on state estimation, leading to improved filtering robustness. Simulations and comparisons with CKF have been conducted to evaluate the performance of the proposed ARWCKF.


## 2. Concept of Random Weighting Methods

DenoteX1,X2,…,Xn as a series of random variables in independent and identical distribution, and the common distribution function as F(x). Then, the corresponding empirical distribution function can be defined as(1)Fnx=1n∑i=1nIXi≤xwhere I(Xi<x) is the indicator function.The random weighted estimation ofFn(x) can be defined as(2)Hnx=∑i=1nλiIXi<xwhere (λ1,λ2,…,λn) represents the random weighted vector subject to Dirichlet distribution D(1,1,…,1), i.e., ∑i=1nλi=1, and the joint density function of (λ1,λ2,…,λn-1) is f(λ1,λ2,…,λn-1)=Γ(n) with (λ1,λ2,…,λn-1)∈Dn-1 and Dn-1=λ1,λ2,…,λn-1:λk≥0,k=1,…,n-1,∑k=1n-1λi≤1.


## 3. Nonlinear System Models

Consider the following nonlinear discrete-time system:(3)xk+1=fkxk+Wkzk=hkxk+Vkwhere xk∈Rn is the state vector at epoch k, zk∈Rm is the measurement vector, Wk∈Rn is the system process noise, Vk∈Rm is the measurement noise, Wk and Vk are the Gaussian white noises, f(·) is the nonlinear system function, and h(·) is the nonlinear measurement function.System process noiseWk and measurement noise Vk are assumed to be the uncorrelated Gaussian white noises of constant statistical properties; i.e.,(4)EWk=qEVk=rcovWk,WjT=EWk-qWj-qT=QδkjcovVk,VjT=EVk-rVj-rT=RδkjcovWk,VjT=0where cov· is the covariance function, Q≥0 and R>0 are the noise intensity matrices, and δkj is the Kronecker-δ function.Let(5)Wk=q+μkVk=r+ηk⇓μk=Wk-qηk=Vk-rSubstituting (5) into (3), the nonlinear system model can be expressed as(6)xk+1=fkxk+q+μkzk=hkxk+r+ηkwhere μk and ηk are the Gaussian white noises with zero means and variances Q and R, respectively.


## 4. Analysis of Cubature Kalman Filter

In order to improve the filtering performance of CKF, let us briefly review the filtering procedure of CKF, which is given as follows.(i) Initialize State Estimate x ^ 0 and Its Associated Error Covariance P 0 (7) x ^ 0 = E x 0 P 0 = c o v x 0 , x 0 T = E x 0 - x ^ 0 x 0 - x ^ 0 Twhere x0 obeys the Gaussian distribution.(ii) Calculation of Cubature Points. Assume the posterior density function at epoch k-1 is pxk-1=Nx^k-1∣k-1,Pk-1∣k-1. By Cholesky decomposition, the error covariance Pk-1∣k-1 at epoch k-1 can be written as Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T, where Sk-1∣k-1 is a diagonal matrix at epoch k-1. The cubature points can be calculated by xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1∣k-1, where xi,k-1∣k-1(i=1,2,…,m) is the system state of the ith cubature point at epoch k-1.Cubature points setξi,ωi can be represented as(8)ξi=m21iωi=1m,i=1,…,m.m=2nwhere m is the number of the cubature points and is set to twice of the state dimension, 1 is a set of points, and 1i is the i-th point of 1.(iii) State Prediction. The estimated state of the ith cubature point from epoch k-1 to k is described as(9)xi,k∣k-1∗=fxi,k-1∣k-1Based on (9), the predicted state of theith cubature point from epoch k-1 to k is(10)x^k∣k-1=1m∑i=1mxi,k∣k-1∗and its covariance is Pk∣k-1=1/m∑i=1mxi,k∣k-1∗xi,k∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.Let(11)x^k+1∣k=fkxkxk←x^k+q=E^fkxk∣zk+qand(12)ρk=fkxk-fkxkxk←x^k=fkxk-E^fkxk∣zkwhere fkxkxk←x^k is the posterior mean of the state estimation x^k, which is propagated by the nonlinear system function fk·.Then(13)Pk+1∣k=EρkρkT+Q(iv) Observation Update. The estimated observation of the ith cubature point from epoch k-1 to k is represented by(14)zi∗,k∣k-1=hxi,k∣k-1Based on (14), the predicted observation of the ith cubature point from epoch k-1 to epoch k can be represented by(15)z^k∣k-1=1m∑i=1mzi∗,k∣k-1and its autocovariance and cross-covariance matrices are Pzz,k∣k-1=1/m∑i=1mzi∗,k∣k-1zi∗T,k∣k-1-z^k∣k-1z^k∣k-1T+Rk and Pxz,k∣k-1=1/m∑i=1mxi,k∣k-1zi∗T,k∣k-1-x^k∣k-1z^k∣k-1T.Let(16)z^k∣k-1=hk+1xk+1xk+1←xk+1∣k+r=E^hk+1xk+1∣zk+rand(17)ζk=hk+1xk+1-hk+1xk+1xk+1←xk+1∣k=hk+1xk+1-E^hk+1xk+1∣zkwhere hk+1xk+1xk+1←xk+1∣k is the posterior mean of the state prediction x^k+1∣k, which is propagated by the nonlinear measurement function hk+1·, and x~k+1∣k=xk+1-x^k+1∣k.Then(18)Pz~k+1=Eζk+1ζk+1T+RPx~k+1z~k+1=Ex~k+1∣kζk+1T(v) State Update (19) x ^ k + 1 = x ^ k + 1 ∣ k + K k + 1 z k + 1 - z ^ k + 1 ∣ k K k + 1 = P x ~ k + 1 z ~ k + 1 P z ~ k + 1 - 1 P k + 1 = P k + 1 ∣ k - K k + 1 P z ~ k + 1 K k + 1 TIt can be seen from (11), (13), (16), and (18) that CKF requires the statistical characteristics of the system and measured noises to be known. However, in engineering practice, it is difficult to accurately know the statistical characteristics of the system and measurement noises, thus degrading the filtering accuracy. In the following, we shall discuss how to adaptively estimate the system noise and measurement noise via the concept of random weighting.


## 5. Adaptive Random Weighting Estimation of Noise Statistics

### 5.1. Estimation of Noise Statistics

Denote the maximum posterior estimates ofq, Q, r, and R by q^, Q^, r^, and R^, respectively, and the posterior estimate of state xjj=0,1,…,k+1  by  x^j∣k+1. Assuming J=pxk+1,q,Q,r,R,zk+1 is a joint probability density function, we have [30](20)J=pxk+1,q,Q,r,R,zk+1=pzk+1∣xk+1,q,Q,r,Rpxk+1∣q,Q,r,Rpq,Q,r,Rwhere pq,Q,r,R can be obtained from prior information, and thus it is considered as a constant.Consider the conditional density function(21)J∗=pxk+1,q,Q,r,R∣zk+1=pxk+1,q,Q,r,R,zk+1pzk+1where xk+1=x0,x1,…,xk+1 and zk+1=z0,z1,…,zk+1.Consequently, the maximum posterior estimatesq^, Q^, r^, and R^ can be obtained by solving for the conditional density function J∗. It is evident that the maximum posterior estimates q^, Q^, r^, and R^ are not related to the denominator pzk+1 in (21). Therefore, the problem of calculating q^, Q^, r^, and R^ can be equivalently obtained by maximizing the following density function pxk+1,q,Q,r,R,zk+1 in (21).According to the multiplication theorem of conditional probability, we can obtain [30](22)pxk+1∣q,Q,r,R=px0∏j=0kpxj+1∣xj,q,Q=12πn/2P01/2exp⁡-12x0-x^0P0-12×∏j=0k12πn/2Q1/2exp⁡-12xj+1-fjxj-qQ-12=12πn/2+nk+1/2P0-1/2Q-k+1/2exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12=C1P0-1/2Q-k+1/2exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12where n is the state dimension, C1=1/2πnk+2/2 is a constant, and A is the determinant of A.Assume that the measurementsz1,z2,…,zk+1 are known and are independent of each other. Then,(23)pzk+1∣xk+1,q,Q,r,R=∏j=0kpzj+1∣xj+1,r,R=∏j=0k12πm/2R1/2exp⁡-12zj+1-hj+1xj+1-rR-12=C2Rk+1/2exp⁡-12∑j=0kzj+1-hj+1xj+1-rR-12where m is the measurement dimension and C2=1/2πmk+1/2 is a constant.Substituting (22) and (23) into (20) yields(24)J=C1C2P0-1/2·Q-k+1/2·R-k+1/2·pq,Q,r,R·exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12+∑j=0kzj+1-hj+1xj+1-rR-12=CQ-k+1/2·R-k+1/2×exp⁡-12∑j=0kxj+1-fjxj-qQ-12+∑j=0kzj+1-hj+1xj+1-rR-12where C=C1C2P0-1/2pq,Q,r,Rexp⁡-1/2x0-x^0P0-12.Taking the logarithm on both sides of (24) yields(25)ln⁡J=-k+12ln⁡Q-k+12ln⁡R-12∑j=0kxj+1-fjxj-qQ-12-∑j=0kzj+1-hj+1xj+1-rR-12+ln⁡CLet(26)∂ln⁡J∂qq=q^k+1=0∂ln⁡J∂QQ=Q^k+1=0The maximum posterior estimates q^ and Q^ can be obtained as (see Appendices A and B for details)(27)q^k+1=1k+1∑j=0kx^j+1∣k+1-fjxjxj←x^j∣k+1Q^k+1=1k+1∑j=0kx^j+1∣k+1-fjxjxj←x^jk+1-qx^j+1∣k+1-fjxjxj←x^j∣k+1-qTwhere fjxjxj←x^j∣k+1 is the posterior mean of state estimate x^j, which is propagated through nonlinear system function fj·.Similarly, let(28)∂ln⁡J∂rr=r^k+1=0∂ln⁡J∂RR=R^k+1=0The maximum posterior estimates r^ and R^ can be obtained as (see Appendices C and D for details)(29)r^k+1=1k+1∑j=0kzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1R^k+1=1k+1∑j=0kzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1-rzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1-rTwhere hj+1xj+1xj+1←x^j+1∣j is the posterior mean of one-step state prediction x^j+1∣j which is propagated through nonlinear measurement function hj+1·.In (27) and (29), replacing the filtering estimations x^j∣k+1 and x^j+1∣k+1 with the estimations x^j and x^j+1 and replacing fjxjxj←x^j∣k+1+q with state prediction x^j+1∣j, we have(30)q^k+1=1k+1∑j=0kx^j+1-E^fjxj∣zkQ^k+1=1k+1∑j=0kx^j+1-E^fjxj∣zk-qx^j+1-E^fjxj∣zk-qT=1k+1∑j=0kx^j+1-x^j+1∣jx^j+1-x^j+1∣jTr^k+1=1k+1∑j=0kzj+1-E^hj+1xj+1∣zkR^k+1=1k+1∑j=0kzj+1-E^hj+1xj+1∣zk-rzj+1-E^hj+1xj+1∣zk-rT=1k+1∑jkzj+1-z^j+1∣jzj+1-z^j+1∣jTwhere fkxkxk←x^k=E^fk(xk)∣zk and hk+1xk+1xk+1←x^k+1∣k=E^hk+1(xk+1)∣zk.Accordingly, the random weighted estimations ofqk+1, Qk+1, rk+1, and Rk+1 can be represented as(31)q^k+1∗=∑j=0kλjx^j+1-E^fjxj∣zkQ^k+1∗=∑j=0kλjx^j+1-E^fjxj∣zk-qx^j+1-E^fjxj∣zk-qT=∑j=0kλjx^j+1-x^j+1∣jx^j+1-x^j+1∣jTr^k+1∗=∑j=0kλjzj+1-E^hj+1xj+1∣zkR^k+1∗=∑j=0kλjzj+1-E^hj+1xj+1∣zk-rzj+1-E^hj+1xj+1∣zk-rT=∑j=0kλjzj+1-z^j+1∣jzj+1-z^j+1∣jTwhere λj (j=1,2,…,k) are the random weighting factors.


### 5.2. Determination of Random Weighting Factors

Suppose the estimation and prediction of the state at epochk-jj=1,2,…,n are x^k-j and x^k-j/k-1-j, respectively. The residual vector of the prediction is assumed to be Δxk-j, which is expressed as(32)Δxk-j=x^k-j-x^k-j/k-1-jj=1,2,…,nThe residual vector of the measurement is expressed as(33)Δzk-j=z^k-j-zk-jj=1,2,…,nwhere z^k-j=hk(x^k-j).When the statistics of the system process noise are changed, the contribution of the state predictionx^k-j/k-1-j to the state estimate will decrease, leading the prediction to be biased. As a result, the magnitude of the residual vector of the state prediction Δxk-j will increase. Similarly, when the statistics of the measurement noise are changed, the measurement residual Δz^k-j will be biased and its magnitude will increase.In order to capture the changes of the system noises, the random weighting factors are required to satisfy(34)λj∝Δxk-j·Δzk-jj=1,2,…,nwhere Δxk=ΔxkT·Δxk, Δzk=ΔzkT·Δzk, and the symbol “∝” indicates the proportional operation.Equation (34) implies that the larger the value of Δxk·Δzk is, the larger the weighting factor is.Therefore, the random weighting factorsλj can be determined as follows.Let(35)wj=Δxk-j·Δzk-jj=1,2,…,nNormalizing wj (j=1,2,…,n), the random weighting factors are obtained as(36)λj=wj∑j=1nwjj=1,2,…,nThe random weighted estimation of noise statistics enables adaptively adjusting the weights of the cubature points to improve the accuracy of the state prediction and measurement prediction and further inhibit the interference of system process noise and measurement noise on state estimation, leading to improved filtering accuracy and reliability.


## 5.1. Estimation of Noise Statistics

Denote the maximum posterior estimates ofq, Q, r, and R by q^, Q^, r^, and R^, respectively, and the posterior estimate of state xjj=0,1,…,k+1  by  x^j∣k+1. Assuming J=pxk+1,q,Q,r,R,zk+1 is a joint probability density function, we have [30](20)J=pxk+1,q,Q,r,R,zk+1=pzk+1∣xk+1,q,Q,r,Rpxk+1∣q,Q,r,Rpq,Q,r,Rwhere pq,Q,r,R can be obtained from prior information, and thus it is considered as a constant.Consider the conditional density function(21)J∗=pxk+1,q,Q,r,R∣zk+1=pxk+1,q,Q,r,R,zk+1pzk+1where xk+1=x0,x1,…,xk+1 and zk+1=z0,z1,…,zk+1.Consequently, the maximum posterior estimatesq^, Q^, r^, and R^ can be obtained by solving for the conditional density function J∗. It is evident that the maximum posterior estimates q^, Q^, r^, and R^ are not related to the denominator pzk+1 in (21). Therefore, the problem of calculating q^, Q^, r^, and R^ can be equivalently obtained by maximizing the following density function pxk+1,q,Q,r,R,zk+1 in (21).According to the multiplication theorem of conditional probability, we can obtain [30](22)pxk+1∣q,Q,r,R=px0∏j=0kpxj+1∣xj,q,Q=12πn/2P01/2exp⁡-12x0-x^0P0-12×∏j=0k12πn/2Q1/2exp⁡-12xj+1-fjxj-qQ-12=12πn/2+nk+1/2P0-1/2Q-k+1/2exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12=C1P0-1/2Q-k+1/2exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12where n is the state dimension, C1=1/2πnk+2/2 is a constant, and A is the determinant of A.Assume that the measurementsz1,z2,…,zk+1 are known and are independent of each other. Then,(23)pzk+1∣xk+1,q,Q,r,R=∏j=0kpzj+1∣xj+1,r,R=∏j=0k12πm/2R1/2exp⁡-12zj+1-hj+1xj+1-rR-12=C2Rk+1/2exp⁡-12∑j=0kzj+1-hj+1xj+1-rR-12where m is the measurement dimension and C2=1/2πmk+1/2 is a constant.Substituting (22) and (23) into (20) yields(24)J=C1C2P0-1/2·Q-k+1/2·R-k+1/2·pq,Q,r,R·exp⁡-12x0-x^0P0-12+∑j=0kxj+1-fjxj-qQ-12+∑j=0kzj+1-hj+1xj+1-rR-12=CQ-k+1/2·R-k+1/2×exp⁡-12∑j=0kxj+1-fjxj-qQ-12+∑j=0kzj+1-hj+1xj+1-rR-12where C=C1C2P0-1/2pq,Q,r,Rexp⁡-1/2x0-x^0P0-12.Taking the logarithm on both sides of (24) yields(25)ln⁡J=-k+12ln⁡Q-k+12ln⁡R-12∑j=0kxj+1-fjxj-qQ-12-∑j=0kzj+1-hj+1xj+1-rR-12+ln⁡CLet(26)∂ln⁡J∂qq=q^k+1=0∂ln⁡J∂QQ=Q^k+1=0The maximum posterior estimates q^ and Q^ can be obtained as (see Appendices A and B for details)(27)q^k+1=1k+1∑j=0kx^j+1∣k+1-fjxjxj←x^j∣k+1Q^k+1=1k+1∑j=0kx^j+1∣k+1-fjxjxj←x^jk+1-qx^j+1∣k+1-fjxjxj←x^j∣k+1-qTwhere fjxjxj←x^j∣k+1 is the posterior mean of state estimate x^j, which is propagated through nonlinear system function fj·.Similarly, let(28)∂ln⁡J∂rr=r^k+1=0∂ln⁡J∂RR=R^k+1=0The maximum posterior estimates r^ and R^ can be obtained as (see Appendices C and D for details)(29)r^k+1=1k+1∑j=0kzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1R^k+1=1k+1∑j=0kzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1-rzj+1-hj+1xj+1xj+1⟵x^j+1∣k+1-rTwhere hj+1xj+1xj+1←x^j+1∣j is the posterior mean of one-step state prediction x^j+1∣j which is propagated through nonlinear measurement function hj+1·.In (27) and (29), replacing the filtering estimations x^j∣k+1 and x^j+1∣k+1 with the estimations x^j and x^j+1 and replacing fjxjxj←x^j∣k+1+q with state prediction x^j+1∣j, we have(30)q^k+1=1k+1∑j=0kx^j+1-E^fjxj∣zkQ^k+1=1k+1∑j=0kx^j+1-E^fjxj∣zk-qx^j+1-E^fjxj∣zk-qT=1k+1∑j=0kx^j+1-x^j+1∣jx^j+1-x^j+1∣jTr^k+1=1k+1∑j=0kzj+1-E^hj+1xj+1∣zkR^k+1=1k+1∑j=0kzj+1-E^hj+1xj+1∣zk-rzj+1-E^hj+1xj+1∣zk-rT=1k+1∑jkzj+1-z^j+1∣jzj+1-z^j+1∣jTwhere fkxkxk←x^k=E^fk(xk)∣zk and hk+1xk+1xk+1←x^k+1∣k=E^hk+1(xk+1)∣zk.Accordingly, the random weighted estimations ofqk+1, Qk+1, rk+1, and Rk+1 can be represented as(31)q^k+1∗=∑j=0kλjx^j+1-E^fjxj∣zkQ^k+1∗=∑j=0kλjx^j+1-E^fjxj∣zk-qx^j+1-E^fjxj∣zk-qT=∑j=0kλjx^j+1-x^j+1∣jx^j+1-x^j+1∣jTr^k+1∗=∑j=0kλjzj+1-E^hj+1xj+1∣zkR^k+1∗=∑j=0kλjzj+1-E^hj+1xj+1∣zk-rzj+1-E^hj+1xj+1∣zk-rT=∑j=0kλjzj+1-z^j+1∣jzj+1-z^j+1∣jTwhere λj (j=1,2,…,k) are the random weighting factors.


## 5.2. Determination of Random Weighting Factors

Suppose the estimation and prediction of the state at epochk-jj=1,2,…,n are x^k-j and x^k-j/k-1-j, respectively. The residual vector of the prediction is assumed to be Δxk-j, which is expressed as(32)Δxk-j=x^k-j-x^k-j/k-1-jj=1,2,…,nThe residual vector of the measurement is expressed as(33)Δzk-j=z^k-j-zk-jj=1,2,…,nwhere z^k-j=hk(x^k-j).When the statistics of the system process noise are changed, the contribution of the state predictionx^k-j/k-1-j to the state estimate will decrease, leading the prediction to be biased. As a result, the magnitude of the residual vector of the state prediction Δxk-j will increase. Similarly, when the statistics of the measurement noise are changed, the measurement residual Δz^k-j will be biased and its magnitude will increase.In order to capture the changes of the system noises, the random weighting factors are required to satisfy(34)λj∝Δxk-j·Δzk-jj=1,2,…,nwhere Δxk=ΔxkT·Δxk, Δzk=ΔzkT·Δzk, and the symbol “∝” indicates the proportional operation.Equation (34) implies that the larger the value of Δxk·Δzk is, the larger the weighting factor is.Therefore, the random weighting factorsλj can be determined as follows.Let(35)wj=Δxk-j·Δzk-jj=1,2,…,nNormalizing wj (j=1,2,…,n), the random weighting factors are obtained as(36)λj=wj∑j=1nwjj=1,2,…,nThe random weighted estimation of noise statistics enables adaptively adjusting the weights of the cubature points to improve the accuracy of the state prediction and measurement prediction and further inhibit the interference of system process noise and measurement noise on state estimation, leading to improved filtering accuracy and reliability.


## 6. Adaptive Random Weighting Cubature Kalman Filter

Assume the posterior density function at epochk-1 is pxk-1=Nx^k-1∣k-1,Pk-1∣k-1. The ARWCKF algorithm for the nonlinear system defined by (6) can be described as follows.(i) Initialize State Estimate x ^ 0 and Error Covariance P 0 (37) x ^ 0 = E x 0 P 0 = c o v x 0 , x 0 T = E x 0 - x ^ 0 x 0 - x ^ 0 T(ii) Time Update and Calculation of Cubature Points. Denote the covariance of state prediction at time k-1 by Pk-1∣k-1. By Cholesky decomposition [16, 17], Pk-1∣k-1 can be written as(38)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1Tand the cubature points can be calculated by(39)xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1∣k-1where xi,k-1∣k-1 (i=1,2,…,m) is the system state of the ith cubature point at epoch k-1, and Sk-1∣k-1 is a diagonal matrix at time k-1.Then, the estimated state of theith cubature point from epoch k to k-1 can be written as(40)xi,k∣k-1∗=fxi,k∣k-1(iii) State Prediction. Based on (40), the predicted state of the ith cubature point from epoch k-1 to k is(41)x^k∣k-1=1m∑i=1mxi,k∣k-1∗+q^k-1Accordingly, the random weighted estimation ofx^k∣k-1 can be written as(42)x^k∣k-1∗=∑i=1mλixi,k∣k-1∗+q^k-1∗The covariance of the state predictionPk∣k-1 is described by(43)Pk∣k-1=1m∑i=1mxi,k∣k-1∗xi,k∣k-1∗T-x^k∣k-1x^k∣k-1T+Q^k-1Accordingly, the random weighted estimation ofPk∣k-1 can be written as(44)Pk∣k-1∗=∑i=1mλixi,k∣k-1∗xi,k∣k-1∗T-x^k∣k-1x^k∣k-1T+Q^k-1∗(iv) Measurement Prediction. The estimated measurement of the ith cubature point from epoch k-1 to k can be written as(45)zi∗,k∣k-1=hxi,k∣k-1The predicted measurement of theith cubature point from epoch k-1 to k can be expressed as(46)z^k∣k-1=1m∑i=1mzi,k∣k-1∗+r^kAccordingly, the random weighted estimation ofz^k∣k-1 can be written as(47)z^k∣k-1∗=∑i=1mλizi,k∣k-1∗+r^k∗The autocovariance of the measurement predictionPzz,k∣k-1 is described by(48)Pzz,k∣k-1=1m∑i=1mzi,k∣k-1∗zi,k∣k-1∗T-z^k∣k-1z^k∣k-1T+R^kAccordingly, the random weighting estimation for the autocovariance of the measurement prediction is(49)Pzz,k∣k-1∗=∑i=1mλizi,k∣k-1∗zi,k∣k-1∗T-z^k∣k-1z^k∣k-1T+R^k∗The cross-covariance of the measurement predictionPxz,k∣k-1 is (50)Pxz,k∣k-1=1m∑i=1mxi,k∣k-1∗zi,k∣k-1∗T-x^k∣k-1z^k∣k-1TAccordingly, the random weighting estimation for the cross-covariance of the measurement prediction is(51)Pxz,k∣k-1∗=∑i=1mλixi,k∣k-1∗zi,k∣k-1∗T-x^k∣k-1z^k∣k-1T(v) State Update. The filter gain at epoch k is(52)Kk=Pxz,k∣k-1∗Pzz,k∣k-1∗-1The state estimation at epochk is(53)x^k∣k=x^k∣k-1∗+Kkzk-z^k∣k-1∗The covariance estimation of the state error at epochk is(54)Pk∣k∗=Pk∣k-1∗-KkPzz,k∣k∗KkTFrom the above, it can be seen that the proposed ARWCKF incorporates random weighting factors in CKF, enabling us to adaptively refrain from the disturbances of system noises by online adjusting random weights to improve the accuracy of state estimation. The ARWCKF algorithm is shown in Figure1.Figure 1
ARWCKF algorithm.


## 7. Performance Evaluation and Discussion

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


### 7.1. System State Model

Due to the high accuracy in the velocity of SRS, the velocity error of SRS is much smaller than that of SINS. Therefore, the velocity error of SRS is assumed as a Gaussian white noise to reduce the dimension of the SINS/SRS integrated navigation system. Accordingly, the state vector of the SINS/SRS integrated navigation system consists of SINS errors only.The SINS/SRS integrated navigation system adopts the inertial coordinate frame as the navigation frame. Its system state vector is defined as(55)Xt=δvxδvyδvzδPxδPyδPzψxψyψzθxθyθzζxζyζzTwhere δvx,δvy,δvz, (δPx,δPy,δPz), and ψx,ψy,ψz are the errors in velocity, position, and attitude from SINS; θx,θy,θz is the constant bias of the gyros; and ζx,ζy,ζz is the zero bias of the accelerometers.The system state equation of the SINS/SRS integration is described by(56)X˙t=fXt+GtWtwhere f(·) is the nonlinear function given by (57), Xt is the system state vector, and Wt is the system noise.(57)fXt=Cω-1I-Cncω^inn+Cncδωinn-CbcδωibbI-CncTCbcf^sfb+CncTCbcδfsfb-2δωien+δωenn×V-2ω^ien+ω^enn×δV+2ωien+ωenn×δV+δgvyRM+h-vy-δvyRM-δRM+h-δhvxsecφRN+h-vx-δvxsecφ-δφRN-δRN+h-δhδvz01×7where Cω is the Euler platform error angle matrix; Cnc is the transformation matrix from the navigation (n) to computer (c) frames in terms of attitude; Cbc is the transformation matrix from the body (b) to computer (c) frames in terms of attitude; ω^ien and ωien are the projections of the actual and ideal angular velocities from the Earth coordinate system (e) to the inertial coordinate system (i) into the navigation frame (n); δωinn and δωibb are the calculation errors of ωinn and ωibb; f^sfb and δfsfb are the real force and its associated error of the accelerometer; V and δV are the real velocity and its associated error; δg is the error of gravity acceleration; φ and h are the latitude and altitude values; and RM and RN are the radii of curvatures of local meridian and prime vertical.The noise coefficient matrix is defined as(58)Gt=Cb3×3c03×303×3Cb3×3c09×309×315×6The system noise is described by Wt=wεxwεywεzw∇xw∇yw∇z6×1T, where wεx,wεy,wεz and w∇x,w∇y,w∇z are the gyro and accelerometer random noise, respectively.


### 7.2. Measurement Model

In order to overcome the drawback that the velocity error of SINS is accumulated in time series, the velocity information from SRS is used to correct the velocity error of SINS. Further, a radar altimeter is used to provide the altitude information to correct the altitude channel of SINS.The measurement of the SINS/SRS integrated navigation system can be chosen as the difference in velocity between SINS and SRS as well as the difference in altitude between the radar altimeter and SINS.Suppose the velocities obtained by SRS and SINS areVSRS=vSx,vSy,vSz and VSINS=vx,vy,vz. The difference in velocity between SRS and SINS is defined as(59)ΔZv=ΔZS1ΔZS2ΔZS3=vx-vSxvy-vSyvz-vSz=HvXt+Vvtwhere Vv is the velocity measurement noise matrix, and Hv=I3×303×12T is the measurement matrix.Further, the difference in altitude between the radar altimeter and SINS can be described as(60)ΔZh=hSINS-hH=HhXt+Vhtwhere hSINS and hH are the altitudes obtained by SINS and the radar altimeter; Vht is the measurement noise matrix in altitude; and Hh=01×5101×9T is the measurement matrix.Combining (59) with (60), the measurement equation of the SINS/SRS integration subsystem is established as(61)ΔZ1t=HvHhXt+VvtVht=H1tXt+V1t


### 7.3. Simulation and Analysis

Assume the spacecraft orbits the earth, where the orbit parameters are described in Table1. A flight period of 4000s was selected for the simulation test, where the initial position was “6359800m, 3076100m, -6268900m,” and the end position was “4550800m, 5222600m, -4356700.” The flight trajectory is shown in Figure 2.Table 1
The orbit parameters of the spacecraft.
Orbit Parameters Value Semi-major axis 6865.112418km Eccentricity 0.001248 Orbit inclination 29.059° Right ascension of ascending node 338.82° Argument of perigee 351.167° True anomaly 229.46° Orbit cycle 50076sFigure 2
Flight trajectory of the spacecraft.In the simulation, the SINS gyro drift is 0.05(°)/h, the random walk of the gyro is 0.005(°)/h, and the zero bias and random drift of the accelerator are 1×10-3g and 1×10-4g/s. The SINS initial alignment error is 0′. The initial position and velocity errors of the spacecraft are 10m, 10m, and 10m and 0.2m/s, 0.2m/s, and 0.2m/s. The sampling period of SINS is 0.01s, the data measurement frequency of SRS is 1Hz, and the velocity error of SRS is 0.05m/s. The noises of the system state model and measurement model are set as 0.01 and 0.02, respectively. The unscented transformation parameters are α=0.5 and β=2.The position error is defined as(62)δP=δPx2+δPy2+δPz2The velocity error is defined as(63)δV=δvx2+δvy2+δvz2For comparison analysis, 100 Monte Carlo simulation trials were conducted under the same conditions by CKF and ARWCKF, respectively.Figure3 shows the estimation errors in position and velocity by both CKF and ARWCKF. It can be seen that the position and velocity errors are within “-165m, 145m” and “-1.72m/s, 1.68m/s.” This poor estimation accuracy is because CKF does not have the capability to inhibit the disturbance of system noises. In contrast, as shown in Figure 4, ARWCKF can effectively estimate the noise statistics of the system state and measurement models to inhibit the disturbances of system noises, leading to the significantly improved accuracy. As shown in Figure 3, the resultant position and velocity errors by ARWCKF are within “-30m, 32m” and “-0.31m/s, 0.35m/s.” Table 2 summaries the means of the root mean square errors (RMSEs) in terms of position and velocity by both methods as well as the RMSEs of the estimated noises by the proposed ARWCKF for the SINS/SRS integrated navigation system.Table 2
RMSEs of CKF and ARWCKF.
Filtering algorithms Position RMSEs (m) Velocity RMSEs (m/s) Estimation RMSEs q r CKF 39.629 0.357 - - ARWCKF 11.289 0.068 3.811×10-4 3.782 × 1 0 - 4Figure 3
Position and velocity errors by CKF and ARWCKF.Figure 4
The estimations of system noise statistics by ARWCKF.The performance evaluation was further analyzed in terms of average normalized estimation error squared (ANEES). Figure5 shows the ANEES values in terms of position and velocity errors for both CKF and ARWCKF. It can be seen that there are significant oscillations involved in the ANEES curves of CKF. This means that a considerable portion of the error covariance matrices obtained by CKF are unreliable due to the existence of system noises. However, the ANEES values of ARWCKF are small during the entire simulation test, showing the estimation consistency of ARWCKF is not affected by system noises. Table 3 summaries the ANEES values of both CKF and ARWCKF. The results in terms of ANEES demonstrate the covariance matrices obtained by ARWCKF are much more reliable than those of CKF.Table 3
ANEESs of the CKF and ARWCKF algorithms.
Filtering algorithm Position ANEESs Velocity ANEESs CKF 0.5~22.3 0.4~20.6 ARWCKF 0.08~1.12 0.07~1.08Figure 5
ANEESs of CKF and ARWCKF.The above results demonstrate that the proposed ARWCKF can effectively inhibit the disturbances of system noises for spacecraft navigation, leading to the improved accuracy and robustness in comparison with CKF.


## 7.1. System State Model

Due to the high accuracy in the velocity of SRS, the velocity error of SRS is much smaller than that of SINS. Therefore, the velocity error of SRS is assumed as a Gaussian white noise to reduce the dimension of the SINS/SRS integrated navigation system. Accordingly, the state vector of the SINS/SRS integrated navigation system consists of SINS errors only.The SINS/SRS integrated navigation system adopts the inertial coordinate frame as the navigation frame. Its system state vector is defined as(55)Xt=δvxδvyδvzδPxδPyδPzψxψyψzθxθyθzζxζyζzTwhere δvx,δvy,δvz, (δPx,δPy,δPz), and ψx,ψy,ψz are the errors in velocity, position, and attitude from SINS; θx,θy,θz is the constant bias of the gyros; and ζx,ζy,ζz is the zero bias of the accelerometers.The system state equation of the SINS/SRS integration is described by(56)X˙t=fXt+GtWtwhere f(·) is the nonlinear function given by (57), Xt is the system state vector, and Wt is the system noise.(57)fXt=Cω-1I-Cncω^inn+Cncδωinn-CbcδωibbI-CncTCbcf^sfb+CncTCbcδfsfb-2δωien+δωenn×V-2ω^ien+ω^enn×δV+2ωien+ωenn×δV+δgvyRM+h-vy-δvyRM-δRM+h-δhvxsecφRN+h-vx-δvxsecφ-δφRN-δRN+h-δhδvz01×7where Cω is the Euler platform error angle matrix; Cnc is the transformation matrix from the navigation (n) to computer (c) frames in terms of attitude; Cbc is the transformation matrix from the body (b) to computer (c) frames in terms of attitude; ω^ien and ωien are the projections of the actual and ideal angular velocities from the Earth coordinate system (e) to the inertial coordinate system (i) into the navigation frame (n); δωinn and δωibb are the calculation errors of ωinn and ωibb; f^sfb and δfsfb are the real force and its associated error of the accelerometer; V and δV are the real velocity and its associated error; δg is the error of gravity acceleration; φ and h are the latitude and altitude values; and RM and RN are the radii of curvatures of local meridian and prime vertical.The noise coefficient matrix is defined as(58)Gt=Cb3×3c03×303×3Cb3×3c09×309×315×6The system noise is described by Wt=wεxwεywεzw∇xw∇yw∇z6×1T, where wεx,wεy,wεz and w∇x,w∇y,w∇z are the gyro and accelerometer random noise, respectively.


## 7.2. Measurement Model

In order to overcome the drawback that the velocity error of SINS is accumulated in time series, the velocity information from SRS is used to correct the velocity error of SINS. Further, a radar altimeter is used to provide the altitude information to correct the altitude channel of SINS.The measurement of the SINS/SRS integrated navigation system can be chosen as the difference in velocity between SINS and SRS as well as the difference in altitude between the radar altimeter and SINS.Suppose the velocities obtained by SRS and SINS areVSRS=vSx,vSy,vSz and VSINS=vx,vy,vz. The difference in velocity between SRS and SINS is defined as(59)ΔZv=ΔZS1ΔZS2ΔZS3=vx-vSxvy-vSyvz-vSz=HvXt+Vvtwhere Vv is the velocity measurement noise matrix, and Hv=I3×303×12T is the measurement matrix.Further, the difference in altitude between the radar altimeter and SINS can be described as(60)ΔZh=hSINS-hH=HhXt+Vhtwhere hSINS and hH are the altitudes obtained by SINS and the radar altimeter; Vht is the measurement noise matrix in altitude; and Hh=01×5101×9T is the measurement matrix.Combining (59) with (60), the measurement equation of the SINS/SRS integration subsystem is established as(61)ΔZ1t=HvHhXt+VvtVht=H1tXt+V1t


## 7.3. Simulation and Analysis

Assume the spacecraft orbits the earth, where the orbit parameters are described in Table1. A flight period of 4000s was selected for the simulation test, where the initial position was “6359800m, 3076100m, -6268900m,” and the end position was “4550800m, 5222600m, -4356700.” The flight trajectory is shown in Figure 2.Table 1
The orbit parameters of the spacecraft.
Orbit Parameters Value Semi-major axis 6865.112418km Eccentricity 0.001248 Orbit inclination 29.059° Right ascension of ascending node 338.82° Argument of perigee 351.167° True anomaly 229.46° Orbit cycle 50076sFigure 2
Flight trajectory of the spacecraft.In the simulation, the SINS gyro drift is 0.05(°)/h, the random walk of the gyro is 0.005(°)/h, and the zero bias and random drift of the accelerator are 1×10-3g and 1×10-4g/s. The SINS initial alignment error is 0′. The initial position and velocity errors of the spacecraft are 10m, 10m, and 10m and 0.2m/s, 0.2m/s, and 0.2m/s. The sampling period of SINS is 0.01s, the data measurement frequency of SRS is 1Hz, and the velocity error of SRS is 0.05m/s. The noises of the system state model and measurement model are set as 0.01 and 0.02, respectively. The unscented transformation parameters are α=0.5 and β=2.The position error is defined as(62)δP=δPx2+δPy2+δPz2The velocity error is defined as(63)δV=δvx2+δvy2+δvz2For comparison analysis, 100 Monte Carlo simulation trials were conducted under the same conditions by CKF and ARWCKF, respectively.Figure3 shows the estimation errors in position and velocity by both CKF and ARWCKF. It can be seen that the position and velocity errors are within “-165m, 145m” and “-1.72m/s, 1.68m/s.” This poor estimation accuracy is because CKF does not have the capability to inhibit the disturbance of system noises. In contrast, as shown in Figure 4, ARWCKF can effectively estimate the noise statistics of the system state and measurement models to inhibit the disturbances of system noises, leading to the significantly improved accuracy. As shown in Figure 3, the resultant position and velocity errors by ARWCKF are within “-30m, 32m” and “-0.31m/s, 0.35m/s.” Table 2 summaries the means of the root mean square errors (RMSEs) in terms of position and velocity by both methods as well as the RMSEs of the estimated noises by the proposed ARWCKF for the SINS/SRS integrated navigation system.Table 2
RMSEs of CKF and ARWCKF.
Filtering algorithms Position RMSEs (m) Velocity RMSEs (m/s) Estimation RMSEs q r CKF 39.629 0.357 - - ARWCKF 11.289 0.068 3.811×10-4 3.782 × 1 0 - 4Figure 3
Position and velocity errors by CKF and ARWCKF.Figure 4
The estimations of system noise statistics by ARWCKF.The performance evaluation was further analyzed in terms of average normalized estimation error squared (ANEES). Figure5 shows the ANEES values in terms of position and velocity errors for both CKF and ARWCKF. It can be seen that there are significant oscillations involved in the ANEES curves of CKF. This means that a considerable portion of the error covariance matrices obtained by CKF are unreliable due to the existence of system noises. However, the ANEES values of ARWCKF are small during the entire simulation test, showing the estimation consistency of ARWCKF is not affected by system noises. Table 3 summaries the ANEES values of both CKF and ARWCKF. The results in terms of ANEES demonstrate the covariance matrices obtained by ARWCKF are much more reliable than those of CKF.Table 3
ANEESs of the CKF and ARWCKF algorithms.
Filtering algorithm Position ANEESs Velocity ANEESs CKF 0.5~22.3 0.4~20.6 ARWCKF 0.08~1.12 0.07~1.08Figure 5
ANEESs of CKF and ARWCKF.The above results demonstrate that the proposed ARWCKF can effectively inhibit the disturbances of system noises for spacecraft navigation, leading to the improved accuracy and robustness in comparison with CKF.


## 8. Conclusions

This paper presents a new ARWCKF for state estimation in nonlinear systems. It adopts the concept of random weighting for the first time to address the problem that the CKF performance is sensitive to system noise. The proposed method incorporates random weights in CKF to estimate system noise statistics as well as predicted state and measurement together with their associated covariances. Based on this, it adaptively adjusts random weights of cubature points to inhibit the disturbances of system noises on state estimation, leading to improved filtering accuracy and robustness. Simulations and comparison analysis demonstrate that the proposed ARWCKF is superior to CKF for spacecraft navigation.Future work will focus on improvement of the proposed ARWCKF. By combining the proposed ARWCKF with advanced artificial intelligence techniques such as deep learning neural networks, swarming algorithms, and fuzzy logic, it is expected to develop an intelligent filtering algorithm for automatic estimation of system noise statistics from various error sources in practical engineering applications.



---

*Source: 4160847-2019-01-17.xml*