# Reliable Distributed Integrated Navigation Based on CI during Mars Entry

Authors: Tai-shan Lou; Nan-hua Chen; Xiao-qian Wang; Zhen-dong He; Jie Liu
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

A reliable distributed covariance intersection (CI) fusion integrated navigation algorithm with information feedback during the Mars atmospheric entry is proposed to meet robust, reliable, and high-precision Mars atmospheric entry navigation strategy. A distributed integrated navigation scheme includes four independent subsystems consisting of an IMU and one radio beacon, but each subsystem is weakly observable under limited Mars entry measurements. The scalar weights are fast obtained by maximizing the information contribution of the corresponding estimate. The distributed framework based upon the CI fusion algorithm is designed by using dynamic information distribution coefficients and information feedback strategy. This distributed fusion approach could meet the lower computation cost and robust and reliable capacity and especially is beneficial to the weakly observable or unobservable subsystems during the Mars entry navigation scheme. Numerical simulations show that the proposed distributed CI fusion integrated navigation algorithm can provide consistent navigation accuracy for the Mars entry vehicle and improve the entry navigation robustness under the weakly observable navigation subsystems.


---

## Body

## 1. Introduction

Future Mars exploration missions require the capability of safely pinpoint landing on a high scientific site to verify some science imaginations or carry out scientific experiments. The Mars entry phase is an important and dangerous period during the Mars entry, descent, and landing (EDL) phases, and its navigation accuracy and reliability play a vital role in the Mars landing mission. Robust, reliable, and high-precision Mars atmospheric entry navigation technology during Mars entry is one of the key EDL autonomous navigation technologies [1, 2].However, many sensors are encapsulated into the heat shield and plasma sheath to insulate high heat, and only the inertial measurement unit (IMU) is usable during the Mars entry phase. To meet the requirements of the higher navigation accuracy, the Mars networks based the radio communication between the Mars entry vehicle and an orbiting radio satellite, or a preset fixed surface radio beacon is proposed [3, 4]. Subsequently, many new Mars navigation scenarios based on new navigation filtering technologies are proposed to solve some key navigation problems, which include the observability [5–7], uncertain parameters [8–11], and biases [12]. For the above navigation scenarios, the designed navigation frameworks are all centralized, and the measurement information is assumed to send the centralized filtering at the same time to estimate the states. The above centralized navigation schemes all obtained satisfying navigation accuracy to meet the requirements of the Mars entry navigation, but the reliability of the navigation strategies is neglected when the communication between the entry vehicle and the Mars navigation networks breaks off or for other reasons.Compared to the centralized navigation framework, the two advantages of the distributed navigation framework could provide reliable and flexible estimations of the system [13]. The key idea of the distributed filtering is that all the measurement information is processed in parallel at each node to estimate the states, and then, these estimates are sent to a fusion node to fuse them as an overall estimate. As an important concept in the data fusion field, the distributed filtering has been widely applied in many multisensor data fusion systems [14–16].For the Mars entry navigation system, there are only finite sensors to provide the navigation information. The on-board IMU could always provide three components of acceleration and three components of the angular rate with random biases. Considering the finite range measurement information coming from finite radio beacons, each subsystem of the distributed navigation scenario is designed including the IMU and only one radio beacon. This design of the subsystem brings another problem; that is, each subsystem is weakly observable. The weakly observability of each Mars entry navigation subsystem may bring unforeseeable navigation errors when the state estimates are fused in the fusion node [5].This paper presented a reliable distributed covariance intersection (CI) fusion integrated navigation algorithm with information feedback during the Mars atmospheric entry phase to meet the requirement for the future Mars exploration missions. Four navigation subsystems are designed to independently estimate the Mars entry vehicle state, and then, these estimations are sent to the master filtering and fused by using the CI algorithm with information feedback to provide consistent estimate results for the Mars entry navigation scheme. In addition, a new fast-distributed scalar weight selection scheme is designed to obtain the scalar weight by using the infinite norm of the matrix.The paper is organized as follows. Section2 introduces the Mars entry dynamical system and analyzes observability of the distributed subsystems for the Mars entry. Section 3 presents the reliable distributed CI fusion algorithm with information feedback based on the extended Kalman filter (EKF) and CI fusion algorithm. Section 4 discusses the results from numerical simulations. The conclusions are summarized in Section 5.


## 2. Mars Entry Navigation Dynamical System

### 2.1. Dynamical Model

