# A New Adaptive Federated Cubature Kalman Filter Based on Chi-Square Test for SINS/GNSS/SRS/CNS Integration

Authors: Fei Xu; Guangle Gao; Longqiang Ni
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

As an emerging means of transportation for the intelligent transportation system (ITS) in aviation and aerospace, hypersonic cruise vehicles (HCVs) have received numerous research interests during the past several decades. However, the navigation and positioning strictly limit the progress and application of HCVs due to their special characteristics on dynamics and environments. To improve the stability of navigation in HCVs, a chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) is proposed in this paper. In the proposed approach, the chi-square test is adopted for the estimation of the measurement noise statistics firstly. Subsequently, a new adaptive information fusion factor is designed for the federated filter to adjust the contribution of each subsystem. Finally, the information sharing factor, which is used for the amendment of the state covariance of each subsystem, is refined based on the judging index of the chi-square test accordingly. Simulation results show that the proposed CAFCKF can be used to improve the accuracy and stability of the navigation system.


---

## Body

## 1. Introduction

The intelligent transportation system (ITS) has received numerous research interests to improve the consumption of vehicles using advanced technologies during the past several decades. Recently, the concept of ITS has been expanded from the land traffic to the fields of aviation and aerospace. Hypersonic cruise vehicle (HCV) is an emerging means of transportation for the ITS in aviation and aerospace. However, the navigation and positioning for the HCVs are still a challenging problem due to their special characteristics on dynamics and environments, leading to the limited progress and application of the HCVs. Currently, the strapdown inertial navigation system (SINS) has been widely used for autonomous navigation in HCVs [1–3]. However, the error of SINS often increases boundlessly in time series due to the gyro drift and accelerometer bias [3]. To overcome the shortcoming using a single SINS, the Global Navigation Satellite System (GNSS) was proposed to provide highly accurate position information [4–6]. But the GNSS usually lacks autonomy, which makes it susceptible to artificial interference. To reduce the accumulated navigation error and to resist electromagnetic interference, the celestial navigation system (CNS) was proposed in [7, 8]. The SINS, GNSS, and CNS were then combined to keep the system reliable when the GNSS was out of service [9]. However, the problem with the CNS is that it has difficulty in star selection and velocity measurement [10]. As a new supplement for the navigation technique, the spectral redshift navigation system (SRS) was then studied, in which the velocity can be calculated from the spectral redshift information of celestial spectrum [11]. Compared with other navigation systems, the SRS has the advantages such as simple navigation principle, easy star selection, and zero time delay, which makes it suitable as an auxiliary navigation system to assist in correcting the velocity error of SINS, avoiding parameter divergence while maintaining system autonomy [12, 13]. Due to these reasons, the integration of INS/GNSS/CNS/SRS will be a promising navigation strategy for the HCVs to address the limitations of INS/GNSS integration in lack of autonomy and INS/GNSS/CNS integration in star selection and velocity measurement.The Kalman filter has been proposed as an important information fusion algorithm for the linear integrated navigation system [14]. Recently, the nonlinear information fusion algorithms, such as extended Kalman filter (EKF) [15], unscented Kalman filter (UKF) [16], and cubature Kalman filter (CKF) [17], were proposed to handle the data fusion problem involved in the nonlinear system. The CKF applies the cubature rule to obtain the consequently fixed cubature points with a constant weight, which is more stable than UKF [18]. A comparison between CKF and UKF was made in [19], and it was shown that the CKF was more accurate than the sigma points approach-based UKF. When the Kalman filter was used to achieve information fusion in the navigation system, an accurate system model and exact noise statistics were required [20]. However, the navigation system always involves uncertainties and faults caused by the outlier in measurement under highly dynamic conditions. To avoid the influence of outlier in measurement, measurement noise was often handled by the H-infinity strategy [21, 22]. However, this method may break down in the presence of randomly occurred outliers since it assumes that the estimation to be achieved is a linear combination of state vectors which is very difficult to satisfy. Through statistical linear regression of a nonlinear system function, the Huber-based KF method was proposed in [23] to curb the effect of outlier in measurement, which can achieve the robustness but sacrifices the estimation accuracy. Moreover, a scaling factor of measurement noise covariance was calculated to further adjust the Kalman gain to deal with the outlier in measurement [24, 25]. However, the scaling factor in this method is often selected by experience. To improve the accuracy of noise estimation, the maximum likelihood principle (MLP) was also studied in [26, 27]. However, this method involves large computational burden, which makes it unable to achieve the real-time navigation performance [25].Considering redundant and homologous measurement information in the multisensor navigation system, the federated Kalman filter (FKF) was proposed for the state fusion estimation of integrated navigation information [28, 29]. With a distributed filter structure, the FKF is more convenient for fault isolation and more efficient than the centralized filter. In the FKF, the information sharing factor has great effects on the accuracy of information fusion and correlation elimination among the local estimations. However, the value of information sharing factor does not always reflect the differences between each state variable. The chi-square test is a statistical method for the real-time detection of abnormal changes in a dynamic system [30, 31]. Thus, it can be used to detect changes of statistical characteristics of measurement noise and to optimize the information sharing factor in the FKF for the application of integrated navigation.In this paper, a SINS/GNSS/SRS/CNS integrated navigation system and a chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) are proposed to improve the accuracy and reliability of the navigation system in HCVs. In CAFCKF algorithm, the chi-square test is used for the estimation of the statistical characteristics of measurement noise. Then, a new adaptive information fusion factor is designed to adjust the contribution of each subsystem. Further, the information sharing factor is also refined based on the judge index of the chi-square test to amend the state covariance of each subsystem for the optimization of the global state estimation. Finally, the proposed CAFCKF algorithm is verified in the SINS/GNSS/SRS/CNS integrated navigation system.


## 2. SINS/GNSS/SRS/CNS Integrated Navigation System

