The differential geometric filter is implemented to estimate the absolute and
relative positions of the spacecraft in a formation. The extended Kalman Filter is
also implemented as a benchmark for the differential geometric estimation. Only
relative positions between the spacecraft are measured. Relative positions are measured
using a wireless local positioning system (WLPS) installed in all spacecraft.
Two different scenarios are studied: (1) the observations include WLPS measurements
only and (2) the observations include WLPS measurements in addition to
measurements for the absolute position of one spacecraft made by a radar that
takes measurements from the earth’s surface. Results show that the differential
geometric estimation has better stability performance and a faster convergence
rate compared to the extended Kalman filter.
1. Introduction
The relative and absolute positions estimation of spacecraft formations is a fundamental task in many space missions. Relative position estimation plays an important role in Spacecraft Formation Flying (SFF) missions, a subject that has been the focus of many researches during last decade. Some SFF missions require that multiple spacecraft, in different orbits, communicate without interruption, for example, satellites in the CITRIS-COSMIC system are required to communicate with each other to monitor the ionospheric irregularities [1]. Relative positions between satellites, such as Cluster and Cluster-II satellites launched by the European Space Agency, are estimated and controlled to support many collaborative tasks where satellites are required to maintain a specific formation in a continuous manner within the mission period [2].
Several relative position estimation methods have been developed. A GPS-like technology that can be applied to SFF have been introduced [3, 4], where each spacecraft is equipped with a communication system to localize other spacecraft in the formation. This system provides high precision estimates for relative positions. Yet, complex hardware is required on each spacecraft. This system does not stand alone and requires a GPS system.
A relative position and attitude estimation through a Vision-Based Navigation System (VISNAV) has been addressed extensively in the literature [5–8]. The VISNAV consists of a Position Sensing Diode (PSD) sensor and an array of LED source beacons installed on a plate. The PSD detects the energy of the light source emitted by the beacons. Then the PSD generates a current flow to four terminals that are installed on the PSD plate. At that point, the centroid of the current flow is determined to measure the direction of the energy source. The covariance study shows that the performance could be affected by the relative distance between spacecraft. The accuracy factors of the estimation depend on the number of beacons installed on the spacecraft. Three or more beacons are suggested to ensure the observability of the system [5].
Psiaki proved the feasibility of estimating the orbits of two spacecraft through measuring their relative distance, azimuth, and elevation angles [9]. The orbital elements estimation accuracy depends on the relative distances between the spacecraft. Only two spacecraft formations were considered. As shown in [9], the relative position between spacecraft impacts the estimation performance. In addition, the system becomes unobservable in a few cases, such as zero inclination.
The extended Kalman filter (EKF) has been widely implemented into the nonlinear system to estimate the relative and absolute positions in SFF [5, 10–12]. The system nonlinearity causes the EKF estimation to be sensitive to initial condition errors, and the linearization step in the EKF may result in estimation instability. The Unscented Kalman Filter (UKF) has been implemented for relative attitude and position estimation in SFF [13, 14]. The UKF has a better robustness to initial condition errors compared to the EKF. However, its computational complexity is higher than the standard EKF. If compared to the standard EKF, the UKF has a faster convergence rate, but the computational time required for each update is longer than that of the EKF [15]. The Batch filter [16], an offline filtering method, has been implemented in [9] for absolute positions estimation using the relative positions measurements. Both studies in [9, 12] show that the configuration of the SFF affects the stability and the accuracy of estimation.
The differential geometric (DG) theory has been widely implemented in the control and guidance research area [17, 18]. In calculus, the differential geometry represents the linear approximation of a smooth curve [19]. References [20, 21] introduced the differential geometric approach into the missile tracking applications. In [20], the system model is transformed from the arc length domain into the time domain; then the guidance law is applied. Both papers have shown the feasibility of implementing the DG for missile guidance.
The differential geometric approach for nonlinear systems has been extended recently for the nonlinear estimation purpose [22]. The DG estimation avoids the linearization step in the EKF. In DG estimation, the nonlinear dynamics are mapped to a linear domain, where a linear estimator can be implemented. Then, the inverse transformation is applied to the estimator [22]. Reference [22] details how the DG estimation can be implemented when the number of available measurements is too few to handle all the system nonlinearities and shows that both the optimal control and the linear filtering approaches can be applied in the mapped linear domain.
In this paper, the DG filter is implemented in estimating the spacecraft relative and absolute positions in formation. While most of the developments in the literature focus on the estimation of relative positions [3, 5], this paper estimates relative and absolute positions of all spacecraft. The differential geometric (DG) estimation and the extended Kalman filter (EKF) are implemented and compared for estimation using wireless local positioning system (WLPS) measurements. The WLPS installed on each spacecraft enables that spacecraft to determine the relative positions of other spacecraft located in its coverage area via time-of-arrival (TOA) and directional-of-arrival (DOA) measurements [12, 23, 24].
In this paper, two scenarios are considered: (1) observations include WLPS measurements only and (2) observations include WLPS measurements in addition to the absolute position of one spacecraft measured by radar systems installed on the earth. Section 2 presents an overview on the WLPS. Section 3 presents the orbit dynamics model. The derivation of the DG Estimation equations for the problem of relative and absolute positions estimation are presented in Section 4. Section 5 presents the EKF implementation. Section 6 discusses the simulation results and presents a complexity analysis that compares the computational costs between the DG filter and the EKF.
2. Wireless Local Positioning System (WLPS)
The WLPS consists of two basic components [12, 23, 24]: A dynamic base station (DBS) and a transponder (TRX). Each DBS is capable of localizing TRXs located in its coverage area via TOA and DOA measurements, as shown in Figure 1. The DBS periodically broadcasts an identification (IDR) signal once every ID Request Repetition Time (IRT) as shown in Figure 2. A TRX within the DBS coverage area receives the IDR signal and transmits a response signal that includes its own ID back to the DBS within the IRT period. The ID of each TRX allows the DBS to distinguish one TRX from another. It also allows the DBS to easily track multiple TRXs located in its coverage area.
WLPS signal transmission between DBS and TRX.
DBS ID signal and TRX response signal in an IRT period.
As shown in Figure 2, the range of a TRX is measured by comparing the TOA of the signal from the TRX at the DBS receiver and the time of the transmission of the signal from the DBS transmitter. Here, it is assumed that the processing time delay in the TRX is known. The processing time estimate can be included in the signal packet transmitted from the TRX to the DBS in order to allow the DBS to correctly measure the range. The DBS, equipped with antenna arrays, allows DOA estimation and beamforming. In addition, beamforming enhances the performance of the DBS by reducing the interference effects [25]. The DOA is measured by each spacecraft relative to its body fixed coordinate system. In this paper, we assume that the attitudes of all spacecraft are known. Hence, all the DOA measurements can be computed relative to a fixed reference frame.
Thus, a WLPS allows single node positioning. In other words, each node equipped with a DBS can independently localize the TRXs located in its coverage area and its field of view (FOV). Now, if all spacecraft are equipped with both a DBS and TRX, each spacecraft can find the position of other spacecraft located in its FOV and coverage area. The position information across multiple spacecraft can be fused to improve the localization performance [26, 27].
3. System Model
In this section, the state and measurement models are derived. Two sets of models are derived for two scenarios: (1) only WLPS measurements are available and (2) WLPS measurements and those taken by a tracking system (such as a radar installed on the ground) are available. For the first scenario, a four-spacecraft formation is considered. Each spacecraft is equipped with both a DBS and a TRX. For the second scenario, a two-spacecraft formation is considered; one spacecraft is equipped with a DBS and the other spacecraft is equipped with a TRX. The radar measures the absolute range, the absolute azimuth, and elevation angles of one spacecraft with respect to a ground station. Since the position of the ground station is known, we assume the absolute position measurement is expressed with respect to earth center [28].
The estimated states are considered to be the spacecrafts' absolute and relative positions and their velocities. If the spacecrafts' orientations are known, the WLPS measurements can be expressed in the inertial reference frame.
3.1. State Model
We assume the spacecraft's absolute position in the Earth Centered Inertial (ECI) frame is [rxryrz]T, the velocity vector is [ṙxṙyṙz]T, and the acceleration vector is [r̈xr̈yr̈z]T. The dynamic model of each spacecraft with respect to the earth center, can be expressed using a two-body model, which is given as [29]r̈=-μr3r+w,
where, μ is the Earth gravitational constant, and w is a zero-mean gaussian acceleration uncertainty, and E[wwT]=Q; here, E[·] refers to the statistical expectation operation. The vectors r and r̈ are the absolute position and acceleration of the spacecraft, respectively, and r denotes the absolute distance of the spacecraft from the center of the Earth.
3.2. WLPS Measurements Model
In the first scenario, we incorporate only WLPS measurements for localization. In Figure 3, ri and rj represent the ith and the jth spacecraft absolute position vectors expressed in an ECI frame. The relative position vector of the jth spacecraft observed by the ith spacecraft equipped with DBS, expressed in ECI frame, isrij=rj-ri,ri=[ri,xri,yri,z]T,rj=[rj,xrj,yrj,z]T.
Relative position vector between two spacecraft.
In general, there are a maximum of 12 possible measurement sets for a four-spacecraft configuration. Here, we consider only four measurement sets in the paper: r12, r23, r34, and r41. If the orientation of the spacecraft (u-reference frame in Figure 3) is known, the relative position vector, Rij, expressed in the ith spacecraft reference frame would beRij=Dirij=[Rij,xRij,yRij,z]T,
where Di is the Directional Cosine Matrix (also known as Attitude Matrix [16]) of the ith spacecraft relative to the ECI frame. The relative distance between spacecraft i and j is a scalar linear function of TOA, hijTOA, that is,
hijTOA=‖rij‖c=‖rj-ri‖c,
where c is the speed of light, and ∥·∥ refers to the magnitude of vector. The DOA between the two spacecraft is given by:hijDOA=[ψijϕij]=[tan-1(Rij,yRij,x)tan-1(Rij,zRij,x2+Rij,y2)].
Here, ψij is the relative azimuth angle, and ϕij is the relative elevation angle between the two spacecraft i and j. Using (6) and (7), we can express the WLPS measurement between the ith and the jth spacecraft asỹp,ij=[hijTOAhijDOA]+νp,ij,
where νp,ij denotes WLPS measurement noise, which is assumed to be zero-mean gaussian with E[νp,ijνp,ijT]=ℜp,ij.
Using (A.2) in Appendix A, the WLPS measurement vector in Cartesian coordinates isỹc,ij=[rij,xrij,yrij,z]+νc,ij.
The new measurement noise νc,ij is a function of νp,ij, hijTOA and hijDOA are defined in (8).
3.3. Radar System Model
In the second scenario, an additional radar system that measures the spacecraft range, azimuth, and elevation angles is included. It is assumed that the ith spacecraft that is observed by the radar is also equipped with a DBS. The absolute position measurement made by the radar isỹr,i=[ρiλriξri]T+νr,i,
where νr,i is radar system noise, which is assumed to be zero-mean gaussian, and E[νrνrT]=ℜr. The ri is the absolute distance of the ith spacecraft from the radar, λri is the azimuth angle, and ξri is the elevation angle of ith spacecraft with respect to a radar-fixed coordinate system. The radar position is known in the ECI frame. So, using radar measurements, it is straightforward to calculate the absolute distance of the ith spacecraft from the earth's center, ∥ri∥ and the azimuth, λi, and elevation, ξi angles with respect to the ECI frame. In this preliminary analysis, we will assume for simplicity that we measure directly ∥ri∥, λi, and ξi. Azimuth and elevation angles are related to the coordinates of the spacecraft throughλi=tan-1ri,yri,x,ξi=tan-1ri,zri,x2+ri,y2,
where ri,x, ri,y, and ri,z are defined in (3).
3.4. Measurement Models for Simulation
In the first scenario, only the linear filtering method presented in Section 4 will be implemented. Four-spacecraft formation is required, as discussed in Section 4.3. Thus, the measurement vector, ỹ isỹ≡[ỹc,12Tỹc,23Tỹc,34Tỹc,41T]T+ν,
where ν≡[νc,12Tνc,23Tνc,34Tνc,41T]T is the measurement noise vector. The corresponding measurement noise covariance is given asR1=[Rc,1203×303×303×303×3Rc,2303×303×303×303×3Rc,3403×303×303×303×3Rc,41].
The ℜc,ij which denotes the relative position measurement noise covariance in the ECI frame in (14) is given asRc,ij=E{(rij-DiTνc,ij)(rij-DiTνc,ij)T}=DiTR¯c,ijDi,
where ℜ¯c,ij is the noise covariance of the relative position measurement, expressed in cartesian coordinates, and Di is the attitude matrix [16]. ℜ¯c,ij is given as a function of ℜp,ij [28]. The transformation from ℜp,ij to ℜ¯c,ij is shown in Appendix B.
For the second scenario, both the linear filtering method and pole placement, which are presented in Section 4, will be implemented for estimation purposes. Only two-spacecraft formation is considered. The measurement vector ỹ isỹ≡[ỹc,12Tỹr,1T]T+ν,
where ν≡[νc,12Tνr,1T]T. The corresponding measurement covariance matrix ℜ2 isR2=[R1203×303×3Rr],
where ℜr=diag[σρ2σλ2σξ2], which the “diag” represents the diagonal matrix, and σρ2, σλ2, and σξ2 are the measurement noise variances for absolute distance, azimuth, and elevation angle, respectively.
4. Differential Geometric Filter
DG estimation was recently extended to deal with nonlinear dynamic systems with fewer measurements than required to handle all nonlinearities [22]. DG estimation avoids the linearization step encountered in the EKF. In the DG estimation, the nonlinear system is transformed into a linear system. The transformation requires the state vector, x, to be expressed as a function of the measurements vector, y, that is x=s(y).
The DG estimation developement is explained in [22] and is briefed here for completness of presentation. The system model for DG estimation is constructed by defining a new state vector z such that the measurement vector y can be expressed as a linear function of zż=Az+Bg(z)+Gw,y=Cz+ν.
Here, A, B, C, and G are linear matrices, w is the process noise, and ν is the measurement noise. The state equation (18) includes a linear system, Az, and a nonlinear input function, Bg(z).
When the available measurements are not enough to handle the nonlinearities in the system, a set of pseudomeasurements, z′, is required. The pseudo-measurements can be obtained by taking the derivatives of y, that is, z′=ẏ.
The pseudo-errors are defined ase=z′-ẑ′
Then, the estimated state vector, ẑ, that includes the pseudomeasurements isẑ≡[ŷTŷ̇T]T=[ŷTz′T]T.
Letting m be the total number of measurements, (21) shows that the size of the estimated state vector, z, is twice the size of the measurement vector, n=2m. In addition, it can be shown that the time rate of change of the pseudoerrors is [22]ė̂=Am+1,nê--Lm+1,n(ỹ-ŷ),
where L is the gain matrix which will be presented in the next section, and Am,n and Lm,n denote mth to nth row of the A and L matrices, respectively.
In [22], two gain computation methods are presented, which are the pole placement method and Kalman Filter (or Linear Filtering) method.
4.1. Linear Filtering Gain
The state (18) is linear. Thus, the Kalman Filter theory can be applied. Note that the input function, g(z), is not used in the updating process in both DG estimation and EKF. The DG estimation gain matrix, L, is determined byL=PCT(CPCT+R)-1,
where P is the state covariance matrix corresponding to the z vector, and R is the measurement noise covariance matrix. Then, the state estimates, z, and its covariance are updated as follows:ẑ+=ẑ-+L1,m(ỹ-ŷ),ẑ′+=ẑ′-+Lm+1,n(ỹ-ŷ),P+=(I-LC)P-,
where I is the identity matrix, the superscript − denotes preupdate estimates, and the superscript + denotes postupdate estimates. The ẑ′ denotes any estimated pseudomeasurements vectors and the estimated state vectors that are not observed by measurements. Lm,n denotes mth to nth row of the gain matrix, L.
The DG estimation filtering process works as follows [22]. The gain matrix, L, is determined using (23). Then the estimated states and state covariance matrix are updated using (24). The updated states and the pseudoerrors are propagated by (18) and (22) with their respective input function. The state covariance matrix is propagated using (25)Ṗ=AP+PAT+GQGT,
where Q is the process noise covariance, which is defined as Q=E{wwT}.
Both the A and G matrices in (18) and (25) are linear time invariant matrices; thus, (25) can be expressed in time discrete representation, which is given asPk+1=ΦPkΦT+Q,
where, subscript k and k+1 denote the current and next time step, respectively, Φ is the state transition matrix, and 𝒬 is the discrete-time process noise covariance. Both Φ and 𝒬 are calculated as follows [30]. LetA=[-AGQGT0FT]Δt,B=eA≡[B11B120B22],
where Δt is the time interval between two successive measurements, e𝒜 is the exponential matrix of 𝒜 matrix, and F is the matrix representation of the system dynamic model. For the nonlinear dynamic model case, for example, (1), F is the first order Taylor series expansion (or Jacobian matrix) of the dynamic model. Both Φ and 𝒬 are given asΦ=B22T,Q=ΦB12.
4.2. Pole Placement Method
Another filtering method presented in [22] is the pole placement method. The pole placement method has been widely used in controls and estimation. Unlike the Kalman filter, the gain, L, in pole placement, does not change dynamically. Pole placement also requires full system observability. However, the pole placement method guarantees the linearity of the measurement model, and hence guarantees stability.
The pole placement gain matrix L is computed as follows. Given that the A matrix has n×n dimensional and canonical form as the following:A=[100⋯0010⋯0001⋯0⋮⋮⋮⋱⋮000⋯0]
and C matrix has n×m dimensional. The system is considered to be fully observable if the rank of O(C,A) matrix is full rank [31] as the following:O(C,A)=[CCACA2⋮CAn-1]
If the full rank observability condition in (30) is fulfilled, the gain matrix L is calculated using the following equation:λ=eig(A-CL),
where λ is the eigenvalue vector, and eig represents the eigenvalue.
If a set of eigenvalues are given, the gain matrix L can be computed using the Ackermann's formula [16]. The selection of the eigenvalues impacts both stability and accuracy of estimation. Therefore, negative eigenvalues are always selected to ensure the stability of the estimation process.
Here, our goal is to set up the DG estimation filter for the two scenarios introduced in Section 3. For the first scenario, only the WLPS measurement is available while we intend to estimate the spacecraft absolute position. Thus, it is required to derive the expression of absolute position of spacecraft in terms of relative position. For the second scenario, an additional absolute position measurement of spacecraft is included.
4.3. Scenario One—Relative Position Estimation with WLPS-Only Measurement
The absolute distance of a spacecraft j from the Earth's center isrj2=ri2+rij2+2rijTri,
where ri and rij denote absolute and relative positions, respectively, defined in (2) to (4).
The orbital equation of spacecraft is [29]r=a(1-ϵ2)1+ϵcosθ.
In (33), r represents the distance of spacecraft to the earth's center, a is the semimajor axis, ϵ is the eccentricity, and θ is the true anomaly. In this scenario, it is assumed that all spacecraft in the formation have the same semimajor axis, eccentricity, and true anomaly. Hence, (33) shows that the distances from all spacecraft to the earth's center would be the same. In this case, rj=ri, leads torij2+2rijTri=0.
If we consider a four-spacecraft configuration, then (34) can provide a closed form solution for absolute positions using relative position measurements. For example, the absolute position of spacecraft 1 can be obtained from[r12,xr12,yr12,zr13,xr13,yr13,zr14,xr14,yr14,z][r1,xr1,yr1,z]=-[r1222r1322r1422].
Here, ri,x, ri,y, and ri,z and rij,x, rij,y, and rij,z are the absolute and relative position elements introduced in (2), respectively. The right hand side of (35) is the square of relative distances between the spacecraft. Similarly, we can determine the absolute positions of the other spacecraft. Note that the solution in (35) requires the computation of inverse of the relative position measurements matrix. Therefore, the closed form solution needs an accurate estimation or unbiased relative position measurements.
Consider the four sets of WLPS measurements mentioned in Section 3 where the total number of measurements, m, in (13) is 12. However, there are 24 states to be estimated; thus, at least 12 pseudomeasurements are required. The pseudomeasurements are defined as the relative velocities. Using the dynamic motion of spacecraft defined in (1), the relative accelerations between spacecraft arer̈ij=μriri3-μrjrj3.
When all spacecraft in the formation have the same semimajor axis, a, eccentricity, ϵ, and true anomaly, θ, at all times, (36) can be simplified to.r̈ij=-μrijr3.
The nonlinear input function, g(z), in (18) is defined as a function of measurements, ỹ, and pseudomeasurement, z′. The only nonlinear function of the dynamic system in this scenario is the relative acceleration introduced in (37). Therefore, the input function consists of the relative acceleration between the spacecraft, which isg(ỹ,z′)=[-μr12Tr3-μr23Tr3-μr34Tr3-μr41Tr3]T,
where z′ is the pseudomeasurements vector defined in (20), which can be written as z′=ẑ′+e.
Equation (38) depicts that the absolute range of spacecraft that is required for the relative dynamic model. Equation (35) determines the absolute position using relative position measurements. However, (35) is vulnerable to the error in rij, which could result in a large estimation error in the absolute range, r. Then, we consider two additional pseudomeasurements which are the absolute range and eccentric anomaly, E. The rate of change of absolute range is [29]ṙ=aϵsinEĖ,
where a and ϵ were defined in (33). The rate of change of eccentric anomaly is:Ė=μa1r.
Substituting (40) into (39), we obtain the expression of ṙ in terms of eccentric anomaly and range only. Adding (39) and (40) into the input function, g(ỹ,z′) in (38), we obtaing(ỹ,z′)=[-μr12Tr3-μr23Tr3-μr34Tr3-μr41Tr3μaϵsinErμa1r]T.
Then, the estimate state vector, z isz=[r12r23r34r41ṙ12ṙ23ṙ34ṙ41rE]T.
The linear matrices A, B, and G in (18) associated with state models in (41) and (42) correspond toA=[012×12I12×12012×2014×12014×12014×2],B=[014×12I14×14]T,G=[014×12I14×14]T.
For the WLPS-only measurement, the C matrix in (19) associated with the measurement model in (13) isC=[I12×12012×14].
4.4. Scenario Two—Relative Position Estimation with WLPS and Radar Measurement
Consider a radar tracks one of the spacecraft in the two-spacecraft formation, for example, ith spacecraft where i represents either spacecraft 1 or spacecraft 2. The absolute distance to the spacecraft is measured at all times. Then, the range and eccentric anomaly pseudomeasurements in (42) can be ommitted. Therefore, the state vector for the second scenario becomesz=[r12Triλiξiṙ12Tṙiλ̇iξ̇i]T,
where, ri is the absolute range, λi is the absolute azimuth angle, ξi is the absolute elevation angle, and ṙi, λ̇i, ξ̇i are their time derivatives, respectively. Based on (47), the nonlinear function are the relative acceleration and the acceleration in terms of r̈i, λ̈i, and ξ̈i. Then, the modified input function, g(ỹ,z′) isg(ỹ,z′)=[μr1Tr13-μr2Tr23r̈iλ̈iξ̈i]T,
where r̈i, λ̈i, and ξ̈i are the spacecraft polar accelerations, which are the second order time derivatives of the absolute range, absolute azimuth, and elevation shown in (10)-(11) respectively. Their corresponding equations arer̈i=riξ̇i2+riλ̇i2cos2ξi-μri2,λ̈i=-2ṙiλ̇iri+2λ̇iξ̇itanξi,ξ̈i=-2ṙiξ̇iri-2λ̇i2cosλisinξi,
where the first order time derivation of polar coordinate and cartesian coordinate are presented in Appendix A.
In this case, the A, B, G, and C matrices for DG estimation's state and measurement models in (18) and (19), are
A=[06×6I6×606×606×6],B=[06×6I6×6]T,G=[06×6I6×6]T,C=[I6×606×6].
The process noises in the G matrix in (54) are assumed along the acceleration axes only.
5. Implementation of Extended Kalman Filter
The standard EKF has been implemented for both scenarios. The state model, (1), and the measurement models, (13) and (16), correspond to the first and second scenarios, respectively. In [12], the EKF has been derived for the WLPS-measurements expressed in polar coordinates.
For n-number of spacecraft in the formation, the state estimate vector for EKF isx̂=[r1Tr2T…rnTv1Tv2T…vnT]T.
For the first scenario, the measurement model is:Hk1=[-I3×3I3×303×603×603×6-I3×3I3×303×603×603×6-I3×3I3×3I3×303×603×6-I3×3].
For the second scenario, the linearized measurement model isHk2=[-I3×3I3×303×6∂hradar∂r03×303×6],
where ∂hradar/∂ri is the jacobian of the three radar measurement components introduced in (10) with respect to the ith spacecraft absolute positions and velocities [28], which is given by∂hradar∂r=[ri,xriri,yriri,zri-sinλriricosξricosλriricosξri0-cosλrisinξriri-sinλrisinξriricosξriri],
where ri=[ri,xri,yri,z]T and ri, λri, and ξri are defined in (10).
Using the measurement noise covariance obtained in (14) and (17) together with the linearized state and measurement models, the EKF for relative position estimation is implemented.
6. Simulation Results and Discussions
Simulations are conducted to study and compare the accuracy and convergence performance between differential geometric estimation and EKF. Two differential geometric estimation methods discussed in Section 4 are implemented in the simulation, which are the pole placement and the linear filter. The accuracy performance measured is their root mean square error (RMSE). Here, the mean square error (MSE) is the average of the square of estimation error over x, y, and z axes of spacecraft position. Thus,RMSE≡‖r̂i-ri‖3,
where r̂i is the estimated absolute position of the ith spacecraft, and ri is the true absolute position of the ith spacecraft.
The convergence rate is determined by the amount of time required by the estimator's RMSE to fall within a RMSE threshold, ethres. The ethres is determined as the average RMSE at the estimator's RMSE steady state. In addition, the complexity of DG Estimation and EKF are compared through the number of multiplications required by each estimator in an iteration.
In the simulation, all spacecraft are in low earth orbit (LEO), with a semimajor-axis of 7000 km, the eccentricity of 0.04, and 0 degree of initial true anomaly. The argument of perigee (ARGPER), the Right ascension of the ascending node (RAAN) and the inclination (INC) of each spacecraft are specified in each scenario in order to meet the formation configuration requirements.
The initial attitude of each spacecraft is constructed using a euler angle with 3-1-3 orientation at periapsis [32]. The first rotation is the spacecraft RAAN angle, followed by a second rotation (inclination angle). Then, the third rotation angle is 90 degrees for spacecraft 1, 3, and 4 and −90 degrees for spacecraft 2. To ensure that one of the spacecraft reference frame axes points toward the center of the Earth at all times, all spacecraft are set to rotate at a constant angular velocity, which is, ωi=[00ω]T. Here, ωi is the angular velocity vector expressed in the ith spacecraft reference frame. Then, the angular velocity of spacecraft expressed in ECI is ωN=DiTωi. The mean motion is ω=μ/a3; where μ is the earth grativational constant, and a is the semimajor axis.
The simulation assumptions are as follows: (i) process and measurement noises for all spacecraft are zero-mean Gaussian; (ii) Initial conditions of spacecraft are well known; (iii) The attitude of each spacecraft is well known; (iv) All spacecraft are equipped with a DBS and TRX; thus, each one of them can localize others; (v) The reference frame of the DBS is aligned with the corresponding spacecraft's attitude reference frame; (vi) The radar system measurement is available at all times for the second scenario; (vii) no signal transmission delay; (viii) The process noise variance is 10-7 km/s2 along each axis for all scenarios.
6.1. Observability Analysis
The pole placement method presented in Section 4.2 requires the system to be fully observable. In this section, the system observability analysis of these two scenarios is presented. For simplicity, we consider a two-spacecraft formation case for the first scenario in this observability analysis. The estimated states for DG estimation, after the transformation, is z=[r12Tv12TrE]T. The A matrix for the dynamic model in (18) isA=[I3×303×505×305×5].
We assume that only relative position is measured in cartesian coordinate. The C matrix is given as:C=[I3×303×5].
The rank of the observability matrix using (30) is three, which is not full rank. Therefore, the pole placement method presented in Section 4.2 is not applicable.
In the second scenario, a radar measures one spacecraft's absolute position (see (16) and (47)). Both the A and C matrix are given in the (52) and (55), respectively. The rank of the observability matrix is twelve, which is full rank. Then, the pole placement method will be implemented in this scenario and compared with other estimation methods.
6.2. Scenario One
Consider only WLPS measurements are available in the four-spacecraft formation. The measurements vector, state vector, and its corresponding nonlinear model and matrices are given in (13), (41) to (45), respectively. Three formation configurations that represent the short range (≃0.25 km), medium range (≃60 km), and long range (≃1200 km) are considered. The spacecraft formation configurations for different formation size are shown in Table 1. We study the impact of formation size on both EKF and DG estimation performance.
Spacecraft formation's configuration for short, medium, and long range formation.
Short
Medium
Long
S/C 1
S/C 2
S/C 3
S/C 4
S/C 1
S/C 2
S/C 3
S/C 4
S/C 1
S/C 2
S/C 3
S/C 4
RAAN (deg)
0
0
0
0
−0.1
0
0
0.1
0.2
0
−0.1
0.1
INC (deg)
15
15
15
15
14.5
14
15.5
15
15
20
25
10
ARGPER (deg)
0
0.001
−0.001
0.002
0.2
0
−0.1
0.1
0
5
−5
10
The measurement noise of WLPS is assumed to be 1 meter in distance (computed based on TOA measurement) and 0.001 degree in DOA. Then, the measurement noise covariance matrix is constructed using (14).
The process noise parameter, w in (1) and (18), is assumed to be 10-7 on each absolute and relative acceleration axis for both EKF and DG Estimation. The initial states standard deviations for EKF are 1 km and 0.5 km/sec in absolute position and velocity, respectively. The initial states standard deviations for DG Estimation are 1 km in relative position, 0.5 km/sec in relative velocity, 2 km in absolute range, 0.001 degree in eccentric anomaly, and 0.001 km/sec for the pseudoerrors.
6.3. Discussion—Scenario One
Table 2 shows the mean RMSE in absolute position for DG estimation and EKF with respect to different mean relative distances. The results show that the performance of DG estimation improves as the mean relative distance increases when the measurements consist of WLPS only. In Figure 4, the signal-to-noise ratio (SNR) between estimation error and relative distance decreases when the spacecraft in the formation get closer (or 1/SNR increases), in which case the errors in the transformation from relative positions to absolute positions using (35) become significant.
Comparing mean RMSE in absolute position: DG Est. versus EKF.
Distance
DG Est.
EKF
Short (≃0.25km)
4.447×103 km
2.657×10-4 km
Medium (≃60km)
16.59km
4.153×10-4 km
Long (≃1200km)
0.901km
7.616×10-3 km
DG Est. Noise-to-Signal Ratio.
Table 2 depicts that the EKF has a better accuracy compared to DG Estimation, when only WLPS measurements are used. The EKF does not require the inverse transformation (34). However, when the relative distance between spacecraft increases, the effect of DOA estimation error on the positioning error increases (see Figure 5). Accordingly, the performance of EKF drops slightly as the mean relative distance increases.
Measurement error wrt. distance.
If only one DBS is available in a three-spacecraft configuration [12], the EKF would be unstable when the mean relative distance between spacecraft falls within a certain range (≤10 km). EKF stability can be improved by increasing the total number of DBSs installed in the formation, which is shown in Table 2. In addition, increasing the number of DBSs improves the estimation accuracy.
6.4. Scenario Two
Consider WLPS and radar measurements are available. Here, the two-spacecraft formation with only the medium range formation configuration is considered. The radar is measuring the absolute range, azimuth and elevation angle of spacecraft 1 at all time. The configuration of both spacecraft is similar to the configuration of S/C 1 and S/C 2 shown in Table 1. The measurement vector, state vector, and its corresponding nonlinear model and matrices are given in (16), (47) to (55), respectively. The performance of the EKF and the DG estimation for a given range of measurement noise is studied. The measurement noises of WLPS and radar are depicted in Table 3. The measurement noise covariance matrix is constructed using (17).
Measurement noise's set configuration.
Minimum
Maximum
Increment
Relative Distance, (km)
0.001
0.04
0.004
Relative azimuth and elevation, (deg)
0.001
0.04
0.004
Absolute range, (km)
0.1
4
0.4
Absolute azimuth and elevation, (deg)
0.001
0.4
0.004
The process noise parameter, w, is assumed to be 10-7 for all axes in EKF. For DG Estimation, the standard deviation of process noises for the relative position acceleration is 10-7, the absolute distance's acceleration is 10×10-4, and the absolute azimuth and elevation angles' acceleration is 10×10-7. The initial states standard deviations for EKF are 2 km in absolute position and 0.1 km/sec in absolute velocity. The initial states standard deviations for DG Estimation are 0.1 km in relative position, 0.05 km/sec in relative velocity, 1 km in absolute range, and 0.05 degrees in azimuth and elevation angles. In addition, the initial variance for the rate of change of the range is 0.1 km/sec, and the rate of change of the absolute azimuth and elevation angles is 0.001 degree.
The simulation for each set of corresponding measurement noise is repeated for 10 times to compare the consistency of results produced by both filters. Then the maximum, minimum, and mean RMS of each set of measurement noise for both EKF and DG Estimation are compared.
6.4.1. Linear Filtering Gain
First, the DG Estimation is compared with the EKF performance using the gain computation presented in Section 4.1. Figures 6 and 7 show the RMSE of DG Estimation and EKF for measurement noise standard deviation of 0.001 km in relative range, 0.001 degree in relative azimuth and elevation angle, 0.01 km in absolute range, and 0.01 degree in absolute azimuth and elevation angle. Figure 6 shows that the DG Estimation converges faster than EKF, because the DG Estimation avoids the linearization steps in the estimation.
DG Est. RMSE.
EKF RMSE.
However, Figure 7 shows that EKF is more accurate than DG Estimation. The nonlinear dynamic model in DG estimation is expressed in terms of measurements and pseudomeasurements (see (41) and (48)). While the linearization step is eliminated in the DG estimator, the propagation step is dependent on the measurements noises. On the other hand, the nonlinear dynamic model of EKF is expressed in terms of the estimated state vector with additional linearization steps. The EKF is able to achieve a better accuracy if the estimation converges.
Figure 8 compares the minimum and maximum RMSE of DG estimation and EKF for a given set of measurement noise standard deviation. The DG gain is computed using the method shown in Section 4.1. With additional radar measurement, the DG estimation accuracy is improved. In addition, the minimum and maximum RMSE of DG Estimation are very close compared to EKF because the DG estimation does not require linearization steps. Therefore, a stable estimation can be always achieved.
Minimum and maximum mean RMSE comparison between two filters with respect to measurement noise increment.
Figure 8 also shows that the EKF has better accuracy performance than the DG estimation. However, the stability of the EKF is not always guaranteed. In Figure 8, we observe large RMSEs in the EKF estimation at higher measurement noise levels. The large RMSE is caused by the unstable estimation of EKF, which is due to the linearization of the nonlinear radar measurement and state models in the EKF algorithm. On the other hand, the DG Estimation does not require any linearization step.
6.4.2. Pole Placement Method
Figures 9 and 10 compare the RMSE performance between the DG Estimation and the EKF. The EKF's configuration is the same as in the previous case. On the other hand, the pole placement method is as follows. The poles for the relative position are [-0.001-0.001-0.001], for the relative velocity vector are [-0.0004-0.0004-0.0004], and for the absolute position and velocity in polar coordinates are [-0.1-0.02-0.02] and [-0.05-0.01-0.01], respectively. The DG estimation gain is computed using the Ackermann's formula [16].
RMSE comparison (Stable EKF).
RMSE comparison (Unstable EKf).
Figure 9 shows that when the EKF estimation is stable, the EKF has better accuracy performance than the DG estimation. However, the linear system characteristic of the DG estimation allows it to converge faster than the EKF. Figure 10 shows that the EKF may suffer instability due to the linearization steps in the algorithm. In Figure 10, the EKF is able to converge to lower RMSE than the DG estimation, but the RMSE diverges after a certain time period. However, the DG estimation does not suffer any instability. Therefore, the pole placement method in DG estimation does guarantee the fast convergence and stability of estimation algorithm.
6.5. Complexity Analysis
In [33], a formalism of 𝒪 function for complexity computational of matrices operation has been introduced, which are: SUM(N×M,N×M)=O(N×M),SUB(N×M,N×M)=O(N×M),MUL(N×M,M×P)=O(N×M×P),INV(N×N)=O(N3),
where, M, N, and P are the dimensions of matrices that perform these operations. Here, P is the number of states in DG estimation, N is the number of states in EKF, M is the number of measurements, and Qp and Qn are the number of process noise in DG estimation and EKF, respectively.
We assume the computational complexity of the jacobian matrices is in the order of 𝒪(1). In addition, the propagation of the state models is considered to have the complexity of 𝒪(1) in both filters. The EKF requires an order of 𝒪(M×N×N) for measurement model computation, while the DG estimation only requires an order of 𝒪(1) computation complexity. Therefore, the EKF requires higher computational complexity to compute the measurement.
The EKF requires the order of 𝒪(M3) and 𝒪(M×N2) computation complexity for gain matrix. There are two different computation complexities in DG estimation for gain matrix computation. For the pole placement method, there is no computational complexity because the gain matrix is predetermined. For the linear filtering method, the gain computational complexity is in the order of 𝒪(M3) and 𝒪(M×P2). Both EKF and DG estimation may have same computation complexity if all states are observed. However, the DG estimation has higher computational complexity if not all states are observed (P2≫N2).
The covariance update and propagation's computational complexity for the EKF are in the order of 𝒪(N3), 𝒪(N2×Qn), and 𝒪(N3). There is no computational complexity for covariance update and propagation for DG estimation if the pole placement method is used. However, for the linear filtering method, the computational complexities are in the order of 𝒪(P3), 𝒪(P2×Qp), and 𝒪(P3).
Therefore, for fully observable case, the DG estimation has lower computational complexity. In addition, among the two DG estimation approaches implemented in this paper, the pole placement method has lower computational complexity compared to the linear filtering method. But the EKF has lower computation complexity if not all states are fully observed.
7. Conclusion
This paper presents an implementation of the differential geometric estimation in relative and absolute positions estimation for spacecraft formations. In the transformed linear domain, two approaches have been implemented in this paper: the pole placement and the linear filtering approaches. Simulations are conducted to compare the estimation performance of the differential geometric estimation and the extended Kalman filter. The results show that the differential geometric estimation has a faster convergence rate, and has better stability, compared to the extended Kalman filter. The complexity analysis shows that the differential geometric estimation has lower complexity when the system is fully observable.
AppendicesA. Time Derivative of State Vectors in Polar and Cartersian Coordinate
For simplicity, in this section, we assume r≡ri, λ≡λi and ξ≡ξi.
First order time derivatives of polar coordinates is
ṙ=xẋ+yẏ+zżr,λ̇=xẏ-yẋx2+y2,ξ̇=ż(x2+y2)1/2r2-z(xẋ+yẏ)r2(x2+y2)1/2.
The Cartesian representation in polar coordinates is
x=rcosλcosξ,y=rsinλcosξ,z=rsinξ.
First order time derivatives of cartesian coordinates is
ẋ=ṙcosλcosξ-rλ̇sinλcosξ-rξ̇cosλsinξ,ẏ=ṙsinλcosξ+rλ̇cosλsinξ-rξ̇sinλsinξ,ż=ṙsinξ+rξ̇cosξ.
B. Conversion of Measurement Noise Covariance from Polar to Cartesian Coordinate
Reference [28] shows the expression cartesian coordinate measurement noise in terms of polar coordinate vector. Given that r is the relative range, ψ and ϕ are the relative elevation and azimuth angles at the current time step, respectively. The corresponding measurement noises in standard deviation are σr, σψ and σϕ, respectively. Then, the measurement noise covariance, ℜij, in cartesian coordinate is given as R̅c,ij=Tkdiag{(r2+σr2)(1+e-2σψ2)(1+e-2σϕ2)4-r2e-σψ2+σϕ2,(r2+σr2)(1-e-2σψ2)(1+e-2σϕ2)4,(r2+σr2)(1+e-2σψ2)(1-e-2σϕ2)4,(r2+σr2)(1-e-2σψ2)(1-e-2σϕ2)4}TkT,
where the “diag” represents the diagonal matrix, and Tk is given as Tk=[cosψcosϕ-sinψcosϕ-cosψsinϕsinψsinϕsinψcosϕcosψcosϕ-sinψsinϕcosψsinϕsinϕ0-cosϕ0].
BernhardtP. A.SiefringC.HubaJ.SelcherC.CITRIS: the COSMIC companion
for LEO radio occultationProceedings of the COSMIC Radio Occultation Workshop2002EscoubetC. P.Cluster-II: scientific objectives and data dissemination20001025460PurcellG.KuangD.LichtenS.WuS. C.YoungL.Autonomous formation
flyer (AFF) sensor technology developmentProceedings of the Guidance and Control Conference1998AAS 98-062CorazziniT.RobertsonA.AdamsJ. C.HassibiA.HowJ. P.GPS sensing for spacecraft formation flyingProceedings of the 10th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GPS '97)September 19977357442-s2.0-0031355166KimS. G.CrassidisJ. L.ChengY.FosburyA. M.JunkinsJ. L.Kalman filtering for relative spacecraft attitude and position estimationProceedings of the AIAA Guidance, Navigation, and Control ConferenceAugust 2005251825352-s2.0-29744452436GunnamK. K.HughesD. C.JunkinsJ. L.KehtarnavazN.A vision-based DSP embedded navigation sensor2002254284412-s2.0-154235746510.1109/JSEN.2002.806212ValasekJ.GunnamK.KimmettJ.TandaleM. D.JunkinsJ. L.HughesD.Vision-based sensor and navigation system for autonomous air refueling20052859799892-s2.0-26244458373JunkinsJ.HughesD.WazniK.PariyapongV.Vision-based navigation for rendezvous,
docking and proximity operationsProceedings of the 22nd Annual AAS Guidance and Control
Conference1999Breckenridge, Colo, USAPsiakiM. L.Autonomous orbit determination for two spacecraft from relative position measurements19992223053122-s2.0-0033098945PhilipN. K.AnanthasayanamM. R.Relative position and attitude estimation and control schemes for the final phase of an autonomous docking mission of spacecraft20035275115222-s2.0-003739589010.1016/S0094-5765(02)00125-XMcTavishD. J.SchumacherR.OkounevaG.Kalman filtering for dynamic pose and relative motion estimation in orbit2007533-4951052-s2.0-44449166236GohS. T.AbdelkhalikO.ZekavatS. A.Spacecraft constellation orbit estimation
via a novel wireless positioning systemProceedings of the 19th AAS/AIAA Space Flight Mechanics Meeting2009Savannah, Ga, USALeeD.PernickaH.Vision-based relative state estimation using the unscented
Kalman filterProceedings of the 19th AAS/AIAA Space Flight Mechanics Meeting2009Savannah, Ga, USAGohS. T.2007University at BuffaloCrassidisJ. L.MarkleyF. L.Unscented filtering for spacecraft attitude estimation20032645365422-s2.0-0042266434CrassidisJ. L.JunkinsJ. L.2004Boca Raton, Fla, USAChapman & Hall/CRCAgrachevA. A.GamkrelidzeR. V.Feedback-invariant optimal control theory and differential geometry. I. Regular extremals1997333433892-s2.0-0031191520KrishchenkoA. P.KushnarevV. I.NazarenkoA. N.TkachevS. B.Numerical methods in a differential geometry approach to problems in nonlinear control theory1991296104114AriffO.ZbikowskiR.TsourdosA.WhiteB. A.Differential geometric guidance based on the involute of the target's trajectory: 2-D aspectsProceedings of the American Control ConferenceJuly 2004Boston, Mass, USA364036452-s2.0-8744311485LiC.JingW.WangH.QiZ.Analytical solution to 3D differential geometric guidance problemProceedings of the 1st International Symposium on Systems and Control in Aerospace and AstronauticsJanuary 20066476522-s2.0-33750959790KuoC.-Y.SoetantoD.ChiouY.-C.Geometric analysis of flight control command for tactical missile guidance20019223424310.1109/87.911375MenonP. K.Differential geometric estimators for nonlinear dynamic systemsProceedings of the AIAA
Guidance, Navigation and Control Conference2008Honolulu, Hawaii, USATongH.htong@mtu.eduPourrostamJ.jpourros@mtu.eduZekavatS. A.LCMV beamforming for a novel wireless local positioning system: nonstationarity and cyclostationarity analysis20072007129824310.1155/2007/98243TongH.htong@mtu.eduZekavatS. A.rezaz@mtu.eduA novel wireless local positioning system via a merger of DS-CDMA and beamforming: probability-of-detection performance analysis under array perturbations20075631307132010.1109/TVT.2007.895499GodaraL. C.Application of antenna arrays to mobile communications, part II: beam-forming and direction-of-arrival considerations1997858119512452-s2.0-0031207725WangZ.ZekavatS.Comparison of semidistributed multinode TOA-DOA fusion localization and GPS-Aided TOA (DOA) fusion localization for MANETs200820082-s2.0-5814950809210.1155/2008/439523439523WangZ.ZekavatS. A.MANET localization via multi-node TOA-DOA optimal
fusionProceedings of the IEEE Milcom2006Washington, DC, USAParkS. T.LeeJ. G.Improved Kalman filter design for three-dimensional radar tracking20013727277392-s2.0-003530055010.1109/7.937485BateR. R.MuellerD. D.WhiteJ. E.1971New York, NY, USADoverVan LoanC. F.Computing integrals involving the matrix exponential19782333954042-s2.0-0017981685ChenC.-T.1999New York, NY, USAOxford University PressSchaubH.JunkinsJ. L.2003New York, NY, USAAmerican Institute of Aeronautics and AstronauticsGasparriA.Sensor networks localization: a computational complexity analysis of an
extended Kalman filter vs. an extended information filter2007Roma, ItalyDipartimento di Informatica ed Automazione (DIA), Universita degli Studi “Roma Tre ”