For the sake of simplicity, the entry vehicle is assumed to fly in a stationary and quiet atmosphere of a nonrotating planet. The entry dynamics equations of the Mars entry vehicle in the Mars-centered Mars-fixed coordinate system are given as [3, 17]
(1)ṙ=vsinγ,v̇=−D−grsinγ,γ̇=vr−grvcosγ+Lvcosϕ,θ̇=vcosγsinψrcosλ,λ̇=vcosγcosψr,ψ̇=vrsinψcosγtanλ+Lvcosγsinϕ,where the altitude r is the distance from the mass center of the entry vehicle to the center of the Mars; v is the radial velocity of the entry vehicle; γ is the flight path angle (FPA); θ is the longitude; and λ is the latitude. The azimuth angle ψ is defined as a clockwise rotation angle starting at due north, and ϕ is the bank angle, which is zero in this work. The gravitational acceleration is gr=μ/r2, and μ is the gravitational constant of the Mars. D and L are, respectively, the aerodynamic drag and lift accelerations given by
(2)D=CDSmq¯,L=D·L/D,where CL and CD are the vehicle lift and drag coefficients, respectively; L/D is the lift-to-drag ratio; S represents the vehicle reference surface area; m is the mass of the vehicle: q¯=ρv2/2 is the dynamic pressure; and B=m/CDS is called the ballistic coefficient. ρ is the Mars atmospheric density, and here, an exponential Mars atmospheric density model is assumed as follows:
(3)ρ=ρ0expr0−rhs,where ρ0 is the density on the surface of the Mars, r0=3437.2 km is the reference radial position (40 km above the Mars surface), and hs is the constant scale height (hs=7.5 km).


### 2.2. Measurement Model

#### 2.2.1. IMU Measurement

The accelerometers of the IMU provide the specific force components along three orthogonal axes, and three components of acceleration are given by(4)a~=a+ba+ηa,where a~ is the accelerometer output along body axes, a is the true linear acceleration, ba is the random acceleration bias, and ηa is the zero-mean Gaussian white noise.Here, the true accelerometer measurement model is defined by(5)a=−D−D·L/D·sinϕD·L/D·cosϕT.


#### 2.2.2. Two-Way Range Measurement

The two-way range radio measurement provides the distance between the entry vehicle and the radio beacon [18]. The two-way range measurement R~ is defined by
(6)R~i=Ri+ξRi,Ri=rl−riTrl−ri,where Ri is the true two-way range measurements; rl is the vehicle’s position vector; ri is the position vector of ith radio beacon; and ξRi is the ith measurement noise, which is distributed to the zero-mean Gaussian distribution.


### 2.3. Framework of Distributed Integrated Navigation

In the Mars entry integrated navigation scheme, there are orbiting radio satellites and two surface radio beacons within sight to provide the two-way range measurements by the radio communication. That is to say, the measurement information comes from the IMU and four radio beacons in the Mars trajectory or on the Mars surface. The integrated navigation scenario of IMU/radio measurement based on the Mars network is in Figure1.Figure 1
Integrated navigation scenario of IMU/radio measurement based on the Mars network.


#### 2.3.1. Dynamic Equation

During the Mars entry phase,x=rθλvγψρL/DT is defined as the state variables of the entry vehicle, in which the Mars atmospheric density ρ and the lift-to-drag ratio L/D are augmented. The dynamic model in equation (1) is rewritten with the noise term wt as follows:
(7)ẋt=fxt,t+wt=vsinγ−D−grsinγvr−grvcosγ+Lvcosϕvcosγsinψrcosλvcosγcosψrvrsinψcosγtanλ+Lvcosγsinϕ00+wt.


#### 2.3.2. Centralized Navigation Scheme

For the centralized navigation scheme, the measurement information from the IMU and the radio beacons are sent to a single filter to estimate the states. So, the measurement equation for the centralized navigation scheme is defined as [3, 8, 10]
(8)zt=hxt,t+vt=av+baR1R2R3R4+ηaξR1ξR2ξR3ξR4.


#### 2.3.3. Distributed Navigation Scheme

For the distributed navigation scheme, the measurement information is processed in parallel at each node to obtain multiple estimates, and then, these estimates are sent to a fusion node to fuse them as an overall estimate.In this work, the IMU is set as the public reference sensor system, and the four radio beacons are separated into four observation subsystems. That is to say, there are four subsystems in this simulation, which are defined as(9)zit=hixt,t+vit=av+baRi+ηaξRi,i=1,2,3,4.


### 2.4. Observability Analysis for the Navigation Scheme

Observability is always used to evaluate the performance of the navigation capability and could be observed from measurement information. When the navigation scheme has poor observability degree, the state estimations of the navigation system might be divergent or even unstable [3, 6, 19, 20].Consider a nonlinear dynamic model:(10)∑:ẋ=fx,z=hx,where x∈Rn is the process state vector and z∈Rm is the measurement vector; f⋅ and h⋅ are the nonlinear functions.The locally weakly observability is analyzed by the Lie algebra for the nonlinear navigation systemsΣ, whose observability matrix is given by
(11)OΣ=dLf0hdLf1h⋮dLfn−1h,where the kth-order Lie derivative is given by
(12)Lf0h=h,Lfkh=∂Lfk−1h∂xf,k=1,2,…,n−1,and the differential of Lfkh is defined as
(13)dLfkh=∂Lfkh∂x,k=0,1,…,n−1.For the navigation system at statex=x0, the local observability means the rank of OΣx=x0 in equation (11) is equal to n, which is the dimensional of the state [21].To represent the observability of the nonlinear system, the observability degree is given by(14)δ=σminOΣσmaxOΣ,where σmaxOΣ and σminOΣ are the maximum and minimum singular numbers of the observability matrix OΣ. So, it can be seen that 0≤δ≤1. δ=0 means rankOΣ<n, and the above system is locally unobservable; δ>0 means rankOΣ=n, and the above system is locally weakly observable.For the above two navigation schemes, Lie algebra and the singular value decomposition (SVD) method (the details of the SVD are in reference [6]) are associated with analyzing the observability degree of the navigation system.Figure2 shows the observability degree of the centralized navigation system with EKF, in which the Mars atmospheric density and the lift-to-drag ratio are augmented into the states. The maximum and minimum observability degrees of the centralized navigation system are 10−8 and 10−7 in Figure 2, but for the distributed subsystems, their observability degrees are between 10−16 and 10−13 in Figure 3, and each subsystem becomes weakly observable. The weakly observable navigation system may bring unforeseeable navigation errors during Mars entry [22]. To avoid this problem, a new CI fusion algorithm is proposed to deal with this weakly observable navigation scenario.Figure 2
Observability degree of the centralized navigation system.Figure 3
Observability degree of the navigation system.