The structure of SINS/GNSS/SRS/CNS integrated navigation system is shown in Figure1, in which the SINS is used as the main navigation system and the GNSS, SRS, and CNS are applied to correct the error of SINS. In this integrated system, the SRS and CNS can be regarded as the supplement of SINS/GNSS integration when GNSS is out of service, since they can provide the velocity observation and position measurement to correct the error of SINS.Figure 1
SINS/GNSS/SRS/CNS integrated navigation system.


### 2.1. Kinematic Model

The state of the kinematic model is chosen as(1)Xt=Φδvnδpεb∇b,where Φ=ϕE,ϕN,ϕU denotes the attitude error, δvn=δvE,δvN,δvU is the velocity error, δp=δL,δλ,δh denotes the position error, εb and ∇b denote the gyro constant drift and accelerometer zero bias, respectively, and n and b represent the n-frame and b-frame, which will be explained in the next paragraph.The navigation frame (n-frame) is selected as the E-N-U (East-North-Up) frame. Denote the inertial frame as i-frame, the body frame as b-frame, the Earth frame as e-frame, and the SINS simulated navigation frame as n’-frame. Based on the E-N-U frame, the error equations of SINS in terms of velocity and attitude are written as(2)Φ˙=−ω^inn×Φ+δω^inn−Cbn′δω^ibbδv˙n=Cbn′f^b×Φ+Cbn′δfb−2ωien′+ωenn′×δvn−2δωien+δωenn×vn,where f^b is the measured speciﬁc force in the b-frame, δω^ibb is the gyro’s measurement error, ωien is the angular velocity of the Earth, ωenn is the angular velocity of the vehicle relative to the Earth, ωinn=ωien+ωenn is the relative angular velocity between n-frame and i-frame, ωinn′, ωenn′, and ωien′ are the corresponding values in the n’-frame, and vn is the velocity obtained by SINS in n-frame.The error equation of SINS in terms of position is given by(3)δL˙=δvNRM+h^−δhvNRM+h^2,δλ˙=δvEsecL^RN+h^+δLvEtanL^secL^RN+h^−δhδvEsecL^RN+h^2,δh˙=δvU,where RM and RN are the median radius and normal radius and h^, λ^, and L^ are the altitude, longitude, and latitude output using SINS in n-frame.The equation ofεb and ∇b can be described as(4)ε˙i=0i=x,y,z∇˙i=0i=x,y,z.Thus, from (2)–(4), the discrete kinematic model of SINS/GNSS/SRS/CNS integration system can be presented by(5)Xk=Fk/k−1Xk−1+Wk,where k is the sample time, Xk is the discrete system’s state vector, Fk/k−1 is the discrete system’s state transformation matrix, and Wk is the discrete system’s kinematic noise matrix.


### 2.2. Measurement Equation of SINS/GNSS Subsystem

The pseudorange and pseudorange rate from a GNSS receiver can be represented as [10](6)ρi=Ri+bp+vρi,i=1,2,…,mρ˙i=Diρi+bp+vρ˙i,i=1,2,…,m,where ρi and ρ˙i represent the pseudorange and pseudorange rate, respectively, and vi=vρivρ˙iT represents the measurement noise.(7)Di=x−xsix˙−x˙si+y−ysiy˙−y˙si+z−zsiz˙−z˙si,where x,y,z is the actual position of the vehicle in the e-frame; xsi,ysi,zsi is the position of the ith satellite in the e-frame; x˙,y˙,z˙ is the actual velocity of vehicle in the e-frame; x˙si,y˙si,z˙si is the velocity of the ith satellite in the e-frame; and Ri represents the geometric range from the i th satellite to the receiver, which can be calculated as(8)Ri=x−xsi2+y−ysi2+z−zsi2.It is known that(9)x=RN+hcosLcosλ,y=RN+hcosLsinλ,z=RN1−f2+hsinL,x˙y˙z˙=CnevEvNvU=−sinλ−sinLcosλcosLcosλcosλ−sinLsinλcosLsinλ0cosLsinLvEvNvU,where L,λ,h is the corresponding position of the vehicle in n-frame, RN is the radius of curvature in the prime vertical, f is the eccentricity of the ellipsoid, Cne is the rotation matrix from n-frame to e-frame, and vE,vN,vU is the corresponding velocity of vehicle in n-frame.The position and velocity estimated by SINS are satisfied such that(10)L^=L+δLλ^=λ+δλh^=h+δh,v^E=vE+δvEv^N=vN+δvNv^U=vU+δvU,where L,λ,h is the real position.By substituting (7)–(11) into (6), the nonlinear measurement equation is obtained as(11)ZGNSS=hρ1,ρ2,ρ3,ρ4,ρ˙1,ρ˙2,ρ˙3,ρ˙4T+v,where vi=vρi;vρ˙i.(12)ρi=RN+h^−δhcosL^−δLcosλ^−δλ−xsi2+RN+h^−δhcosL^−δLsinλ^−δλ−ysi2+RN1−f2+h^−δhsinL^−δL−zsi21/2+bp,ρ˙i=RN+h^−δhcosL^−δLcosλ^−δλ−xsi×v^U−δvUcosL^−δLcosλ^−δλ−v^E−δvEsinλ−v^N−δvNsinLcosλ+RN+h^−δhcosL^−δLsinλ^−δλ−ysi×v^U−δvUcosL^−δLsinλ^−δλ+v^E−δvEcosλ−v^N−δvNsinLsinλ+RN1−f2+h^−δhsinL^−δL−zsi×v^U−δvUsinL^−δL+v^N−δvNL^−δLρi+bp.


### 2.3. Measurement Equation of SINS/SRS Subsystem