## 2.1. Dynamical Model

For the sake of simplicity, the entry vehicle is assumed to fly in a stationary and quiet atmosphere of a nonrotating planet. The entry dynamics equations of the Mars entry vehicle in the Mars-centered Mars-fixed coordinate system are given as [3, 17]
(1)ṙ=vsinγ,v̇=−D−grsinγ,γ̇=vr−grvcosγ+Lvcosϕ,θ̇=vcosγsinψrcosλ,λ̇=vcosγcosψr,ψ̇=vrsinψcosγtanλ+Lvcosγsinϕ,where the altitude r is the distance from the mass center of the entry vehicle to the center of the Mars; v is the radial velocity of the entry vehicle; γ is the flight path angle (FPA); θ is the longitude; and λ is the latitude. The azimuth angle ψ is defined as a clockwise rotation angle starting at due north, and ϕ is the bank angle, which is zero in this work. The gravitational acceleration is gr=μ/r2, and μ is the gravitational constant of the Mars. D and L are, respectively, the aerodynamic drag and lift accelerations given by
(2)D=CDSmq¯,L=D·L/D,where CL and CD are the vehicle lift and drag coefficients, respectively; L/D is the lift-to-drag ratio; S represents the vehicle reference surface area; m is the mass of the vehicle: q¯=ρv2/2 is the dynamic pressure; and B=m/CDS is called the ballistic coefficient. ρ is the Mars atmospheric density, and here, an exponential Mars atmospheric density model is assumed as follows:
(3)ρ=ρ0expr0−rhs,where ρ0 is the density on the surface of the Mars, r0=3437.2 km is the reference radial position (40 km above the Mars surface), and hs is the constant scale height (hs=7.5 km).


## 2.2. Measurement Model

### 2.2.1. IMU Measurement

The accelerometers of the IMU provide the specific force components along three orthogonal axes, and three components of acceleration are given by(4)a~=a+ba+ηa,where a~ is the accelerometer output along body axes, a is the true linear acceleration, ba is the random acceleration bias, and ηa is the zero-mean Gaussian white noise.Here, the true accelerometer measurement model is defined by(5)a=−D−D·L/D·sinϕD·L/D·cosϕT.


### 2.2.2. Two-Way Range Measurement

The two-way range radio measurement provides the distance between the entry vehicle and the radio beacon [18]. The two-way range measurement R~ is defined by
(6)R~i=Ri+ξRi,Ri=rl−riTrl−ri,where Ri is the true two-way range measurements; rl is the vehicle’s position vector; ri is the position vector of ith radio beacon; and ξRi is the ith measurement noise, which is distributed to the zero-mean Gaussian distribution.


## 2.2.1. IMU Measurement

The accelerometers of the IMU provide the specific force components along three orthogonal axes, and three components of acceleration are given by(4)a~=a+ba+ηa,where a~ is the accelerometer output along body axes, a is the true linear acceleration, ba is the random acceleration bias, and ηa is the zero-mean Gaussian white noise.Here, the true accelerometer measurement model is defined by(5)a=−D−D·L/D·sinϕD·L/D·cosϕT.


## 2.2.2. Two-Way Range Measurement

The two-way range radio measurement provides the distance between the entry vehicle and the radio beacon [18]. The two-way range measurement R~ is defined by
(6)R~i=Ri+ξRi,Ri=rl−riTrl−ri,where Ri is the true two-way range measurements; rl is the vehicle’s position vector; ri is the position vector of ith radio beacon; and ξRi is the ith measurement noise, which is distributed to the zero-mean Gaussian distribution.


## 2.3. Framework of Distributed Integrated Navigation

In the Mars entry integrated navigation scheme, there are orbiting radio satellites and two surface radio beacons within sight to provide the two-way range measurements by the radio communication. That is to say, the measurement information comes from the IMU and four radio beacons in the Mars trajectory or on the Mars surface. The integrated navigation scenario of IMU/radio measurement based on the Mars network is in Figure1.Figure 1
Integrated navigation scenario of IMU/radio measurement based on the Mars network.


### 2.3.1. Dynamic Equation

During the Mars entry phase,x=rθλvγψρL/DT is defined as the state variables of the entry vehicle, in which the Mars atmospheric density ρ and the lift-to-drag ratio L/D are augmented. The dynamic model in equation (1) is rewritten with the noise term wt as follows:
(7)ẋt=fxt,t+wt=vsinγ−D−grsinγvr−grvcosγ+Lvcosϕvcosγsinψrcosλvcosγcosψrvrsinψcosγtanλ+Lvcosγsinϕ00+wt.


### 2.3.2. Centralized Navigation Scheme

For the centralized navigation scheme, the measurement information from the IMU and the radio beacons are sent to a single filter to estimate the states. So, the measurement equation for the centralized navigation scheme is defined as [3, 8, 10]
(8)zt=hxt,t+vt=av+baR1R2R3R4+ηaξR1ξR2ξR3ξR4.


### 2.3.3. Distributed Navigation Scheme

For the distributed navigation scheme, the measurement information is processed in parallel at each node to obtain multiple estimates, and then, these estimates are sent to a fusion node to fuse them as an overall estimate.In this work, the IMU is set as the public reference sensor system, and the four radio beacons are separated into four observation subsystems. That is to say, there are four subsystems in this simulation, which are defined as(9)zit=hixt,t+vit=av+baRi+ηaξRi,i=1,2,3,4.


## 2.3.1. Dynamic Equation

During the Mars entry phase,x=rθλvγψρL/DT is defined as the state variables of the entry vehicle, in which the Mars atmospheric density ρ and the lift-to-drag ratio L/D are augmented. The dynamic model in equation (1) is rewritten with the noise term wt as follows:
(7)ẋt=fxt,t+wt=vsinγ−D−grsinγvr−grvcosγ+Lvcosϕvcosγsinψrcosλvcosγcosψrvrsinψcosγtanλ+Lvcosγsinϕ00+wt.


## 2.3.2. Centralized Navigation Scheme

For the centralized navigation scheme, the measurement information from the IMU and the radio beacons are sent to a single filter to estimate the states. So, the measurement equation for the centralized navigation scheme is defined as [3, 8, 10]
(8)zt=hxt,t+vt=av+baR1R2R3R4+ηaξR1ξR2ξR3ξR4.


## 2.3.3. Distributed Navigation Scheme

For the distributed navigation scheme, the measurement information is processed in parallel at each node to obtain multiple estimates, and then, these estimates are sent to a fusion node to fuse them as an overall estimate.In this work, the IMU is set as the public reference sensor system, and the four radio beacons are separated into four observation subsystems. That is to say, there are four subsystems in this simulation, which are defined as(9)zit=hixt,t+vit=av+baRi+ηaξRi,i=1,2,3,4.


## 2.4. Observability Analysis for the Navigation Scheme

Observability is always used to evaluate the performance of the navigation capability and could be observed from measurement information. When the navigation scheme has poor observability degree, the state estimations of the navigation system might be divergent or even unstable [3, 6, 19, 20].Consider a nonlinear dynamic model:(10)∑:ẋ=fx,z=hx,where x∈Rn is the process state vector and z∈Rm is the measurement vector; f⋅ and h⋅ are the nonlinear functions.The locally weakly observability is analyzed by the Lie algebra for the nonlinear navigation systemsΣ, whose observability matrix is given by
(11)OΣ=dLf0hdLf1h⋮dLfn−1h,where the kth-order Lie derivative is given by
(12)Lf0h=h,Lfkh=∂Lfk−1h∂xf,k=1,2,…,n−1,and the differential of Lfkh is defined as
(13)dLfkh=∂Lfkh∂x,k=0,1,…,n−1.For the navigation system at statex=x0, the local observability means the rank of OΣx=x0 in equation (11) is equal to n, which is the dimensional of the state [21].To represent the observability of the nonlinear system, the observability degree is given by(14)δ=σminOΣσmaxOΣ,where σmaxOΣ and σminOΣ are the maximum and minimum singular numbers of the observability matrix OΣ. So, it can be seen that 0≤δ≤1. δ=0 means rankOΣ<n, and the above system is locally unobservable; δ>0 means rankOΣ=n, and the above system is locally weakly observable.For the above two navigation schemes, Lie algebra and the singular value decomposition (SVD) method (the details of the SVD are in reference [6]) are associated with analyzing the observability degree of the navigation system.Figure2 shows the observability degree of the centralized navigation system with EKF, in which the Mars atmospheric density and the lift-to-drag ratio are augmented into the states. The maximum and minimum observability degrees of the centralized navigation system are 10−8 and 10−7 in Figure 2, but for the distributed subsystems, their observability degrees are between 10−16 and 10−13 in Figure 3, and each subsystem becomes weakly observable. The weakly observable navigation system may bring unforeseeable navigation errors during Mars entry [22]. To avoid this problem, a new CI fusion algorithm is proposed to deal with this weakly observable navigation scenario.Figure 2
Observability degree of the centralized navigation system.Figure 3
Observability degree of the navigation system.


## 3. Design of the Distributed Navigation Fusion Algorithm

### 3.1. System Description