Considering the redshift principle of spectrum and the Doppler frequency shift formula, the relationship between redshift of celestial spectrum observed in vehicle and the velocity of vehicle is [12](13)1+zic2−vp−vci2−vp−vci⋅ui−c=0,where i denotes the observed objects which is not less than 3, zi denotes spectral redshift value of celestial body calculated in the target vehicle, vp denotes the velocity vector of the vehicle in the i-frame, vc denotes the velocity vector of the celestial body in the inertial coordinate system, which can be obtained by querying the celestial ephemeris, c denotes the velocity of light, and u denotes the position unit vector of the celestial object in the i-frame, which can be measured by the star sensors.Assume that(14)Zivp=vp−vci⋅ui−1+zic2−vp−vi2+c.Through the first order Taylor expansion atvp=0 and omitting the higher-order terms, (14) can be transformed to(15)Z10+∂Z1vp∂vpx|vp=0⋅vpx+∂Z1vp∂vpy|vp=0⋅vpy+∂Z1vp∂vpz|vp=0⋅vpz=0,Z20+∂Z2vp∂vpx|vp=0⋅vpx+∂Z2vp∂vpy|vp=0⋅vpy+∂Z3vp∂vpz|vp=0⋅vpz=0,Z30+∂Z3vp∂vpx|vp=0⋅vpx+∂Z3vp∂vpy|vp=0⋅vpy+∂Z3vp∂vpz|vp=0⋅vpz=0,where vpx,vpy,vpzT represents the components of vp in i-frame.A nonhomogenous equation can be obtained as(16)∂Z1∂vpx|vp=0∂Z1∂vpy|vp=0∂Z1∂vpz|vp=0∂Z2∂vpx|vp=0∂Z2∂vpy|vp=0∂Z2∂vpz|vp=0∂Z3∂vpx|vp=0∂Z3∂vpy|vp=0∂Z3∂vpz|vp=0vp=Lvp=−Z10−Z20−Z30.Due to the fact that the three observed celestial bodies are noncollinear,L is a full-rank matrix:(17)vp=−Z10Z20Z30L−1.The velocity of vehicle calculated by SRS in ENU-frame can be obtained as(18)vSRS=Cgivp=−CeiCgeZ10Z20Z30L−1,where vSRS is the velocity of SRS in ENU-frame, Cge is the rotation matrix from Earth frame to ENU-frame, and Cei is the rotation matrix from i-frame to the e-frame.The velocity error measurement equation of SINS/SRS subsystem can be expressed as(19)ZSRS=vn−vSRS=δvEδvNδvU+VSRS,where VSRS is the measurement noise.


### 2.4. Measurement Equation of SINS/CNS Subsystem

The measurement equation of SINS/CNS subsystem is chosen as the difference of longitude and latitude information between SINS and CNS:(20)Zp=λ^−λCNSL^−LCNS,where λCNS,LCNS denote the longitude and latitude measurements of CNS.In order to prevent the divergence of altitude channel in SINS, a barometric altimeter is introduced to the integrated navigation system. The difference in altitude between the barometric altimeter and SINS is taken as the measurement. Thus, the measurement equation is written as(21)Zh=h^−hU,where hU denotes the altitude output of barometric altimeter.Then, the measurement equation of SINS/CNS subsystem can be further written as(22)ZCNS&BA=HCNS&BAX+VCNS&BA,where HCNS&BA=03×3I3×303×9 is the measurement matrix of the SINS/CNS subsystem and Vk=VCNS;VBA is the measurement noise.


## 2.1. Kinematic Model

The state of the kinematic model is chosen as(1)Xt=Φδvnδpεb∇b,where Φ=ϕE,ϕN,ϕU denotes the attitude error, δvn=δvE,δvN,δvU is the velocity error, δp=δL,δλ,δh denotes the position error, εb and ∇b denote the gyro constant drift and accelerometer zero bias, respectively, and n and b represent the n-frame and b-frame, which will be explained in the next paragraph.The navigation frame (n-frame) is selected as the E-N-U (East-North-Up) frame. Denote the inertial frame as i-frame, the body frame as b-frame, the Earth frame as e-frame, and the SINS simulated navigation frame as n’-frame. Based on the E-N-U frame, the error equations of SINS in terms of velocity and attitude are written as(2)Φ˙=−ω^inn×Φ+δω^inn−Cbn′δω^ibbδv˙n=Cbn′f^b×Φ+Cbn′δfb−2ωien′+ωenn′×δvn−2δωien+δωenn×vn,where f^b is the measured speciﬁc force in the b-frame, δω^ibb is the gyro’s measurement error, ωien is the angular velocity of the Earth, ωenn is the angular velocity of the vehicle relative to the Earth, ωinn=ωien+ωenn is the relative angular velocity between n-frame and i-frame, ωinn′, ωenn′, and ωien′ are the corresponding values in the n’-frame, and vn is the velocity obtained by SINS in n-frame.The error equation of SINS in terms of position is given by(3)δL˙=δvNRM+h^−δhvNRM+h^2,δλ˙=δvEsecL^RN+h^+δLvEtanL^secL^RN+h^−δhδvEsecL^RN+h^2,δh˙=δvU,where RM and RN are the median radius and normal radius and h^, λ^, and L^ are the altitude, longitude, and latitude output using SINS in n-frame.The equation ofεb and ∇b can be described as(4)ε˙i=0i=x,y,z∇˙i=0i=x,y,z.Thus, from (2)–(4), the discrete kinematic model of SINS/GNSS/SRS/CNS integration system can be presented by(5)Xk=Fk/k−1Xk−1+Wk,where k is the sample time, Xk is the discrete system’s state vector, Fk/k−1 is the discrete system’s state transformation matrix, and Wk is the discrete system’s kinematic noise matrix.


## 2.2. Measurement Equation of SINS/GNSS Subsystem