Consider the discrete nonlinear dynamic and measurement models defined as(15)xk=fxk−1+wk−1,zk=hxk+vk,where xk is the state vector and zk is the measurement vector. f and h are the nonlinear process and nonlinear measurement vector-valued function, respectively. wk and vk are independent zero-mean Gaussian white noises, and their covariance are positive semidefinite Qk and positive definite Rk, respectively. They satisfy
(16)EwkwjT=Qkδij,EvkvjT=Rkδij,EwkvjT=0,where δkj is the Kronecker delta function.


### 3.2. Extended Kalman Filter

In this work, the classical EKF algorithm is used as the navigation filtering algorithm in the simulation. Here, the classical EKF algorithm is given by three steps as follows:Firstly, initialize the state and covariance asx^0=Ex0 and P0=Ex0−x^0x0−x∧0T, respectively.Secondly, time update equations are(17)x^k−=fx^k−1+,Pk−=Φk/k−1Pk−1+Φk/k−1T+Qk−1.Thirdly, measurement update equations are(18)x^k+=x^k−+Kkzk−hx^k−,Pk+=I−KkHkPk−I−KkHkT+KkRkKkT,Kk=Pk−HkTHkPk−HkT+Rk−1,where the symbol “–” denotes a priori estimate and superscript “+” denotes a posteriori estimate. Φk/k−1=∂fxk−1/∂xk−1xk−1=x^k−1+ and Hk=∂hxk/∂xkxk=x^k− are the coefficient matrices. Pk− and Pk+ are, respectively, the a priori error covariance propagation matrix and the a posteriori state estimate error covariance matrix at time-step k.


### 3.3. Covariance Intersection Fusion Algorithm

Covariance intersection (CI) provides consistent state estimates by combining the estimations and covariance matrices. The estimate accuracy generally is not better than that of the centralized estimates when the cross-covariance is known exactly, because the centralized filter incorporates the cross-covariance, and the CI method only uses the a posterior information to fuse. But, when the cross-covariance is unknown, the CI method still provides a consistent estimate [13]. Compared with the centralized filtering system, the CI fusion algorithm as a distributed filtering can degrade the computation cost and has better fault tolerance.Consider three estimate covariance pairs,x^1,P1, x^2,P2, x^3,P3, and x^4,P4. A consistent estimate with four scalar weights is obtained by
(19)x^CI=PCI∑i=14ωiPi−1x^i,(20)PCI=∑i=14ωiPi−1−1,where ∑i=14ωi=1 and 0≤ωi≤1.Generally, the scalar weightsωi can be obtained by minimizing the trace or determinant of the covariance matrix Pi. Many approaches are proposed to solve the above optimization problem [23–26]. To fastly obtain the suitable scalar weights for real-time applications, some fast solution has been proposed [26, 27]. Franken and Hupper have used the information matrix P−1 of the estimation error variance matrix P as considering estimation certainty [27]. The information matrix represents the information contribution of the corresponding estimate to overall estimation. Inspired by the above theory, the infinite norm of the information matrix P−1 is used to maximize the information contribution of the corresponding estimate. So, the scalar weight selection scheme by using its orientation sensitivity is given by
(21)ωi=Y∞−Y−Yi∞+Yi∞4Y∞+∑j=14Yi∞−Y−Yi∞,Y=Y1+Y2+Y3+Y4,where Yi=Pi−1i=1,2,3,4 are the estimation information matrices. Obviously, the results for four subsystems can be generalized to n subsystems.


### 3.4. Distributed CI Fusion Algorithm with Information Feedback

Generally, the CI fusion algorithm only provides a consistent estimate and does not feedback the fusion information to the subsystems. However, when the subsystems are unobservable, the state maybe has a poor estimate, and the CI fusion results maybe have a poor performance under the above algorithm. So, feeding back the fusion results in the master filter to the local filters for the subsystems is a suitable choice for the Mars entry navigation scheme.In this section, the information feedback rule will be given, and the rule includes the feedbacks of the state estimate and covariance. Figure4 shows the distributed CI fusion algorithm with information feedback (DCIFAIF) for four local filters.Figure 4
CI fusion algorithm with information feedback for four local filters.The dynamic information distribution coefficients of the DCIFAIF are the same as the above scalar weights denotedωii=1,2,3,4. By applying the information distribution coefficients, the information of the master filter is feedback to four subfilters, and the local state estimate is given by
(22)x^i,k+=x^CI,k,Pi,k+=ωi−1PCI,k.The DCIFAIF algorithm is given by three steps as follows:Firstly, initialize the state and covariance asx^CI,0 and PCI,0, respectively.Secondly, the four subsystems all use the EKF algorithm in Section3.2. Each subsystem outputs its a posteriori state estimate x^i+ and covariance Pi+.Finally, the CI fusion estimation of the master filter is given by equations (19) and (20).


## 3.1. System Description

Consider the discrete nonlinear dynamic and measurement models defined as(15)xk=fxk−1+wk−1,zk=hxk+vk,where xk is the state vector and zk is the measurement vector. f and h are the nonlinear process and nonlinear measurement vector-valued function, respectively. wk and vk are independent zero-mean Gaussian white noises, and their covariance are positive semidefinite Qk and positive definite Rk, respectively. They satisfy
(16)EwkwjT=Qkδij,EvkvjT=Rkδij,EwkvjT=0,where δkj is the Kronecker delta function.


## 3.2. Extended Kalman Filter