The pseudorange and pseudorange rate from a GNSS receiver can be represented as [10](6)ρi=Ri+bp+vρi,i=1,2,…,mρ˙i=Diρi+bp+vρ˙i,i=1,2,…,m,where ρi and ρ˙i represent the pseudorange and pseudorange rate, respectively, and vi=vρivρ˙iT represents the measurement noise.(7)Di=x−xsix˙−x˙si+y−ysiy˙−y˙si+z−zsiz˙−z˙si,where x,y,z is the actual position of the vehicle in the e-frame; xsi,ysi,zsi is the position of the ith satellite in the e-frame; x˙,y˙,z˙ is the actual velocity of vehicle in the e-frame; x˙si,y˙si,z˙si is the velocity of the ith satellite in the e-frame; and Ri represents the geometric range from the i th satellite to the receiver, which can be calculated as(8)Ri=x−xsi2+y−ysi2+z−zsi2.It is known that(9)x=RN+hcosLcosλ,y=RN+hcosLsinλ,z=RN1−f2+hsinL,x˙y˙z˙=CnevEvNvU=−sinλ−sinLcosλcosLcosλcosλ−sinLsinλcosLsinλ0cosLsinLvEvNvU,where L,λ,h is the corresponding position of the vehicle in n-frame, RN is the radius of curvature in the prime vertical, f is the eccentricity of the ellipsoid, Cne is the rotation matrix from n-frame to e-frame, and vE,vN,vU is the corresponding velocity of vehicle in n-frame.The position and velocity estimated by SINS are satisfied such that(10)L^=L+δLλ^=λ+δλh^=h+δh,v^E=vE+δvEv^N=vN+δvNv^U=vU+δvU,where L,λ,h is the real position.By substituting (7)–(11) into (6), the nonlinear measurement equation is obtained as(11)ZGNSS=hρ1,ρ2,ρ3,ρ4,ρ˙1,ρ˙2,ρ˙3,ρ˙4T+v,where vi=vρi;vρ˙i.(12)ρi=RN+h^−δhcosL^−δLcosλ^−δλ−xsi2+RN+h^−δhcosL^−δLsinλ^−δλ−ysi2+RN1−f2+h^−δhsinL^−δL−zsi21/2+bp,ρ˙i=RN+h^−δhcosL^−δLcosλ^−δλ−xsi×v^U−δvUcosL^−δLcosλ^−δλ−v^E−δvEsinλ−v^N−δvNsinLcosλ+RN+h^−δhcosL^−δLsinλ^−δλ−ysi×v^U−δvUcosL^−δLsinλ^−δλ+v^E−δvEcosλ−v^N−δvNsinLsinλ+RN1−f2+h^−δhsinL^−δL−zsi×v^U−δvUsinL^−δL+v^N−δvNL^−δLρi+bp.


## 2.3. Measurement Equation of SINS/SRS Subsystem

Considering the redshift principle of spectrum and the Doppler frequency shift formula, the relationship between redshift of celestial spectrum observed in vehicle and the velocity of vehicle is [12](13)1+zic2−vp−vci2−vp−vci⋅ui−c=0,where i denotes the observed objects which is not less than 3, zi denotes spectral redshift value of celestial body calculated in the target vehicle, vp denotes the velocity vector of the vehicle in the i-frame, vc denotes the velocity vector of the celestial body in the inertial coordinate system, which can be obtained by querying the celestial ephemeris, c denotes the velocity of light, and u denotes the position unit vector of the celestial object in the i-frame, which can be measured by the star sensors.Assume that(14)Zivp=vp−vci⋅ui−1+zic2−vp−vi2+c.Through the first order Taylor expansion atvp=0 and omitting the higher-order terms, (14) can be transformed to(15)Z10+∂Z1vp∂vpx|vp=0⋅vpx+∂Z1vp∂vpy|vp=0⋅vpy+∂Z1vp∂vpz|vp=0⋅vpz=0,Z20+∂Z2vp∂vpx|vp=0⋅vpx+∂Z2vp∂vpy|vp=0⋅vpy+∂Z3vp∂vpz|vp=0⋅vpz=0,Z30+∂Z3vp∂vpx|vp=0⋅vpx+∂Z3vp∂vpy|vp=0⋅vpy+∂Z3vp∂vpz|vp=0⋅vpz=0,where vpx,vpy,vpzT represents the components of vp in i-frame.A nonhomogenous equation can be obtained as(16)∂Z1∂vpx|vp=0∂Z1∂vpy|vp=0∂Z1∂vpz|vp=0∂Z2∂vpx|vp=0∂Z2∂vpy|vp=0∂Z2∂vpz|vp=0∂Z3∂vpx|vp=0∂Z3∂vpy|vp=0∂Z3∂vpz|vp=0vp=Lvp=−Z10−Z20−Z30.Due to the fact that the three observed celestial bodies are noncollinear,L is a full-rank matrix:(17)vp=−Z10Z20Z30L−1.The velocity of vehicle calculated by SRS in ENU-frame can be obtained as(18)vSRS=Cgivp=−CeiCgeZ10Z20Z30L−1,where vSRS is the velocity of SRS in ENU-frame, Cge is the rotation matrix from Earth frame to ENU-frame, and Cei is the rotation matrix from i-frame to the e-frame.The velocity error measurement equation of SINS/SRS subsystem can be expressed as(19)ZSRS=vn−vSRS=δvEδvNδvU+VSRS,where VSRS is the measurement noise.


## 2.4. Measurement Equation of SINS/CNS Subsystem

The measurement equation of SINS/CNS subsystem is chosen as the difference of longitude and latitude information between SINS and CNS:(20)Zp=λ^−λCNSL^−LCNS,where λCNS,LCNS denote the longitude and latitude measurements of CNS.In order to prevent the divergence of altitude channel in SINS, a barometric altimeter is introduced to the integrated navigation system. The difference in altitude between the barometric altimeter and SINS is taken as the measurement. Thus, the measurement equation is written as(21)Zh=h^−hU,where hU denotes the altitude output of barometric altimeter.Then, the measurement equation of SINS/CNS subsystem can be further written as(22)ZCNS&BA=HCNS&BAX+VCNS&BA,where HCNS&BA=03×3I3×303×9 is the measurement matrix of the SINS/CNS subsystem and Vk=VCNS;VBA is the measurement noise.


## 3. Chi-Square Test-Based Adaptive Federated Cubature Kalman Filter

### 3.1. Federated Cubature Kalman Filter