In this work, the classical EKF algorithm is used as the navigation filtering algorithm in the simulation. Here, the classical EKF algorithm is given by three steps as follows:Firstly, initialize the state and covariance asx^0=Ex0 and P0=Ex0−x^0x0−x∧0T, respectively.Secondly, time update equations are(17)x^k−=fx^k−1+,Pk−=Φk/k−1Pk−1+Φk/k−1T+Qk−1.Thirdly, measurement update equations are(18)x^k+=x^k−+Kkzk−hx^k−,Pk+=I−KkHkPk−I−KkHkT+KkRkKkT,Kk=Pk−HkTHkPk−HkT+Rk−1,where the symbol “–” denotes a priori estimate and superscript “+” denotes a posteriori estimate. Φk/k−1=∂fxk−1/∂xk−1xk−1=x^k−1+ and Hk=∂hxk/∂xkxk=x^k− are the coefficient matrices. Pk− and Pk+ are, respectively, the a priori error covariance propagation matrix and the a posteriori state estimate error covariance matrix at time-step k.


## 3.3. Covariance Intersection Fusion Algorithm

Covariance intersection (CI) provides consistent state estimates by combining the estimations and covariance matrices. The estimate accuracy generally is not better than that of the centralized estimates when the cross-covariance is known exactly, because the centralized filter incorporates the cross-covariance, and the CI method only uses the a posterior information to fuse. But, when the cross-covariance is unknown, the CI method still provides a consistent estimate [13]. Compared with the centralized filtering system, the CI fusion algorithm as a distributed filtering can degrade the computation cost and has better fault tolerance.Consider three estimate covariance pairs,x^1,P1, x^2,P2, x^3,P3, and x^4,P4. A consistent estimate with four scalar weights is obtained by
(19)x^CI=PCI∑i=14ωiPi−1x^i,(20)PCI=∑i=14ωiPi−1−1,where ∑i=14ωi=1 and 0≤ωi≤1.Generally, the scalar weightsωi can be obtained by minimizing the trace or determinant of the covariance matrix Pi. Many approaches are proposed to solve the above optimization problem [23–26]. To fastly obtain the suitable scalar weights for real-time applications, some fast solution has been proposed [26, 27]. Franken and Hupper have used the information matrix P−1 of the estimation error variance matrix P as considering estimation certainty [27]. The information matrix represents the information contribution of the corresponding estimate to overall estimation. Inspired by the above theory, the infinite norm of the information matrix P−1 is used to maximize the information contribution of the corresponding estimate. So, the scalar weight selection scheme by using its orientation sensitivity is given by
(21)ωi=Y∞−Y−Yi∞+Yi∞4Y∞+∑j=14Yi∞−Y−Yi∞,Y=Y1+Y2+Y3+Y4,where Yi=Pi−1i=1,2,3,4 are the estimation information matrices. Obviously, the results for four subsystems can be generalized to n subsystems.


## 3.4. Distributed CI Fusion Algorithm with Information Feedback

Generally, the CI fusion algorithm only provides a consistent estimate and does not feedback the fusion information to the subsystems. However, when the subsystems are unobservable, the state maybe has a poor estimate, and the CI fusion results maybe have a poor performance under the above algorithm. So, feeding back the fusion results in the master filter to the local filters for the subsystems is a suitable choice for the Mars entry navigation scheme.In this section, the information feedback rule will be given, and the rule includes the feedbacks of the state estimate and covariance. Figure4 shows the distributed CI fusion algorithm with information feedback (DCIFAIF) for four local filters.Figure 4
CI fusion algorithm with information feedback for four local filters.The dynamic information distribution coefficients of the DCIFAIF are the same as the above scalar weights denotedωii=1,2,3,4. By applying the information distribution coefficients, the information of the master filter is feedback to four subfilters, and the local state estimate is given by
(22)x^i,k+=x^CI,k,Pi,k+=ωi−1PCI,k.The DCIFAIF algorithm is given by three steps as follows:Firstly, initialize the state and covariance asx^CI,0 and PCI,0, respectively.Secondly, the four subsystems all use the EKF algorithm in Section3.2. Each subsystem outputs its a posteriori state estimate x^i+ and covariance Pi+.Finally, the CI fusion estimation of the master filter is given by equations (19) and (20).


## 4. Simulation Results and Analysis

To evaluate the performance of the proposed DCIFAIF for the IMU/radio measurement integrated navigation scenario during Mars entry, MATLAB/Simulink simulations have been carried out. Three methods which are EKF, CI, and DCIFAIF are used in the Mars entry navigation simulations. The CPU of the simulation computer is Intel® Core™ i3-4150 @3.50 GHz, and the RAM of the computer is DDR3 1600 MHz 12 GB.Initial condition in simulations is all listed in Table1. The Mars gravitational constant is μ=42828.29×109 m3/s2. The true value and the estimated value of the ballistic coefficient of the Mars entry vehicle in simulation are set as 0.016 m2/kg and 0.0176 m2/kg. The acceleration bias coming from IMU is -0.03 m/s2. Four possible beacons are selected to provide the two-way range measurements in the simulations, and their initial position and velocity are listed in Table 2. The initial covariance is P0=diag104,10−6,10−6,102,10−2,10−2,10−2,10−2, process noise covariance is Qk=diag102,10−8,10−8,10,10−4,10−4,10−4,10−4, and measurement noise covariance is Rk=diag10−6,10−6,10−6,20,20,40,40 for the centralized EKF navigation scheme and Rks=diag10−6,10−6,10−6,20 and Rko=diag10−6,10−6,10−6,40 for the surface beacon and the orbiting beacon, respectively.Table 1
Initial condition for integrated navigation.
Initial parameters
True
Estimated
Altitude
3518.2 km
3519.2 km
Velocity
5515 m/s
5525 m/s
FPA
-11.8 deg
-12 deg
Longitude
-89.872 deg
-90.072 deg
Latitude
-28.02 deg
-28.22 deg
Azimuth
5.156 deg
5.356 deg
Surface density
2e-4
2.2e-4
Lift-to-drag ratio
0.156
0.17Table 2
Positions and velocities of beacons.
Beacons
Initial position (km)
Initial velocity (m/s)
Surface beacon 1
(3300, 420, 1350)
(0, 0, 0)
Surface beacon 2
(3290, 570, 755)
(0, 0, 0)
Orbiting beacon 1
(7855.7, -461.8, 749.82)
(66.2, 2206.4, -413)
Orbiting beacon 2
(3492.7, 3659.2, 215.11)
(-605.1, 97, 3232.8)The simulation lasts 300 s, and the sampling frequency is 1 Hz. 5000 simulations are run to compute the error ellipse of the parachute deployment point and the root mean squared error (RMSE).


### 4.1. Simulations with No Fault Beacon

In these simulations, there are two orbiting radio satellites and two surface radio beacons to provide the two-way range measurements for the navigation system all the time. That is to say, there is no fault for the radio measurement system during the Mars entry phase.Figure5 shows the results for three methods in one single simulation. It can be seen that the navigation accuracy of the CI is the worst in three methods because four subsystems are all unobservable. The altitude, velocity, FPA, longitude, and azimuth are divergent in the simulation in Figure 5. The EKF has the best performance in simulation, because all the measurements are sent to the centralized filter algorithm, the system has stronger observability, and the uncertain parameters are augmented into the state to estimate. The performance of the proposed DCIFAIF between the CI and the EKF, the feedback state estimates, and covariance are conducive to degrade the uncertainties in the CI algorithm and increase the navigation accuracy. Furthermore, the required one-step CPU times for the above filters are listed in Table 3. In this table, it can be seen that the proposed DCIFAIF is the most efficient algorithm. The CI algorithm is the slowest, because the suitable scalar weights are computed by the determinant of the covariance.Figure 5
State estimate errors for three algorithms.Table 3
CPU time of one-step simulations in MATLAB.
Filter
EKF
CI
DCIFAIF
CPU time (sec)
0.0417
0.0600
0.0190The state estimate errors and 3σ bounds of the DCIFAIF are shown in Figure 6. From Figure 6, all the six state errors of the DCIFAIF are captured by its three-sigma covariance bounds. Figure 7 shows the information distribution coefficients in the same simulation with Figure 6. Obviously, the coefficients change frequently to adapt the estimates of four subsystems and obtain better navigation accuracy. ω1 and ω2 are the coefficients of the surface radio beacons, and ω3 and ω4 are the coefficients of the orbiting radio beacons. From Figure 6, it can be seen that the surface beacons provide more contribution than the orbiting beacons during the Mars entry phase.Figure 6
State estimate errors and 3σ bounds for DCIFAIF.Figure 7
Information distribution coefficients in one simulation.Figure8 shows the RMSEs of EKF, CI, and DCIFAIF by using the logarithmic scales under 5,000 simulations. Obviously, the CI method has the biggest RMSEs, the EKF has the smallest RMSEs, and the RMSEs of the proposed DCIFAIF are between that of CI and that of EKF.Figure 8
RMSEs for three methods during Mars entry.


### 4.2. Simulations with One Fault Beacon

In these simulations, one beacon breaks down in some times and cannot provide the two-way range measurements for the navigation system. Here, we assume that the last orbiting beacon has a fault after 200 s in the simulations, and only three two-way range measurements are provided for the navigation system during the Mars entry phase.Figure9 shows the state estimate errors and 3σ bounds of the DCIFAIF in one single simulation when the last orbiter has a fault from 200 s to 300 s. From Figure 9, the navigation errors become bigger when the fault occurs, but all the six state errors of the DCIFAIF are also captured by its three-sigma covariance bounds.Figure 9
State estimate errors and 3σ bounds for DCIFAIF when fault occurs.Figure10 shows the RMSEs of EKF, CI, and DCIFAIF under 5,000 simulations when the last orbiter has a fault from 200 s to 300 s. In these simulations, the EKF is divergent from 200 s to 300 s, because no measurements are provided for the centralized navigation system. Obviously, the results of the CI and proposed DCIFAIF all become worse when the fault occurs. Also, the proposed DCIFAIF has a better performance in altitude, longitude, and latitude.Figure 10
RMSEs for three methods when faults occur during Mars entry.As a whole, the proposed DCIFAIF uses the distributed framework for the Mars entry navigation scheme and provides robust and reliable state estimates for the Mars entry vehicle.