To improve the computation efficiency and reliability, a federated cubature Kalman filter (FCKF) is established and used for the multisensor nonlinear system. Its procedure is shown below.Step 1.
State prediction:(23)ηi,s,k|k−1=Ss,k−1|k−1ξi+Xk−1,where, Sk−1|k−1Sk−1|k−1T=Ps,k−1.(24)xi,s,k|k−1=fηi,s,k|k−1,where f· denotes the state equation and ξi denotes the ith element of ξd×2d.
ξd×2d can be expressed as(25)ξd×2d=10⋯0−10⋯00100−10⋮⋮⋱⋮⋮⋱00⋯100⋯−1d×2d,where d is the dimension of system state.(26)Xs,k|k−1=∑i=12dwixi,s,k|k−1,(27)Ps,k|k−1=∑i=12dwixi,s,k|k−1xi,s,k|k−1T+Xs,k|k−1Xs,k|k−1T+Qs,k,where wi=1/2d.Step 2.
Calculate the gain matrix of the subfilter:(28)ςi,s,k|k−1=Ss,k|k−1ξ+Xs,k|k−1,(29)Ss,k|k−1Ss,k|k−1T=Ps,k|k−1,(30)zi,s,k|k−1=hsςi,k|k−1+rs,k,(31)Zs,k|k−1=∑i=12dwizi,s,k|k−1.
The prediction innovation covariance matrix and cross-covariance matrix of subfilters can be written as(32)Ps,k|k−1zz=∑i=12dwizi,s,k|k−1zi,s,k|k−1T+Zs,k|k−1Zs,k|k−1T+Rs,k,(33)Ps,k|k−1xz=∑i=12dwiςi,k|k−1zi,s,k|k−1T+Xk|k−1Zs,k|k−1T,where Zs,k|k−1 denotes the measurement prediction of subsystem s.
The gain of subfilters can be written as(34)Ks,k=Ps,k|k−1xzPs,k|k−1zz−1.Step 3.
Update state of the subfilter as(35)X^s,k=Xk|k−1+Ks,kZs,k−Zs,k|k−1,(36)P^s,k=Pk|k−1+Ks,kTPs,k|k−1zzKs,k,where Ps,k is the covariance matrix of subfilter s and X^s,k is the state estimation of subfilter s.Step 4.
Calculate the global fusion through the main filter:(37)Pg,k=∑Ps,k−1−1,X^g,k=Pg,k∑Ps,k−1X^s,k,where Pg,k is the covariance matrix of the global filter and X^g,k is the state estimation of the global filter.


### 3.2. Establishment of the Chi-Square Test-Based Adaptive FCKF (CAFCKF)

In reality, the statistic characteristics of measurement noise are always unknown or changed with time. Under this situation, the estimator has great difficulty in ensuring the system accuracy. To address this problem, a noise estimator for the measurement noise statistics based on the chi-square test (CST) is firstly proposed in this paper.The innovation ofsth subsystem is defined as(38)νs,k=Zs,k−Zs,k/k−1.The hypothesis test based on innovation can be constructed as(39)H0:EνsνsT=Ps,k|k−1zz,noise statistic unchanged,H1:EνsνsT=νs,k−μ^s,kνs,k−μ^s,kT≠Ps,k|k−1zz,noise statistic changed.Then, calculate(40)rs,k=1M∑j=1Mνs,k−j+1.According to the principle of CST, the judge index can be expressed as(41)Js,k=νs,k−rs,kTPs,k|k−1zz−1νs,k−rs,k,where Js,k∼χ2m.According to the hypothesis test, by setting the significance levelα (0 < α < 1), there will be a threshold Ts, which makes α follow [28](42)PJs,k>Ts=α.When the statistical characteristics of measurement noise are unchanged,λk will be smaller than Ts. Otherwise, the judge index will exceed the threshold, and the covariance of predicted measurement should be adjusted according to(43)fPs,k|k−1zz=JPs,k|k−1zz−Ts.Based on Newton’s method, we have(44)Ps,k|k−1zzi+1=Ps,k|k−1zzi+νs,k−rs,kTPs,k|k−1zzi−1−1νs,k−rs,k−Tsνs,k−rs,kTPs,k|k−1zzi−2νs,k−rs,kT,where i denotes the times of iteration in Newton’s method.SettingP^s,kzz0=Ps,k|k−1zz, the estimation of measurement noise covariance can be obtained by running the iteration of (44) until fP^s,kzzi is less than 0. To limit the times of iteration, a cutoff time C is set to 5, which means if JP^s,kzz is not less than the threshold when the times of iteration reach C, the iteration will end.Subsequently, to improve the performance of the information procedure, a new adaptive information fusion factor is defined in FCKF. With the update of state estimation through the subfilter, a judge index of the chi-square test can be calculated according to the state estimation and the state prediction from the subfilter:(45)JX,s,k=X^s,k−Xk||k−1TPs,k||k−1−1X^s,k−Xk||k−1,where JX,s,k denotes the state judge index of subfilter s.Since state estimation error of a subsystem increases asJX,s,k increases, the information fusion factor under the chi-square test can be defined as(46)As,k=diagAsk,Ask,…Askm×m,where(47)Ask=1/JX,s,k∑1/JX,s,k.Then, the global fusion of FCKF can be modified as(48)Pg,k=∑As,kPs,k−1−1,(49)X^g,k=Pg,k∑As,kPs,k−1X^s,k.After the global fusion process, the information sharing factor for each subfilter is needed to refine the estimation of each subsystem. The judge index of the chi-square test calculated by the global state estimation and the local state estimation can be written as(50)JX,sg,k=X^g,k−X^s,kTPg,k−1X^g,k−X^s,k.WhenJX,g,k is larger than those of other subsystems, the real contribution of the sth subsystem to the integration system is smaller than that of other subsystems, which means this subsystem should have a bigger state covariance than others. So, we refine the information sharing factor by calculating it as(51)Bs,k=diagBskBsk⋯Bskm×m,where(52)Bsk=1/JX,sg,k∑1/JX,sg,k.It is known that(53)∑s=1LBs,k=Im×m.Thus, the refined state covariance of the subfilter can be expressed as(54)Ps,k=Bs,k−1Pg,k.


### 3.3. Procedure of CAFCKF