## 4.1. Simulations with No Fault Beacon

In these simulations, there are two orbiting radio satellites and two surface radio beacons to provide the two-way range measurements for the navigation system all the time. That is to say, there is no fault for the radio measurement system during the Mars entry phase.Figure5 shows the results for three methods in one single simulation. It can be seen that the navigation accuracy of the CI is the worst in three methods because four subsystems are all unobservable. The altitude, velocity, FPA, longitude, and azimuth are divergent in the simulation in Figure 5. The EKF has the best performance in simulation, because all the measurements are sent to the centralized filter algorithm, the system has stronger observability, and the uncertain parameters are augmented into the state to estimate. The performance of the proposed DCIFAIF between the CI and the EKF, the feedback state estimates, and covariance are conducive to degrade the uncertainties in the CI algorithm and increase the navigation accuracy. Furthermore, the required one-step CPU times for the above filters are listed in Table 3. In this table, it can be seen that the proposed DCIFAIF is the most efficient algorithm. The CI algorithm is the slowest, because the suitable scalar weights are computed by the determinant of the covariance.Figure 5
State estimate errors for three algorithms.Table 3
CPU time of one-step simulations in MATLAB.
Filter
EKF
CI
DCIFAIF
CPU time (sec)
0.0417
0.0600
0.0190The state estimate errors and 3σ bounds of the DCIFAIF are shown in Figure 6. From Figure 6, all the six state errors of the DCIFAIF are captured by its three-sigma covariance bounds. Figure 7 shows the information distribution coefficients in the same simulation with Figure 6. Obviously, the coefficients change frequently to adapt the estimates of four subsystems and obtain better navigation accuracy. ω1 and ω2 are the coefficients of the surface radio beacons, and ω3 and ω4 are the coefficients of the orbiting radio beacons. From Figure 6, it can be seen that the surface beacons provide more contribution than the orbiting beacons during the Mars entry phase.Figure 6
State estimate errors and 3σ bounds for DCIFAIF.Figure 7
Information distribution coefficients in one simulation.Figure8 shows the RMSEs of EKF, CI, and DCIFAIF by using the logarithmic scales under 5,000 simulations. Obviously, the CI method has the biggest RMSEs, the EKF has the smallest RMSEs, and the RMSEs of the proposed DCIFAIF are between that of CI and that of EKF.Figure 8
RMSEs for three methods during Mars entry.


## 4.2. Simulations with One Fault Beacon

In these simulations, one beacon breaks down in some times and cannot provide the two-way range measurements for the navigation system. Here, we assume that the last orbiting beacon has a fault after 200 s in the simulations, and only three two-way range measurements are provided for the navigation system during the Mars entry phase.Figure9 shows the state estimate errors and 3σ bounds of the DCIFAIF in one single simulation when the last orbiter has a fault from 200 s to 300 s. From Figure 9, the navigation errors become bigger when the fault occurs, but all the six state errors of the DCIFAIF are also captured by its three-sigma covariance bounds.Figure 9
State estimate errors and 3σ bounds for DCIFAIF when fault occurs.Figure10 shows the RMSEs of EKF, CI, and DCIFAIF under 5,000 simulations when the last orbiter has a fault from 200 s to 300 s. In these simulations, the EKF is divergent from 200 s to 300 s, because no measurements are provided for the centralized navigation system. Obviously, the results of the CI and proposed DCIFAIF all become worse when the fault occurs. Also, the proposed DCIFAIF has a better performance in altitude, longitude, and latitude.Figure 10
RMSEs for three methods when faults occur during Mars entry.As a whole, the proposed DCIFAIF uses the distributed framework for the Mars entry navigation scheme and provides robust and reliable state estimates for the Mars entry vehicle.


## 5. Conclusions

Robust, reliable, and high-precision Mars atmospheric entry navigation is the key requirement for the future Mars exploration missions. Based on the CI fusion algorithm and distributed framework, this paper presented a distributed CI fusion integrated navigation algorithm with information feedback during the Mars atmospheric entry. A distributed integrated navigation scheme includes four subsystems, in which each subsystem consists of an IMU and one radio beacon. Each subsystem independently estimates the Mars entry vehicle state by using EKF, and then, these estimations are sent to the master filtering and fused by using the CI algorithm. Unfortunately, each subsystem is weakly observable under limited Mars entry measurements. A new fast-distributed scalar weight selection scheme is proposed to maximize the information contribution of the corresponding estimate by using the infinite norm of the matrix. The distributed framework with dynamic information distribution coefficients based on the CI fusion algorithm is designed to meet the lower computation cost and robust and reliable capacity. Especially, the proposed DCIFAIF can provide consistent estimate results when the subsystems are weakly observable or unobservable during the Mars entry navigation scheme with only IMU and one radio measurement. Numerical simulations show that the distributed CI fusion integrated navigation algorithm can provide consistent navigation accuracy and improve the entry navigation robustness and fault tolerance under the weakly observable navigation subsystems.



---

*Source: 1802659-2019-05-02.xml*