The structure of CAFCKF is shown in Figure2. The procedure is shown below.Step 1.
Initialization ofPs,o,Xs,o,Qs,o,andRs,o.Step 2.
State prediction through equations (23)–(27).Step 3.
Estimation of predicted measurement covariance through equations (38)–(44).Step 4.
Calculation of the gain matrix of the subfilter through equations (28)–(34).Step 5.
State updation of the subfilter through equations (35) and (36).Step 6.
Calculation of the defined information fusion factor through equations (45)–(47).Step 7.
Global fusion through equations (48) and (50).Step 8.
Calculation of the information sharing factor through equations (50)–(53).Step 9.
Information sharing through equation (54).Step 10.
Repeat Steps 2–9 until the fusion ends.Figure 2
Procedure of CAFCKF.


## 3.1. Federated Cubature Kalman Filter

To improve the computation efficiency and reliability, a federated cubature Kalman filter (FCKF) is established and used for the multisensor nonlinear system. Its procedure is shown below.Step 1.
State prediction:(23)ηi,s,k|k−1=Ss,k−1|k−1ξi+Xk−1,where, Sk−1|k−1Sk−1|k−1T=Ps,k−1.(24)xi,s,k|k−1=fηi,s,k|k−1,where f· denotes the state equation and ξi denotes the ith element of ξd×2d.
ξd×2d can be expressed as(25)ξd×2d=10⋯0−10⋯00100−10⋮⋮⋱⋮⋮⋱00⋯100⋯−1d×2d,where d is the dimension of system state.(26)Xs,k|k−1=∑i=12dwixi,s,k|k−1,(27)Ps,k|k−1=∑i=12dwixi,s,k|k−1xi,s,k|k−1T+Xs,k|k−1Xs,k|k−1T+Qs,k,where wi=1/2d.Step 2.
Calculate the gain matrix of the subfilter:(28)ςi,s,k|k−1=Ss,k|k−1ξ+Xs,k|k−1,(29)Ss,k|k−1Ss,k|k−1T=Ps,k|k−1,(30)zi,s,k|k−1=hsςi,k|k−1+rs,k,(31)Zs,k|k−1=∑i=12dwizi,s,k|k−1.
The prediction innovation covariance matrix and cross-covariance matrix of subfilters can be written as(32)Ps,k|k−1zz=∑i=12dwizi,s,k|k−1zi,s,k|k−1T+Zs,k|k−1Zs,k|k−1T+Rs,k,(33)Ps,k|k−1xz=∑i=12dwiςi,k|k−1zi,s,k|k−1T+Xk|k−1Zs,k|k−1T,where Zs,k|k−1 denotes the measurement prediction of subsystem s.
The gain of subfilters can be written as(34)Ks,k=Ps,k|k−1xzPs,k|k−1zz−1.Step 3.
Update state of the subfilter as(35)X^s,k=Xk|k−1+Ks,kZs,k−Zs,k|k−1,(36)P^s,k=Pk|k−1+Ks,kTPs,k|k−1zzKs,k,where Ps,k is the covariance matrix of subfilter s and X^s,k is the state estimation of subfilter s.Step 4.
Calculate the global fusion through the main filter:(37)Pg,k=∑Ps,k−1−1,X^g,k=Pg,k∑Ps,k−1X^s,k,where Pg,k is the covariance matrix of the global filter and X^g,k is the state estimation of the global filter.


## 3.2. Establishment of the Chi-Square Test-Based Adaptive FCKF (CAFCKF)

In reality, the statistic characteristics of measurement noise are always unknown or changed with time. Under this situation, the estimator has great difficulty in ensuring the system accuracy. To address this problem, a noise estimator for the measurement noise statistics based on the chi-square test (CST) is firstly proposed in this paper.The innovation ofsth subsystem is defined as(38)νs,k=Zs,k−Zs,k/k−1.The hypothesis test based on innovation can be constructed as(39)H0:EνsνsT=Ps,k|k−1zz,noise statistic unchanged,H1:EνsνsT=νs,k−μ^s,kνs,k−μ^s,kT≠Ps,k|k−1zz,noise statistic changed.Then, calculate(40)rs,k=1M∑j=1Mνs,k−j+1.According to the principle of CST, the judge index can be expressed as(41)Js,k=νs,k−rs,kTPs,k|k−1zz−1νs,k−rs,k,where Js,k∼χ2m.According to the hypothesis test, by setting the significance levelα (0 < α < 1), there will be a threshold Ts, which makes α follow [28](42)PJs,k>Ts=α.When the statistical characteristics of measurement noise are unchanged,λk will be smaller than Ts. Otherwise, the judge index will exceed the threshold, and the covariance of predicted measurement should be adjusted according to(43)fPs,k|k−1zz=JPs,k|k−1zz−Ts.Based on Newton’s method, we have(44)Ps,k|k−1zzi+1=Ps,k|k−1zzi+νs,k−rs,kTPs,k|k−1zzi−1−1νs,k−rs,k−Tsνs,k−rs,kTPs,k|k−1zzi−2νs,k−rs,kT,where i denotes the times of iteration in Newton’s method.SettingP^s,kzz0=Ps,k|k−1zz, the estimation of measurement noise covariance can be obtained by running the iteration of (44) until fP^s,kzzi is less than 0. To limit the times of iteration, a cutoff time C is set to 5, which means if JP^s,kzz is not less than the threshold when the times of iteration reach C, the iteration will end.Subsequently, to improve the performance of the information procedure, a new adaptive information fusion factor is defined in FCKF. With the update of state estimation through the subfilter, a judge index of the chi-square test can be calculated according to the state estimation and the state prediction from the subfilter:(45)JX,s,k=X^s,k−Xk||k−1TPs,k||k−1−1X^s,k−Xk||k−1,where JX,s,k denotes the state judge index of subfilter s.Since state estimation error of a subsystem increases asJX,s,k increases, the information fusion factor under the chi-square test can be defined as(46)As,k=diagAsk,Ask,…Askm×m,where(47)Ask=1/JX,s,k∑1/JX,s,k.Then, the global fusion of FCKF can be modified as(48)Pg,k=∑As,kPs,k−1−1,(49)X^g,k=Pg,k∑As,kPs,k−1X^s,k.After the global fusion process, the information sharing factor for each subfilter is needed to refine the estimation of each subsystem. The judge index of the chi-square test calculated by the global state estimation and the local state estimation can be written as(50)JX,sg,k=X^g,k−X^s,kTPg,k−1X^g,k−X^s,k.WhenJX,g,k is larger than those of other subsystems, the real contribution of the sth subsystem to the integration system is smaller than that of other subsystems, which means this subsystem should have a bigger state covariance than others. So, we refine the information sharing factor by calculating it as(51)Bs,k=diagBskBsk⋯Bskm×m,where(52)Bsk=1/JX,sg,k∑1/JX,sg,k.It is known that(53)∑s=1LBs,k=Im×m.Thus, the refined state covariance of the subfilter can be expressed as(54)Ps,k=Bs,k−1Pg,k.


## 3.3. Procedure of CAFCKF

The structure of CAFCKF is shown in Figure2. The procedure is shown below.Step 1.
Initialization ofPs,o,Xs,o,Qs,o,andRs,o.Step 2.
State prediction through equations (23)–(27).Step 3.
Estimation of predicted measurement covariance through equations (38)–(44).Step 4.
Calculation of the gain matrix of the subfilter through equations (28)–(34).Step 5.
State updation of the subfilter through equations (35) and (36).Step 6.
Calculation of the defined information fusion factor through equations (45)–(47).Step 7.
Global fusion through equations (48) and (50).Step 8.
Calculation of the information sharing factor through equations (50)–(53).Step 9.
Information sharing through equation (54).Step 10.
Repeat Steps 2–9 until the fusion ends.Figure 2
Procedure of CAFCKF.


## 4. Results and Discussion

A simulation case study is conducted to verify the performance of the proposed CAFCKF for the SINS/GNSS/SRS/CNS integrated navigation system. In the simulation, a comparison between the proposed CAFCKF, FCKF, and cubature rule-based distributed optimal fusion (CRDOF) [28] is made and discussed. The dynamic flight trajectory of HCVs is shown in Figure 3. The parameters set in the simulation are listed in Table 1. The entire simulation time is set to 1000 s, and the filtering period is set to 1 s.Figure 3
Dynamic flight trajectory.Table 1
Parameters in simulations.
Initial positionEast longitude108.9°North latitude34.025°Altitude40 kmInitial velocityEast151 m/sNorth125 m/sUp125 m/sInitial attitudePitch0°Roll0°Yaw0°Gyro parametersConstant drift0.5°/hWhite noise0.5°/hSampling frequency10 HzAccelerometer parametersZero bias0.1 mgWhite noise0.1 mgSampling frequency10 HzGNSS parametersPseudorange measurement error20 mPseudorange rate measurement error0.5 m/sSampling frequency10 HzSRS parametersRedshift measurement error10^-8Sampling frequency1HzCNS parametersMeasurement error15mSampling frequency1HzBarometric altimeter parameterAltitude measurement error10mSampling frequency1HzThe initial value ofX0, P0, r0, q0, R0, and Q0 is selected as(55)X^0=0.1°,0.1°,0.1°,0.3m/s,0.3m/s,0.3m/s,10m,10m,10m,1°/h,1°/h,1°/h,2mg,2mg,2mg,50m,1m/sT,P^0=diag0.1°2,0.1°2,0.1°2,0.3m/s2,0.3m/s2,0.3m/s2,10m2,10m2,10m2,1°/h2,1°/h2,1°/h2,2mg2,2mg2,2mg2,50m2,1m/s2,r0=0,q0=0,Q=diag0.5°/h2,0.5°/h2,0.5°/h2,0.1mg2,0.1mg2,0.1mg2.With the aforementioned simulated parameters, the effects of the proposed CAFCKF, FCKF, and CRDOF on the SINS/GNSS/SRS/CNS integrated navigation system are analyzed. The GNSS, SRS, and CNS are given with changed measurement noise and described as condition 1:(56)vGNSS∼N0,RGNSS,0s<k≤300s,N0,4RGNSS,200s<k≤300s,N0,RGNSS,300s<k≤1000s,vSRS∼N0,RSRS,0s<k≤300s,N0,5RSRS,500s<k≤600s,N0,RSRS,600s<k≤1000s,vCNS∼N0,RCNS,0s<k≤300s,N0,6RCNS,800s<k≤900s,N0,RCNS,900s<k≤1000s,where RGNSS=diag20m2I3×3,0.5m/s2I3×3, RCNS=diag15m2,15m2,10m2, and RSRS=diag0.5m/s2,0.5m/s2,0.5m/s2; these are also the initial values of R0 for each subfilter.Under condition 1, the estimation error curve of the velocity and position using CAFCKF, FCKF, and CRDOF is shown in Figure4. The mean absolute error of the velocity and position is listed in Table 2.Figure 4
Error curve of velocity and position under measurement noise with changed statistical characteristics under condition 1.Table 2
Mean absolute error of different algorithms under the measurement noise with changed statistical characteristics under condition 1.
AlgorithmError typeMean absolute error(200 s, 300 s)(500 s, 600 s)(800 s, 900 s)Other timesFCKFVelocity0.195 m/s0.189 m/s0.141 m/s0.133 m/sPosition19.36 m6.46 m17.66 m/s6.36 mCRDOFVelocity0.133 m/s0.149 m/s0.134 m/s0.126 m/sPosition6.64 m6.15 m6.86 m6.77 mCAFCKFVelocity0.121 m/s0.103 m/s0.093 m/s0.116 m/sPosition3.57 m3.24 m3.98 m3.43 mAs shown in Figure4 and Table 2, during the time interval of (200 s, 300 s), with the changed covariance in the measurement noise of SINS/GNSS subsystem, the biggest fluctuation occurs for both the error curve of velocity and position by the FCKF. Compared to FCKF, smaller position and velocity errors are obtained by the CRDOF. The mean absolute error of velocity and position reaches 0.133 m/s and 6.64 m. For the chi-square-based noise estimation algorithm and the adaptive information fusion factors, smaller mean absolute errors are obtained by the CAFCKF than those of FCKF, which are about 0.121 m/s and 3.57 m. During the time interval of (500 s, 600 s), the error curve of velocity by the FCKF fluctuates abruptly, and the mean absolute error of velocity is about 0.189 m/s. On the contrary, compared to the FCKF, a smaller mean absolute error of velocity is obtained by the CRDOF, which is 0.149 m/s. Compared to the CRDOF, a smaller mean absolute error of velocity is obtained by the CAFCKF, which is about 0.1 m/s. During the interval of (800 s,900 s), the covariance of the measurement noise is not adjusted and the contribution of each subsystem is not ensured for the FCKF, leading to the biggest fluctuation in the position error curve. Compared to the FCKF, a more smooth error curve is obtained by the CAFCKF. Meanwhile, a smaller mean absolute error of position, which is 3.43 m, is obtained by the CAFCKF.To verify the universality under condition 1, the ARMSE of velocity and position is calculated after 50 Monte Carlo runs and shown in Figure5.Figure 5
ARMSE of position and velocity under condition 1.As shown in Figure5, the FCKF results in the biggest ARMSE during all time intervals. During the time interval of (200 s, 300 s), the ARMSE of velocity and position is 0.14 m/s and 4.57 m for the CAFCKF, which is 22% and 39% smaller than that of CRDOF. During the time interval of (500 s, 600 s), the ARMSE of velocity and position is 0.11 m/s and 4.34 m for the CAFCKF, which is 36% and 37% smaller than that of CRDOF. During the interval of (800 s, 900 s), the ARMSE of velocity and position is 0.12 m/s and 4.68 m/s for the CAFCKF, which is 20% and 77% smaller than that of CRDOF.The GNSS, SRS, and CNS are given with another changed measurement noise and described as condition 2:(57)vGNSS∼N0,RGNSS,0s<k≤300s,N3rGNSS,RGNSS,200s<k≤300s,N0,RGNSS,300s<k≤1000s,vSRS∼N0,RSRS,0s<k≤300s,N5rSRS,RSRS,500s<k≤600s,N0,RSRS,600s<k≤1000s,vCNS∼N0,RCNS,0s<k≤300s,N4rCNS,RCNS,800s<k≤900s,N0,RCNS,900s<k≤1000s,where rGNSS=20m;20m;20m;0.5m/s;0.5m/s;0.5m/s, rCNS=15m,15m,10m, and rSRS=0.5m/s,0.5m/s,0.5m/s.Under condition 2, the estimation error curve of the velocity and position using CAFCKF, FCKF, and CRDOF is shown in Figure6. The mean absolute error of the velocity and position is listed in Table 3.Figure 6
Error curve of velocity and position under measurement noise with changed statistical characteristics under condition 2.Table 3
Mean absolute error of different algorithms under the measurement noise with changed statistical characteristics under condition 2.
AlgorithmError typeMean absolute error(200 s, 300 s)(500 s, 600 s)(800 s, 900 s)Other timesFCKFVelocity1.73 m/s1.84 m/s0.69 m/s0.82 m/sPosition71.36 m21.36 m64.14 m/s10.36 mCRDOFVelocity0.78 m/s0.65 m/s0.123 m/s0.14 m/sPosition10.57 m5.33 m12.58 m5.44 mCAFCKFVelocity0.33 m/s0.21 m/s0.093 m/s0.15 m/sPosition6.57 m3.69 m4.58 m4.92 mAs shown in Figure6 and Table 3, during the time interval of (200 s, 300 s), the error of velocity and position by the FCKF obviously increases. Its mean absolute errors in velocity and position are 1.73 m/s and 71m, which are the biggest among these methods. The mean absolute errors of velocity and position by the CAFCKF, which are 0.33 m/s and 6.57 m, are smaller than those of CRDOF. During the time interval of (500 s, 600 s), the mean absolute errors of velocity and position by the FCKF are about 1.84 m/s and 21.36 m. The CAFCKF and CRDOF present better performance of information fusion than FCKF. The mean absolute errors by the CAFCKF are smaller than those of CRDOF. During the time interval of (800 s, 900 s), the FCKF presents the biggest error in both velocity and position. The mean absolute errors are 0.69 m/s and 64.14 m. In contrast, the smallest mean absolute errors of velocity and position are obtained by the CAFCKF, which are 0.1093 m/s and 4.58 m.Under condition 2, the ARMSE of velocity and positions is also calculated after 50 Monte Carlo runs and shown in Figure7.Figure 7
ARMSE of position and velocity under condition 2.As shown in Figure7, during the time interval of (200 s, 300 s), the ARMSE of velocity and position is 0.43 m/s and 7.67 m by the CAFCKF, which is 62% and 40% smaller than that of CRDOF. During the time interval of (500 s, 600 s), the ARMSE of velocity and position is 0.31 m/s and 4.33 m by the CAFCKF, which is 56% and 21% smaller than that of CRDOF. During the time interval of (800 s, 900 s), the ARMSE of velocity and position by the CAFCKF is 0.11 m/s and 5.58 m, which is 22% and 62% smaller than that of CRDOF. Furthermore, without adaptive information fusion factor, the biggest ARMSE is obtained by the FCKF.


## 5. Conclusions

In this paper, a SINS/GNSS/SRS/CNS integrated navigation system and chi-square test-based adaptive federated cubature Kalman filter (CAFCKF) are proposed to improve the accuracy and reliability of the navigation system in HCVs. In the proposed CAFCKF algorithm, the chi-square test is used to estimate the statistical characteristics of measurement noise firstly. An adaptive information fusion factor and the information sharing factor are also designed by using the judge index of the chi-square test, which optimize the contribution of each subsystem and lead to a more accurate global state estimation. Finally, the proposed CAFCKF algorithm is verified through simulation. The result shows that, compared with other filtering methods, the CAFCKF leads to the smallest mean absolute errors in velocity and position, demonstrating that it can be used to improve the accuracy and stability of the navigation system.



---

*Source: 7588265-2022-03-24.xml*