TDMA Datalink Cooperative Navigation Algorithm Based on INS/JTIDS/BA

Abstract

Position information is very important tactical information in large-scale joint military operations. Positioning with datalink time of arrival (TOA) measurements is a primary choice when a global navigation satellite system (GNSS) is not available, datalink members are randomly distributed, only estimates with measurements between navigation sources and positioning users may lead to a unsatisfactory accuracy, and positioning geometry of altitude is poor. A time division multiple address (TDMA) datalink cooperative navigation algorithm based on INS/JTIDS/BA is presented in this paper. The proposed algorithm is used to revise the errors of the inertial navigation system (INS), clock bias is calibrated via round-trip timing (RTT), and altitude is located with height filter. The TDMA datalink cooperative navigation algorithm estimate errors are stated with general navigation measurements, cooperative navigation measurements, and predicted states. Weighted horizontal geometric dilution of precision (WHDOP) of the proposed algorithm and the effect of the cooperative measurements on positioning accuracy is analyzed in theory. We simulate a joint tactical information distribution system (JTIDS) network with multiple members to evaluate the performance of the proposed algorithm. The simulation results show that compared to an extended Kalman filter (EKF) that processes TOA measurements sequentially and a TDMA datalink navigation algorithm without cooperative measurements, the TDMA datalink cooperative navigation algorithm performs better.

1. Introduction

In large-scale joint military operations, datalink members can be commanded and controlled as a unified whole with position information, and position information is crucial to combat operation. At present, positions of network members can be obtained through absolute navigation methods like GNSS or long-range navigation (LORAN) systems [1,2]. However, GNSS is vulnerable to jamming, which will affect the accuracy of localization [3]. LORAN is also easily affected by noise and cross-rate interference [4]. A joint tactical information distribution system (JTIDS) can provide advantages like high transmission power and high anti-jamming capability [5]. Most of all, JTIDS can share location information between network members, and JTIDS users can extract TOA information; therefore, when GNSS or LORAN is jammed, the JTIDS network is a primary choice [6].

However, JTIDS has some disadvantages when positioning. First, JTIDS members are randomly distributed in the horizontal plane, and an uneven distribution of members may result in unevenness in the distribution of network members’ horizontal dilution of precision (HDOP) [7]. Members with poor HDOP of the network have low precision of latitude and longitude. Second, coverage of a large effective zone of the JTIDS network is about 500 km radius, but the height of network members is much smaller than the effective radius, and the distribution range of JTIDS members in the horizontal plane is much larger than the vertical difference, which result in a poor vertical dilution of precision (VDOP) and low estimate accuracy of members’ vertical ranges. In the general navigation algorithm of TDMA datalinks, joint units (JUs) process information of navigation sources and TOA measurements with a Kalman filter (KF) based on sequential processing, which can process TOA measurements sequentially and timely in the TDMA system, but the positioning performance is dissatisfactory when positioning geometry is poor [8,9,10]. To enhance the stability and accuracy of positioning, an integrated navigation method based on INS and datalink information was presented in [11,12], but they did not propose a good method to improve vertical accuracy. Therefore, a more practical positioning method of TDMA datalinks, such as JTIDS, is urgently needed.

To improve the altitude observation accuracy effectively, many scholars choose to add measurements of sensors, such as a barometer altimeter (BA), in the vertical direction. Authors in [13] proposed methods of loosely-coupled and tightly-coupled schemes with barometer information to improve location accuracy of a low cost INS/GNSS system under a harsh GNSS-degraded environment. To improve height accuracy of flight, authors in [14] investigated the combination of data from GNSS, radar, and barometer sensors. With pseudo-range, Doppler information, and MEMS barometric information, authors in [15] proposed a method of positioning with two satellites. Authors in [16] presented a state estimation technique by fusing measurements of long-range stereo visual odometry, a global positioning system (GPS), and barometric and inertial measurement units, and they improved positioning performance for the aggressive intermittent GPS and high-altitude micro-aerial vehicle (MAV) flight environment.

To improve positioning accuracy of network positioning users, the method of cooperative navigation was proposed by many scholars. Cooperative navigation has received extensive interest from the research field, like wireless sensor networks, mobile networks, and unmanned aerial vehicles (UAVs) [17,18,19]. Currently existing approaches for cooperative navigation are factor graphs and sum-product algorithms [20], semidefinite programming [21], particle filters [22], Kalman filters [23], and so on. In the cooperative navigation method, positioning users help each other to determine their locations. Compared to single positioning users, a group of cooperative positioning users may provide many navigational benefits, such as tolerance against individual user or sensor failures, distribution of sensors across a larger spatial area, and shared observations [24]. Cooperative navigation increases localization performance in terms of both accuracy and coverage [25]. Cooperative navigation is always used to improve positioning accuracy of network members in defective positioning environments. In GNSS-denied environments, authors in [24] addressed the cooperative localization approach for a small group of unmanned aerial vehicles (UAVs), and the proposed approach estimated each UAV’s relative position inside the group using ranging measurements and estimating global positioning magnetic anomaly measurements. A cooperative localization algorithm with TOA and received signal strength measurements was proposed in [26], and the proposed cooperative localization algorithm significantly improved the localization accuracy of mobile nodes that could not directly connect to a sufficient number of anchor nodes in a wireless sensor network. Authors in [27] investigated the operational framework for cooperative localization of UAVs using GNSS, microelectromechanical systems, INS, and ultra-wide-band (UWB) sensors to improve accuracy in regions that lack GNSS, and they provided a comparison of distributed and centralized architectures and proved that centralized architecture generally provides higher localization accuracy compared with the distributed architecture. An incentive mechanism for cooperative localization was proposed in [28] to improve the localization accuracy of wireless network nodes in harsh environments due to poor coverage or signal blockage. A hybrid cooperative positioning method based on GNSS, network anchors, and cooperative measurements was proposed in [29], and the proposed method improved localization accuracy of network agents with cooperative measurements between them.

In order to improve JTIDS network users’ accuracy of the horizontal plane and vertical direction, the TDMA datalink cooperative navigation method based on INS/JTIDS/BA is proposed. In the proposed method, an estimator is decomposed into altitude and horizontal planes. In the vertical direction, a height filter based on a barometric altimeter (BA) is used to correct altitude errors of the inertial navigation system (INS) independently. In the horizontal plane, the TDMA datalink cooperative navigation algorithm uses general navigation measurements, cooperative navigation measurements, and predicted states to estimate latitude and longitude errors of INS.

The rest of this paper is organized as follows: The second section overviews the basic principles of JTIDS navigation and introduces TDMA datalink cooperative navigation method of JTIDS. The third section introduces the integration architecture of the TDMA datalink cooperative navigation algorithm based on INS/JTIDS/BA and presents the RTT filter and altitude filter, and then explains the division method of estimate time slice and navigation slots, measurements, and WLS estimator of the proposed algorithm in detail. In the fourth section, the WHDOP of the TDMA datalink cooperative navigation algorithm and the effect of cooperative measurements on estimate errors are analyzed. In the fifth section, a simulation study is conducted to analyze and evaluate the proposed algorithm. The sixth section concludes the paper.

2. Principles of JTIDS Navigation and TDMA Datalink Cooperative Navigation Method of JTIDS

JTIDS is a synchronous, time division multiple access, spread spectrum communication system. As illustrated in Figure 1, members of JTIDS operate with different roles. The navigation controller (NC) establishes the relative coordinates. One user of the network runs time reference (TR), and other users will synchronize with TR directly or indirectly. Primary users (PUs) are permitted to synchronize with RTT protocol, and secondary users (SUs) are permitted to perform clock synchronization and navigation passively. Terminals with high absolute position accuracy are designated as position references (PRs) [6].

Figure 1. JTIDS navigation network architecture.

2.1. Principles of JTIDS Navigation

Some slots of JTIDS are selected as navigation slots to transmit precise participant location and identification (PPLI) messages. PRs and terminals with high accuracy positions can transmit PPLI messages in their navigation slots as navigation sources. The structure of JTIDS messages is illustrated in Figure 2. The information of PPLI messages contains source terminal positions and speed, as well as position quality and time quality. Positioning users can extract general TOA navigation information from the synchronization header of the PPLI message and obtain position information of source terminals, and then estimate positions with these pieces of information [30].

Figure 2. JTIDS message structure.

In the general JTIDS navigation method, estimators of users process information and estimate distributed positions, and users only use PPLI messages transmitted from the navigation sources with higher accuracy.

2.2. TDMA Datalink Cooperative Navigation Method of JTIDS

In JTIDS network positioning, users and navigation sources are randomly distributed in the horizontal plane, and the geometrical distribution of navigation sources may not meet the requirements of each positioning user; therefore, more measurements are needed. However, the number of navigation sources is fixed within a short period time, so we take advantage of measurements between positioning users to improve their positioning accuracy; moreover, the height filter with measurements of BA is designed to estimate the heights of users.

In the TDMA datalink cooperative navigation method, several positioning users estimate their state together as a whole, and one centralized estimator is used to process positioning users’ information and estimate their latitude and longitude errors of INS together. We call these positioning users cooperative members.

As shown in Figure 3, in order to distinguish measurements used in the horizontal plane estimator, we define the measurements transmitted between the datalink navigation sources. Cooperative members are general navigation measurements, and measurements transmitted between cooperative members are cooperative navigation measurements.

Figure 3. Cooperative navigation method.

In the TDMA datalink cooperative navigation method, latitude and longitude errors of all cooperative members are the error states we need to estimate, so if there are K cooperative members participating in the cooperative navigation calculation, the state vector of estimator has 2K dimensions. The TDMA datalink cooperative navigation algorithm needs to process all cooperative navigation members’ general navigation measurements and cooperative navigation measurements and predicted states. Cooperative measurements provide more constraint relationships between cooperative members, and cooperative members’ states will convergence together and lead to a higher positioning accuracy.

3. The Implementation of TDMA Datalink Cooperative Navigation Algorithm Based on INS/JTIDS/BA

JTIDS network members’ VDOP is poor, considering the independence of measurements and poor VDOP, and we decompose the dimension of the estimator into altitude and horizontal planes. The TDMA datalink cooperative navigation algorithm is an integrated navigation system based on INS/JTIDS/BA. The architecture of the algorithm is shown in Figure 4, and the output of the RTT filter is used to correct the TOA measurements. In the vertical direction, the height filter processes BA measurements with EKF independently [31], and the TDMA datalink cooperative navigation algorithm is used to estimate longitude and latitude errors.

Figure 4. INS/JTIDS/BA integration architecture.

3.1. Height Filter

INS altitude error Δh and height velocity error ΔV_h are the states of the height filter. According to the second-order damped error propagation equation of INS, the state equation of the height filter can be obtained as follows:

$${\mathit{X}}_h\left(k\right)={\mathit{\Phi }}_h\left(k,k-1\right){\mathit{X}}_h\left(k-1\right)+{\mathit{v}}_h\left(k-1\right)$$

(1)

where ${\mathit{X}}_h\left(k\right)={[\Delta h\left(k\right) \Delta \mathit{V}\left(k\right)]}^T$ denotes the state vector, and ${\mathit{v}}_h\left(k-1\right)$ denotes the process noise vector, and covariance matrix Q_h is calculated by

$${\mathit{Q}}_h\left(k-1\right)=\left[\begin{array}{cc}{\sigma }_v^2\frac{T^3}{3}& {\sigma }_v^2\frac{T^2}{2}\\ {\sigma }_v^2\frac{T^2}{2}& {\sigma }_v^2{T}^2\end{array}\right]$$

(2)

where ${\sigma }_v^2$ denotes the variance of altitude velocity noise, and T denotes the discrete interval. The state transition matrix is shown in Equation (3).

$${\mathit{\Phi }}_h\left(k,k-1\right)=\left[\begin{array}{cc}1-k_{1}T& T\\ (\frac{2g_0}{R_e}-\frac{V_e^2}{R_1^2}-\frac{V_n^2}{R_2^2})T& 1\end{array}\right]$$

(3)

where $R_e$ denotes the major axis of the earth reference ellipsoid. $R_1^{}=R_N^{}+h$, where R_N denotes the curvature radius of the prime vertical, $R_2^{}=R_M^{}+h$, where R_M denotes the curvature radius of meridian. $V_e$ and $V_n$ are the velocities of east and north, respectively.

The measurement equation is

$$\begin{array}{cc}\hfill Z_k& =h_{BA}-h_{INS}\hfill \\ & =-\Delta h+w_h\hfill \end{array}$$

(4)

$h_{BA}$ denotes altitude obtained from BA, $h_{INS}$ denotes altitude obtained from INS, and w_h denotes measurement noise [32].

3.2. RTT Filter

The frequency difference is assumed as a first order Markov process, and clock offset is the integral of the frequency difference.

$${\mathit{X}}_t\left(k\right)={\mathit{\Phi }}_t\left(k,k-1\right){\mathit{X}}_t\left(k-1\right)+{\mathit{W}}_t\left(k-1\right)$$

(5)

where ${\mathit{X}}_t\left(k\right)={\left[\begin{array}{cc}\Delta t\left(k\right)& \Delta f\left(k\right)\end{array}\right]}^T$, and ${\mathit{W}}_t\left(k-1\right)$ denote the process noise vector, and the state transition matrix can be expressed as

$${\mathit{\Phi }}_t\left(k,k-1\right)=\left[\begin{array}{cc}1& T\\ 0& 1-{\beta }_{f}T\end{array}\right]$$

(6)

where ${\beta }_f$ denotes the correlation coefficient, and the covariance matrix Q_k−1 is calculated by

$${\mathit{Q}}_{k-1}^{}=\left[\begin{array}{cc}{\sigma }_{fN}^2\frac{T^3}{3}& {\sigma }_{fN}^2\frac{T^2}{2}\\ {\sigma }_{fN}^2\frac{T^2}{2}& \left(1-{\beta }_{f}T\right){\sigma }_{fN}^{2}T\end{array}\right]$$

(7)

where ${\sigma }_{fN}^2$ denotes variance of clock frequency drift. The measurement equation of RTT filter is

$$Z_t=\Delta b+v$$

(8)

where $\Delta b$ denotes the user’s clock error obtained with RTT, and v is measurement noise of RTT [33].

3.3. Cooperative Navigation Algorithm in Horizontal Plane

In the horizontal plane, the state we estimate is all cooperative members’ longitude and latitude errors of INS. We compute the difference between TOA measurement and calculate pseudo-range, given the measurement equation which describes the relationship between the difference and longitude and latitude errors. Then, error states are estimated with general navigation measurements, cooperative navigation measurements, and predicted error states.

3.3.1. Estimate Time Slice and Arrangement of Time Slot

The architecture of the proposed cooperative navigation algorithm is centralized. One of these cooperative members runs the cooperative navigation algorithm as a computing center unit. The interval of algorithm execution time is a short time slice T_p, and we assume that INS errors do not change much during each interval time T_p. INS errors of cooperative members can be estimated as invariable error states in this period of time.

As shown in Figure 5, each time slice contains many navigation slots. Part of the navigation time slots are occupied by cooperative members to transmit cooperative information messages as cooperative navigation slots, and other navigation slots are occupied by navigation sources to transmit PPLI messages as general navigation slots. General navigation slots and cooperative navigation slots are alternately distributed in each estimate period list.

Figure 5. Estimate period and arrangement of navigation slots.

As shown in Figure 6, we give an example of three cooperative members within one estimate time slice. Navigation slot n, navigation slot n − 2, and navigation slot n − 4 are cooperative navigation slots, and cooperative members c3, c2, and c1 transmit cooperative messages in these navigation slots. Navigation slot n − 1, navigation slot n − 3, and navigation slot n − 5 are general navigation slots, and navigation source s1 and s2 transmit PPLI messages in these navigation slots.

Figure 6. Relationship of navigation slots and measurements relation.

The information computing center unit used to compute is divided into two parts. One part is the measuring the computing center extracted from PPLI messages and cooperative navigation messages, and the other part is other cooperative members sharing to the computing center unit with cooperative navigation messages.

The shared information of the cooperative member includes measured values of general TOA measurements, positions of the navigation source, measured values of cooperative TOA measurements, the INS output of the cooperative member, and the predicted error states. The computing center will estimate cooperative members’ error states with weighted least squares (WLS) when enough information is collected. Estimate results will be broadcasted, and then these cooperative members revise latitude and longitude errors of INS. The centralized algorithm need the guarantee of network traffic; therefore, the proposed algorithm is more suitable for a small number of cooperative members.

3.3.2. General Navigation Measurements

• Measurement model

If slot n is the general navigation slot, navigation source s transmit a PPLI message. The TOA measurement between the navigation source s and the cooperative member c is given as follows [34]:

$$TO{A}_{c-s}^n=r_{T:c-s}^n+\Delta t_c^n-(\Delta t_s^n-\Delta {\widehat{t}}_s^n)+w_{c-s}^n$$

(9)

where $r_{T:c-s}^n$ is the actual distance between source s and cooperative member c. $\Delta t_c^n$ and $\Delta t_s^n$ are clock offsets of member c and source s, respectively, and $\Delta {\widehat{t}}_s^n$ denotes the clock offset of navigation source estimated by the RTT filter, and the clock offset unit is converted from second into meter. $w_{c-s}^n$ denotes TOA measurement noise and is modeled as a zero-mean white Gaussian process.

The calculate distance between cooperative member c and source s in slot n is

$$r_{c-s}^n={\left[{(X_c^n-X_s^n)}^2+{(Y_c^n-Y_s^n)}^2+{(Z_c^n-Z_s^n)}^2\right]}^{\frac{1}{2}}$$

(10)

where ${\mathit{s}}_c^n=\left(\begin{array}{ccc}{X}_c^n& Y_c^n& Z_c^n\end{array}\right)$ is the earth centered earth fixed (ECEF) rectangular coordinate vector obtained from INS. The calculate pseudo-range is

$${\rho }_{c-s}^n=r_{c-s}^n+\Delta {\widehat{t}}_c^n$$

(11)

where $\Delta {t^}_c^n$ denotes cooperative members’ clock offsets, which are approximatively estimated by the RTT filter.

The actual distance between cooperative member c and navigation source s in slot n can be linearized applying a Taylor series around ${\mathit{s}}_c^n=(\begin{array}{ccc}{X}_c^n& Y_c^n& Z_c^n\end{array})$.

$$\begin{array}{cc}\hfill r_{T:c-s}^n& =r_{c\text{-}s}^n-\frac{X_c^n-X_s^n}{r_{c-s}^n}(X_{cT}^n-X_c^n)-\frac{Y_c^n-Y_s^n}{r_{c-s}^n}(Y_{cT}^n-Y_c^n)-\frac{Z_c^n-Z_s^n}{r_{c-s}^n}(Z_{cT}^n-Z_c^n)\hfill \\ & =r_{c\text{-}s}^n+\frac{X_c^n-X_s^n}{r_{c-s}^n}\Delta X_c^n+\frac{Y_c^n-Y_s^n}{r_{c-s}^n}\Delta Y_c^n+\frac{Z_c^n-Z_s^n}{r_{c-s}^n}\Delta Z_c^n\hfill \end{array}$$

(12)

$\Delta {\mathit{s}}_c^n={\mathit{s}}_c^n-{\mathit{s}}_{cT}^n$ is the INS errors vector of cooperative member c.${\mathit{s}}_{sT}^n=\left(\begin{array}{ccc}{X}_{sT}^n& Y_{sT}^n& Z_{sT}^n\end{array}\right)$ is the true position of navigation source s. The measurement equation can be written as

$$\begin{array}{cc}\hfill \Delta {\rho }_{c\text{-}s}^n& =TO{A}_{c\text{-}s}^n-{\rho }_{c\text{-}s}^n\hfill \\ & =r_{}^n+\Delta t_s^n-\Delta t_c^n+w_{c\text{-}s}^n-(r_{c\text{-}s}^n+\Delta {\widehat{t}}_s^n-\Delta {\widehat{t}}_c^n)\hfill \\ & =r_{c\text{-}s}^n+\frac{X_s^n-X_c^n}{r_{c-s}^n}\Delta {\overline{X}}_c^n+\frac{Y_s^n-Y_c^n}{r_{c-s}^n}\Delta {\overline{Y}}_c^n+\frac{Z_s^n-Z_c^n}{r_{c-s}^n}\Delta {\stackrel{\leftharpoonup }{Z}}_c^n+ (\Delta t_c^n-\Delta {\widehat{t}}_s^n)-{(\Delta t_s^n-\Delta {\widehat{t}}_s^n)}_s^n+w_{c\text{-}s}^n\hfill \end{array}$$

(13)

INS errors are converted from ECEF rectangular coordinates to geodetic coordinates with Equations (A2)–(A4), derivation process is shown in the Appendix A. Equations (A2)–(A4) are substituted into Equation (13), and the equation is simplified.

$$\begin{array}{l}TO{A}_{c\text{-}s}^n-{\rho }_{c\text{-}s}^n\\ =h_{\ln g}^n\Delta {\lambda }_c^n+h_{lat}^n\Delta L_c^n+h_h^n(\Delta {\widehat{h}}_c^n+{\epsilon }_{h c}^n)+(\Delta t_c^n-\Delta {\widehat{t}}_c^n)-(\Delta t_s^n-\Delta {\widehat{t}}_s^n)+w_{c\text{-}s}^n\end{array}$$

(14)

where $h_{\ln gs}$, $h_{lats}$, and $h_h{}_s$ denote coefficients after simplification, and $\Delta {\widehat{h}}_c^n$ denote INS height errors estimated by the height filter of c. $\Delta L_c^n$ and $\Delta {\lambda }_c^n$ are the longitude and latitude errors of INS, respectively, which want to estimate in the horizontal plane. The measurement equation of the general navigation measurement can be expressed as

$$\begin{array}{cc}\hfill Z_{c\text{-}s}^n& =TO{A}_{c\text{-}s}^n-{\rho }_{c\text{-}s}^n-h_h^n\Delta {\widehat{h}}_c^n\hfill \\ & =h_{\ln g}^n\Delta {\lambda }_c^n+h_{lat}^n\Delta L_c^n+v_{c\text{-}s}^n\hfill \end{array}$$

(15)

$$v_{c-s}^n=h_h^n{\epsilon }_{h c}^n+{\epsilon }_{t c}^n+w_{c-s}^n-{\epsilon }_{t s}^n$$

(16)

where $v_{c-s}^n$ denotes errors of measurement, ${\epsilon }_{t c}^n$ and ${\epsilon }_{t s}^n$ denote differences between the true clock offset and the result of the RTT filter namely ${\epsilon }_{t c}^n= \Delta t_c^n-\Delta {\widehat{t}}_c^n$ and ${\epsilon }_{t s}^n=\Delta t_s^n-\Delta {\widehat{t}}_s^n$, respectively.

2.

Analysis of Measurement Errors

The measurement noise variance is

$$R_{c-s}^n=E\left[v_{c\text{-}s}^n{v}_{c\text{-}s}^n{}^T\right]$$

(17)

$$v_{c-s}^n=h_h^n{\epsilon }_{h c}^n+{\epsilon }_{t c}^n+w_{c-s}^n-{\epsilon }_{t s}^n$$

(18)

${\epsilon }_{h c}^n$ denotes the errors of the height filter result. We assume that errors are independent of each other. $R_{c1\text{–}c2}^n{}_{}$ is written as

$$R_{c1\text{–}c2}^n{}_{}=(h_{hc}^n{)}^2{\sigma }_{h c}^n{}^2+ {\sigma }_{t c}^n{}^2+ {\sigma }_{t s}^n{}^2+{\sigma }_{c-s}^n{}^2$$

(19)

where ${\sigma }_{c-s}^n{}^2$ denotes the variance of measurement noise, ${\sigma }_{h c}^n{}^2$ denotes the variance of ${\epsilon }_{h c}^n$, which can be estimated by the covariance matrix of height filter, ${\sigma }_{t c}^n{}^2$ and ${\sigma }_{t s}^n{}^2$ denote the variance of ${\epsilon }_{t c}^n$ and ${\epsilon }_{t s}^n$, respectively. ${\sigma }_{t c}^n{}^2$ and ${\sigma }_{t s}^n{}^2$ can be approximatively estimated by the covariance matrix of the RTT filter.

3.3.3. Cooperative Navigation Measurements

1.

Measurement model

If slot n is the cooperative navigation slot, the TOA between cooperative member c1 and c2 in slot n is given by

$$TO{A}_{c1\text{–}c2}^n=r_{T:c1\text{–}c2}^n+\Delta t_{c1}^n-\Delta t_{c2}^n+w_{c1\text{–}c2}^n$$

(20)

where $r_{T:c1\text{–}c2}^n$ denotes actual distance between cooperative member c1 and c2, $\Delta t_{c1}^n$ and $\Delta t_{c2}^n$ denote time bias of member c1 and c2, respectively, and $w_{c1\text{–}c2}^n$ represents TOA measurement errors modeled as a zero-mean white Gaussian process. ${\mathit{s}}_{c1}^n=\left(\begin{array}{ccc}{X}_{c1}^n& Y_{c1}^n& Z_{c1}^n\end{array}\right)$ and ${\mathit{s}}_{c2}^n=\left(\begin{array}{ccc}{X}_{c2}^n& Y_{c2}^n& Z_{c2}^n\end{array}\right)$ are positions in the ECEF rectangular coordinate system of cooperative and are outputs of INS. The calculate distance between c1 and c2 is

$$r_{c1\text{–}c2}^n={\left[{(X_{c1}^n-X_{c2}^n)}^2+{(Y_{c1}^n-Y_{c2}^n)}^2+{(Z_{c1}^n-Z_{c2}^n)}^2\right]}^{\frac{1}{2}}$$

(21)

The calculate pseudo-range is

$${\rho }_{c1\text{–}c2}^n=r_{c1\text{–}c2}^n+\Delta {\widehat{t}}_{c1}^n-\Delta {\widehat{t}}_{c2}^n$$

(22)

where $\Delta {\widehat{t}}_{c1}^n$, $\Delta {\widehat{t}}_{c2}^n$ denote cooperative members’ clock offsets, which are approximatively estimated by RTT filter.

The actual distance between cooperative member c1 and c2 in slot n can be linearized by applying a Taylor series around $(\begin{array}{cccccc}{X}_{c1}^n& Y_{c1}^n& Z_{c1}^n& X_{c2}^n& Y_{c2}^n& Z_{c2}^n\end{array})$

$$\begin{array}{l}{r}_{T:c1-c2}^n=r_{c1-c2}^n+\frac{X_{c1}^n-X_{c2}^n}{r_{c1-c2}^n}(X_{c1T}^n-X_{c1}^n)+\frac{Y_{c1}^n-Y_{c2}^n}{r_{c1-c2}^n}(Y_{c1T}^n-Y_{c1}^n)+\frac{Z_{c1}^n-Z_{c2}^n}{r_{c1-c2}^n}(Z_{c1T}^n-Z_{c1}^n)-\\ \frac{X_{c1}^n-X_{c2}^n}{r_{c1-c2}^n}(X_{c2T}^n-X_{c2}^n)-\frac{Y_{c1}^n-Y_{c2}^n}{r_{c1-c2}^n}(Y_{c2T}^n-Y_{c2}^n)-\frac{Z_{c1}^n-Z_{c2}^n}{r_{c1-c2}^n}(Z_{c2T}^n-Z_{c2}^n)\\ =r_{c1-c2}^n+\frac{X_{c1}^n-X_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{X}}_{c2}^n-\Delta {\overline{X}}_{c1}^n)+\frac{Y_{c1}^n-Y_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{Y}}_{c2}^n-\Delta {\overline{Y}}_{c1}^n)+\frac{Z_{c1}^n-Z_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{Z}}_{c2}^n-\Delta {\overline{Z}}_{c1}^n)\end{array}$$

(23)

where ${\mathit{s}}_{c1T}^n=\left(\begin{array}{ccc}{X}_{c1T}^n& Y_{c1T}^n& Z_{c1T}^n\end{array}\right)$, ${\mathit{s}}_{c2T}^n=\left(\begin{array}{ccc}{X}_{c2T}^n& Y_{c2T}^n& Z_{c2T}^n\end{array}\right)$ denote the true positions of cooperative members. $\Delta {\overline{\mathit{s}}}_{c1}^n=\left(\begin{array}{ccc}\Delta {\overline{X}}_{c1}^n& \Delta {\overline{Y}}_{c1}^n& \Delta {\overline{Z}}_{c1}^n\end{array}\right)$ and $\Delta {\overline{\mathit{s}}}_{c2}^n=\left(\begin{array}{ccc}\Delta {\overline{X}}_{c2}^n& \Delta {\overline{Y}}_{c2}^n& \Delta {\overline{Z}}_{c2}^n\end{array}\right)$ are INS errors of cooperative members. The measurement equation of cooperative navigation measurement can be presented as follows:

$$\begin{array}{cc}\hfill \Delta {\rho }_{c1-c2}^n& =TO{A}_{c1-c2}^n-{\rho }_{c1-c2}^n\hfill \\ & =r_{}^n+\Delta t_{c1}^n-\Delta t_{c2}^n+w_{c1-c2}^n-(r_{c1-c2}^n+\Delta {\widehat{t}}_{c1}^n-\Delta {\widehat{t}}_{c2}^n)\hfill \\ & =r_{c1-c2}^n+\frac{X_{c1}^n-X_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{X}}_{c2}^n-\Delta {\overline{X}}_{c1}^n)+\frac{Y_{c1}^n-Y_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{Y}}_{c2}^n-\Delta {\overline{Y}}_{c1}^n)+\frac{Z_{c1}^n-Z_{c2}^n}{r_{c1-c2}^n}(\Delta {\overline{Z}}_{c2}^n-\Delta {\overline{Z}}_{c1}^n)\hfill \\ & + \Delta t_{c1}^n-\Delta t_{c2}^n+w_{c1-c2}^n-(r_{c1-c2}^n+\Delta {\widehat{t}}_{c1}^n-\Delta {\widehat{t}}_{c2}^n)\hfill \end{array}$$

(24)

INS errors are converted from ECEF rectangular coordinates to geodetic coordinates with Equations (A2)–(A4). Equations (A2)–(A4) are substituted into Equation (24) and the equation is simplified so that we obtain

$$\begin{array}{l}TO{A}_{c1-c2}^n-{\rho }_{c1-c2}^n\\ =h_{\ln gc2}^n\Delta {\lambda }_{c2}^n+h_{\mathrm{lat}c2}^n\Delta L_{c2}^n-h_{\ln gc1}^n\Delta {\lambda }_{c1}^n-h_{\mathrm{lat}c1}^n\Delta L_{c1}^n-h_{hc1}^n\left(\Delta {\widehat{h}}_{c1}^n+{\epsilon }_{h c1}^n\right)\\ +h_{hc2}^n\left(\Delta {\widehat{h}}_{c2}^n+{\epsilon }_{h c2}^n\right)+ \Delta t_{c1}^n-\Delta {\widehat{t}}_{c1}^n-(\Delta t_{c2}^n-\Delta {\widehat{t}}_{c2}^n)+w_{c1-c2}^n\end{array}$$

(25)

Measurement equation of cooperative measurement can be expressed as

$$\begin{array}{cc}\hfill Z_{c1-c2}^n& =TO{A}_{c1-c2}^n-{\rho }_{c1-c2}^n+h_{hc1}^n\Delta {\widehat{h}}_{c1}^n-h_{hc2}^n\Delta {\widehat{h}}_{c2}^n\hfill \\ & =h_{\ln gc2}^n\Delta {\lambda }_{c2}^n+h_{\mathrm{lat}c2}^n\Delta L_{c2}^n-h_{\ln gc1}^n\Delta {\lambda }_{c1}^n-h_{\mathrm{lat}c1}^n\Delta L_{c1}^n+v_{c1\text{–}c2}^n\hfill \end{array}$$

(26)

where $v_{c1\text{–}c2}^n$ denote errors of measurement and can be expressed as

$$v_{c1\text{–}c2}^n =h_{hc2}^n{\epsilon }_{h c2}^n-h_{hc1}^n{\epsilon }_{h c1}^n+ {\epsilon }_{t c1}^n-{\epsilon }_{t c2}^n+w_{c1\text{–}c2}^n$$

(27)

where $\Delta {\lambda }_{c1}^n$, $\Delta {\lambda }_{c1}^n$, $\Delta L_{c2}^n$, $\Delta {\lambda }_{c2}^n$ denote the longitude and latitude errors of INS. $h_{lngc1}^n$, $h_{latc1}^n$, $h_{hc1}^n$, $h_{lngc2}^n$, $h_{latc2}^n$, $h_{hc2}^n$ denote the coefficients after being simplified. $\Delta {\widehat{h}}_{c1}^n$, $\Delta {\widehat{h}}_{c2}^n$ denote INS heights errors estimated by the height filter and ${\epsilon }_{h c2}^n$, ${\epsilon }_{h c1}^n$ denote estimate errors of $\Delta {\widehat{h}}_{c1}^n$, $\Delta {\widehat{h}}_{c2}^n$. ${\epsilon }_{t c1}^n$, ${\epsilon }_{t c2}^n$ denote differences between the true clock offset and result of RTT filter ${\epsilon }_{t c1}^n= \Delta t_{c1}^n-\Delta {\widehat{t}}_{c1}^n$, ${\epsilon }_{t c2}^n=\Delta t_{c2}^n-\Delta {\widehat{t}}_{c2}^n$.

2.

Analysis of Measurement Errors

The noise variance of cooperative navigation measurement is

$$R_{c1\text{–}c2}^n=E\left[v_{c1\text{–}c2}^n{v}_{c1\text{–}c2}^n{}^T\right]$$

(28)

We assume that errors are independent of each other; $R_{c1\text{–}c2}^n$ is written as

$$R_{c1\text{–}c2}^n=(h_{hc2}^n{\sigma }_{h c2}^n{)}^2+(h_{hc1}^n{\sigma }_{h c1}^n{)}^2+ {\sigma }_{t c1}^n{}^2+{\sigma }_{t c2}^n{}^2+{\sigma }_{c1\text{–}c2}^n{}^2$$

(29)

where ${\sigma }_{c1\text{–}c2}^n{}^2$ denotes the variance of measurement noise, ${\sigma }_{h c2}^n{}^2$ and ${\sigma }_{h c1}^n{}^2$ denote the variance of ${\epsilon }_{h c2}^n$ and ${\epsilon }_{h c1}^n$, respectively. ${\sigma }_{h c2}^n{}^2$ and ${\sigma }_{h c1}^n{}^2$ can be estimated by the covariance matrix of height filter, ${\sigma }_{t c1}^n{}^2$ and ${\sigma }_{t c2}^n{}^2$ denote the variance of ${\epsilon }_{t c1}^n$ and ${\epsilon }_{t c2}^n$. ${\sigma }_{t c1}^n{}^2$ and ${\sigma }_{t c2}^n{}^2$, which can be approximatively estimated by the covariance matrix of the height filter and RTT filter.

3.3.4. State-Transition Equation

The error state vector has 7 dimensions. Each cooperative member will predict its own INS error state locally, and the predicted time interval is the estimate time slice.

$$\mathit{X}={[{\varphi }_e,{\varphi }_n,{\varphi }_u,\Delta \lambda ,\Delta L,\Delta V_e,\Delta V_n]}^T$$

(30)

State vector contain angle errors, longitude and latitude errors, and east and north directional velocity errors, but only longitude and latitude errors are used for the WLS estimate.

We assume that the current estimate time slice is k, and thus the state equation is

$$\mathit{X}(k)=\mathit{\Phi }(k,k-1)\mathit{X}(k-1)+\mathit{W}(k-1)$$

(31)

$$\mathit{\Phi }\left(k,k-1\right)=\mathit{I}+\mathit{A}T$$

(32)

where $\mathit{\Phi }(k,k-1)$ denotes the state-transition matrix, $\mathit{W}(k-1)$ denotes the process noise vector, T denotes the discrete interval, and the length of T is equal to the short period of time T_p. The state equation predicts according to the second-order damped error propagation equation of INS, and we get matrix A. The noise variance matrix of state equation is

$$\begin{array}{cc}\hfill \overline{\mathit{Q}}\left(t\right)& =E\left[\overline{\mathit{W}}\left(t\right){\overline{\mathit{W}}}^T\left(t\right)\right]\hfill \\ & =\mathrm{diag}\left[\begin{array}{ccccccc}{\sigma }_{\epsilon e}^2& {\sigma }_{\epsilon n}^2& {\sigma }_{\epsilon z}^2& 0& 0& {\sigma }_{\Delta e}^2& {\sigma }_{\Delta n}^2\end{array}\right]\hfill \end{array}$$

(33)

where ${\sigma }_{\epsilon e}^2$, ${\sigma }_{\epsilon n}^2$, and ${\sigma }_{\epsilon z}^2$ denote the variance of angle errors, and ${\sigma }_{\Delta e}^2$ and ${\sigma }_{\Delta n}^2$ denote variance of east and north velocity errors, respectively.

$${\mathit{Q}}_{k-1\to k}=T\overline{\mathit{Q}}+\frac{T^2}{2}\left(\overline{\mathit{Q}}{\mathit{A}}_{k-1}^T+{\mathit{A}}_{k-1}^{}\overline{\mathit{Q}}\right)$$

(34)

The variance of error state is

$${\mathit{R}}_P=\mathit{\Phi }\left(k,k-1\right){\mathit{P}}_{k-1}\mathit{\Phi }{\left(k,k-1\right)}^T+{\mathit{Q}}_{k-1\to k}$$

(35)

3.3.5. Least-Squares Estimation

We assume that there are K cooperative members participating in the cooperative navigation calculation. We estimate all cooperative members’ latitude and longitude errors in one WLS estimator, so the state vector has 2K dimensions.

$${\mathit{X}}_K={[\Delta {\lambda }_1 \Delta L_1 \Delta {\lambda }_2 \Delta L_2 \cdots \cdots \Delta {\lambda }_K \Delta L_K]}^T$$

(36)

$${\mathit{X}}_K^{}={[\begin{array}{cccc}{\mathit{\theta }}_1^{}{}^T& {\mathit{\theta }}_2^{}{}^T& \cdots \cdots & {\mathit{\theta }}_K^{}{}^T\end{array}]}^T$$

(37)

The corresponding coefficients of ${\mathit{X}}_K^{}$ are

$${\mathit{X}}_K^{}={[\begin{array}{cccc}{\mathit{C}}_1^{}{}^T& {\mathit{C}}_2^{}{}^T& \cdots \cdots & {\mathit{C}}_K^{}{}^T\end{array}]}^T$$

(38)

The TDMA datalink cooperative navigation algorithm can be solved with WLS. We assume that H is a measurement matrix after linearization.

$$\mathit{H}=\left(\begin{array}{c}{\mathit{H}}_{RN}^k\\ {\mathit{H}}_{CN}^k\\ {\mathit{H}}_P^k\end{array}\right)$$

(39)

The corresponding weight matrix is

$$\begin{array}{cc}\hfill \mathit{W}& =\left(\begin{array}{ccc}{\mathit{W}}_{RN}^k& & \\ & {\mathit{W}}_{CN}^k& \\ & & {\mathit{W}}_P^k\end{array}\right)\hfill \\ & =\mathrm{diag}\left[\begin{array}{cccc}1/{\sigma }_1^2& 1/{\sigma }_2^2& \cdots & 1/{\sigma }_m^2\end{array}\right]\hfill \\ & =\mathrm{diag}\left[\begin{array}{cccc}{w}_1^{}& w_2^{}& \cdots & w_m^{}\end{array}\right]\hfill \end{array}$$

(40)

where ${\mathit{W}}_{RN}^k$, ${\mathit{W}}_{CN}^k$, and ${\mathit{W}}_P^k$ denote diagonal matrixes whose diagonal elements are reciprocal of corresponding variance of measurements and predicted error states. The iteration process is solved with the Levenberg–Marquardt (LM) algorithm [35].

The weighted least squares solution is Equation [36]:

$$\Delta {\mathit{X}}_K^{}={({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\mathit{H}}^T\mathit{W}\mathit{b}$$

(41)

4. Analysis of WHDOP and Effect of Cooperative Navigation Measurement on Least Squares Solution Accuracy

4.1. WHDOP of Cooperative Navigation Algorithm

We assume that ${\mathit{\epsilon }}_K^{}$ denotes error vector of $\Delta {\mathit{X}}_K^{}$

$${\mathit{\epsilon }}_K^{}={[{\epsilon }_{\lambda 1} {\epsilon }_{L1} {\epsilon }_{\lambda 2} {\epsilon }_{L2} \cdots \cdots {\epsilon }_{\lambda K} {\epsilon }_{LK}]}^T$$

(42)

With Equation (39), the relationship between ${\epsilon }_K^{}$ and measurement errors vector ${\mathit{\epsilon }}_{\mathit{\rho }}$ can be presented as follows:

$${\mathit{\epsilon }}_K^{}={({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\mathit{H}}^T\mathit{W}{\mathit{\epsilon }}_{\mathit{\rho }}$$

(43)

The covariance matrix of vector ${\mathit{\epsilon }}_K^{}$ is [36]

$$\begin{array}{ll}Cov({\mathit{\epsilon }}_K^{})& =E\left\{{({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\mathit{H}}^T\mathit{W}{\mathit{\epsilon }}_{\mathit{\rho }}\cdot {\left[{({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\mathit{H}}^T\mathit{W}{\mathit{\epsilon }}_{\mathit{\rho }}\right]}_{}^T\right\}\\ & ={({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\sigma }_e^2\end{array}$$

(44)

where ${\sigma }_e^2$ is introduced as a scaling constant to define the weight and is a user-equivalent range error denoting the statistical measurement error [7]. We define the matrix G

$$\mathit{G}={({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}$$

(45)

We assume that cooperative member c is the cth cooperative member of K, and the state ${\mathit{\theta }}_c^{}{}^T={\left[\begin{array}{cc}\Delta {\lambda }_c^{}& \Delta L_c^{}\end{array}\right]}_{}^T$ of cooperative member c corresponds to the 2cth and (2c − 1)th elements of $X_K^{}$.The WHDOP of cooperative member c is

$$WHDO{P}_{}^c=\sqrt{{\mathit{G}}_{2c}^{}+{\mathit{G}}_{2c-1}^{}}$$

(46)

where ${\mathit{G}}_{2c-1}^{}$ and ${\mathit{G}}_{2c}^{}$ denote the (2c − 1)th and 2cth diagonal element of matrix $\mathit{G}$. The variance of positioning error in the horizontal plane can be expressed as

$${\sigma }_P^2=({\mathit{G}}_{2c}^{}+{\mathit{G}}_{2c-1}^{}){\sigma }_e^2$$

(47)

4.2. The Effect of Cooperative Navigation Measurement on Cooperative Members’ Positioning Accuracy

We assume that ${\mathit{h}}_i$ is a cooperative navigation measurement vector between member c1 and c2, and their state vectors are ${\mathit{\theta }}_{c1}^{}{}^T={\left[\begin{array}{cc}\Delta {\lambda }_{c1}^{}& \Delta L_{c1}^{}\end{array}\right]}_{}^T$ ${\mathit{\theta }}_{c2}^{}{}^T={\left[\begin{array}{cc}\Delta {\lambda }_{c2}^{}& \Delta L_{c2}^{}\end{array}\right]}_{}^T$.

$${\mathit{h}}_i=[0 0 \cdots h_{\mathrm{l}ngc1}^{} h_{latc1}^{} 0 0 \cdots h_{\mathrm{l}ngc2}^{} h_{latc2}^{} \cdots 0 0]$$

(48)

We define the coefficient matrix D

$$\begin{array}{cc}\hfill \mathit{D}& =\mathrm{diag}\left[\begin{array}{cccc}{d}_1^{}& d_2^{}& \cdots & d_m^{}\end{array}\right]\hfill \\ & =\mathrm{diag}\left[\begin{array}{cccc}1/\sqrt{w_{11}}& 1/\sqrt{w_{22}}& \cdots & 1/\sqrt{w_m}\end{array}\right]\hfill \end{array},\mathit{W}={\mathit{D}}^T\mathit{D}$$

(49)

$${\mathit{\epsilon }}_K^{}={({\mathit{H}}^T\mathit{W}\mathit{H})}^{-1}{\mathit{H}}^T\mathit{W}{\epsilon }_{\rho }={({(\mathit{D}\mathit{H})}^T\mathit{D}\mathit{H})}^{-1}{(\mathit{D}\mathit{H})}^T\mathit{D}{\mathit{\epsilon }}_{\rho }$$

(50)

Furthermore, ${\mathit{D}}_m{\mathit{H}}_m$ is a matrix with m weighted measurement vectors, and ${\mathit{D}}_{m-1}^i{\mathit{H}}_{m-1}^i$ is ${\mathit{D}}_m{\mathit{H}}_m$ without cooperative navigation measurement vector ${\mathit{h}}_i$; thus, we can obtain

$$\begin{array}{cc}\hfill {\mathit{H}}_m{}^T{\mathit{W}}_m{\mathit{H}}_m& ={({\mathit{D}}_m{\mathit{H}}_m)}^T{\mathit{D}}_m{\mathit{H}}_m\hfill \\ & ={({\mathit{D}}_{m-1}^i{\mathit{H}}_{m-1}^i)}^T{\mathit{D}}_{m-1}^i{\mathit{H}}_{m-1}^i+d_i{\mathit{h}}_i{}^T{d}_i{\mathit{h}}_i\end{array}$$

(51)

We assume that $\stackrel{\wedge }{{\mathit{H}}_m^{}}={\mathit{D}}_m{\mathit{H}}_m$, ${\stackrel{\wedge }{\mathit{h}}}_i=d_i{\mathit{h}}_i$, $\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}$ is $\stackrel{\wedge }{{\mathit{H}}_m^{}}$ without cooperative navigation measurement vector ${\stackrel{\wedge }{h}}_i$

$$\stackrel{\wedge }{{\mathit{H}}_m^{}}=\left(\begin{array}{c}\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}\\ {\stackrel{\wedge }{\mathit{h}}}_i\end{array}\right)$$

(52)

$${\stackrel{\wedge }{{\mathit{H}}_m^{}}}^T\stackrel{\wedge }{{\mathit{H}}_m^{}}={\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}}^T\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}+{\stackrel{\wedge }{{\mathit{h}}_i}}^T\stackrel{\wedge }{{\mathit{h}}_i}$$

(53)

$${\mathit{C}}_m^{}={\stackrel{\wedge }{{\mathit{H}}_m^{}}}^T\stackrel{\wedge }{{\mathit{H}}_m^{}}={\mathit{H}}_m{}^T{\mathit{W}}_m\mathit{H}, {\mathit{C}}_{m-1}^i={\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}}^T\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}={\mathit{H}}_{m-1}^i{}^T{\mathit{W}}_{m-1}^i{\mathit{H}}_{m-1}^i$$

(54)

We use the Sherman–Morrison formula.

$${\left({\mathit{C}}_m^{}\right)}_{}^{-1}={\left({\mathit{C}}_{m-1}^i+{\stackrel{\wedge }{{\mathit{h}}_i}}^T\stackrel{\wedge }{{\mathit{h}}_i}\right)}_{}^{-1}={\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}-\frac{{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}\stackrel{\wedge }{{\mathit{h}}_i}{\stackrel{\wedge }{{\mathit{h}}_i}}_{}^T{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}}{1+{\stackrel{\wedge }{{\mathit{h}}_i}}_{}^T{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}\stackrel{\wedge }{{\mathit{h}}_i}}$$

(55)

$${\mathit{Q}}_{m-1}^i=\frac{{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}\stackrel{\wedge }{{\mathit{h}}_i}{\stackrel{\wedge }{{\mathit{h}}_i}}_{}^T{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}}{1+{\stackrel{\wedge }{{\mathit{h}}_i}}_{}^T{\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}\stackrel{\wedge }{{\mathit{h}}_i}}$$

(56)

We can obtain

$${\left({\mathit{C}}_{m-1}^i\right)}_{}^{-1}={\left({\mathit{C}}_m^{}\right)}_{}^{-1}+{\mathit{Q}}_{m-1}^i$$

(57)

$${\mathit{G}}_{m-1}^i={\mathit{G}}_m^i+{\mathit{Q}}_{m-1}^i$$

(58)

${\mathit{Q}}_{m-1}^i$ is the positive definite matrix, and diagonal elements of ${\mathit{Q}}_{m-1}^i$ are positive [37]. So we can obtain the relation between $WHDO{P}_{m-1}^{i c1}$ and $WHDO{P}_m^{c1}$ from (57).

$$WHDO{P}_m^{c1}=\sqrt{{\left({\mathit{G}}_m^i\right)}_{2c_1}+{\left({\mathit{G}}_m^i\right)}_{2c_1+1}}<\sqrt{{\left({\mathit{G}}_{m-1}^i\right)}_{2c_1}+{\left({\mathit{G}}_{m-1}^i\right)}_{2c_1+1}}=WHDO{P}_{m-1}^{i c1}$$

(59)

$WHDO{P}_{m-1}^{i c1}$ is WHDOP estimate with $\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}$, and $WHDO{P}_m^{c1}$ is WHDOP estimate with $\stackrel{\wedge }{{\mathit{H}}_m^i}$. The variance of positioning error in the horizontal plane can be expressed as

$${\sigma }_{c1P}^2=\left({\mathit{G}}_m^i{}_{:2c1}^{}+{\mathit{G}}_{m:2c1-1}^i\right){\sigma }_e^2=\left[{\mathit{G}}_{m-1}^i{}_{:2c1}^{}+{\mathit{G}}_{m-1}^i{}_{:2c1-1}^{}-\left({\mathit{Q}}_{m-1}^i{}_{:2c1}^{}+{\mathit{Q}}_{m-1}^i{}_{:2c1-1}^{}\right)\right]{\sigma }_e^2$$

(60)

$$q_{c1}^{}=\left({\mathit{Q}}_{m-1}^i{}_{:2c1}^{}+{\mathit{Q}}_{m-1}^i{}_{:2c1-1}^{}\right){\sigma }_e^2$$

(61)

$q_{c1}^{}$ is the reduced variance of member c1 after adding cooperative navigation measurement ${\mathit{h}}_i$ to $\stackrel{\wedge }{{\mathit{H}}_{m-1}^i}$, and in the same way we can obtain the reduced variance of c2.

$$q_{c2}^{}=\left(Q_{m-1}^i{}_{:2c2}^{}+Q_{m-1}^i{}_{:2c2-1}^{}\right){\sigma }_e^2$$

(62)

We came to the conclusion that cooperative navigation measurements can improve WHDOP and positioning accuracy of cooperative member c1 and c2. One cooperative navigation measurement’s contribution to cooperative members’ reduction of horizontal plane variance is $q_{c1}^{}$ and $q_{c2}^{}$, which can be obtained by Equations (61) and (62).

5. Simulation Experiments and Analysis

We simulated a JTIDS network with multi-access mode of TDMA, and an observed member operated the TDMA datalink cooperative navigation algorithm to evaluate the performance of the proposed algorithm. We processed data of BA, INS, and TOA of JTIDS JUs in a computer.

We generated location, speed, and attitude data of JTIDS members by the preset real trajectories, we and simulated INS errors with the error propagation equation of INS. We added INS errors to real navigation information to simulate INS information and added Gaussian white noise and clock error to the real distance of two network members to simulate TOA measurement.

5.1. Simulation Conditions

Within an area of about 100 km², we simulated a JTIDS datalink network of 8 members, and these members were simulated as aircraft members. Member 1 was preset as NC of this JTIDS network, and at the same time it also operated as a time reference; the other members would synchronize their clocks with member 1.

Members 1, 3, 5, and 7 positioning with a federated Kalman filter based on INS, GPS, TOA, and BA filters’ structure is shown in Figure 7. Members 1, 3, 5, and 7 could reach a high localization accuracy in this JTIDS network, so they broadcast PPLI messages as navigation sources, and members 2, 4, 6, and 8 were cooperative members positioned with the TDMA datalink cooperative navigation algorithm.

Figure 7. Filter structure of navigation sources.

The trajectories of the network members are shown in Figure 8: the flight modes of the members including level flight, wheel flight, curve flight, simulation time of 500 s, JTIDS network members’ clock frequency random error of 3 × 10⁻⁶/h, 1-sigma gyro bias error of INS of 0.1° per hour, and the 1-sigma bias error of accelerometer of 0.0001 g.

Figure 8. Horizontal vehicle trajectories of network members.

The basic slot was 7.8125 ms, and the slot interval of two adjoining navigation slots was 8 basic slots. Cooperative members carried out the cooperative navigation algorithm every 500 ms, which means that the algorithm would be carried out every 8 navigation slots, and the estimated time slice T_p was 500 ms. Members 2, 4, 6, and 8 transmitted cooperative navigation information in their navigation slots. Navigation sources broadcast PPLI messages in their navigation slots.

First, we set members 2, 4, 6, and 8 as the observed members and compared the performance of TDMA datalink cooperative navigation algorithm based on INS/JTIDS/BA and EKF based on sequentially processing and the TDMA datalink WLS navigation algorithm without cooperative navigation measurements under the same conditions.

Second, we set member 6 as the observed member and compared the performance of the proposed algorithm with different numbers of cooperative members, and then we analyzed the effect of TOA random errors of measurement and clock calibration accuracy.

5.2. Performance Comparison of Algorithms

The TDMA datalink cooperative navigation algorithm based on INS/JTIDS/BA and EKF based on sequentially processing and TDMA datalink WLS algorithm without cooperative navigation measurement are compared in this part. Cooperative members carry out the compared algorithms in horizontal positioning, and at the same time the same height filter is used. We preset the 1-sigma error of BA to be 50 m, and we preset 1-sigma random noise of pseudo-range measurement to be 3 m.

Compared with the TDMA datalink cooperative navigation algorithm, cooperative navigation measurements are not be used in TDMA datalink WLS.

The state equation of EKF based on sequentially processing is established according to the second-order damped error propagation equation of INS.

The measurement equation is

$$\Delta \rho ={\rho }_{TOA}-{\rho }_c$$

(63)

Cooperative members’ longitude, latitude RMS errors of compared algorithms are presented in Figure 9 and Figure 10. Figure 11 shows the altitude RMS errors of cooperative members.

Figure 9. RMS longitude error comparison between TDMA datalink cooperative navigation, sequentially processing EKF, and TDMA datalink WLS.

Figure 10. RMS latitude error comparison between TDMA datalink cooperative navigation, sequentially processing EKF, and TDMA datalink WLS.

Figure 11. RMS altitude error comparison between TDMA datalink cooperative navigation, sequentially processing EKF, and TDMA datalink WLS.

Cooperative members’ longitude and latitude errors in every estimate moment are presented in Figure 12 and Figure 13, respectively.

Figure 12. Longitude error comparison between TDMA datalink cooperative navigation algorithm, sequentially processing EKF, and TDMA datalink WLS.

Figure 13. Latitude error comparison between TDMA datalink cooperative navigation, sequentially processing EKF, and TDMA datalink WLS.

Conclusions can be drawn from the analysis of positioning results in horizontal plane positioning, and positioning precision of the proposed algorithm performs better. A height filter based on BA revises altitude errors of INS in an independent dimension, so the accuracy of the horizontal plane has little effect on altitude accuracy.

5.3. Effect of the Number of Members That Participate in Cooperative Navigation on Localization Accuracy

We set member 6 as the observed member, let member 6 operate the TDMA datalink cooperative navigation algorithm with measurements from different numbers of cooperative members, and compared the performance in different situations.

As shown in Figure 14, in horizontal plane positioning compared with the case calculate without cooperative navigation measurements, more cooperative members mean more cooperative navigation measurements and better WHDOP in the horizontal plane, which leads to better accuracy. Furthermore, altitude accuracy is not affected by the accuracy of the horizontal plane, so it is not changed much.

Figure 14. RMS error comparison between estimates with different numbers of cooperative members.

5.4. Effect of TOA Measurement Random Error on Localization Accuracy

The 1-sigma random error of TOA measurements was set to be 3 m the first time, and then it was increased 3 m at a time. The results of RMS errors are shown in Figure 15.

Figure 15. RMS error comparison of member 6 between different random errors of TOA.

As shown in Figure 14, random errors of TOA measurement effect both general navigation measurements and cooperative navigation measurements, so it mainly affects the accuracy of the horizontal plane. The vertical direction is estimated separately, so random errors of TOA measurements have little influence on accuracy of altitude.

5.5. Effect of Clock Calibration Accuracy on Localization Accuracy

We set member 6 as the observed member, and changed the clock frequency drift of cooperative members to analysis the effect of clock calibration accuracy on localization accuracy.

As shown in Figure 16, clock errors become larger along with the change of clock frequency drift. Clock errors mainly affect TOA measurements, so positioning accuracy of the horizontal plane is more affected. The vertical direction is estimated with BA measurements, so accuracy of altitude is not affected by clock calibration accuracy.

Figure 16. RMS error comparison of member 6 between different clock frequency drift.

6. Conclusions

The TDMA datalink cooperative navigation algorithm based on INS/JTIDS/BA is proposed in this paper. Members of JTIDS calibrate the clock via RTT, and altitude is located by a height filter independently. In the horizontal plane, a cooperative navigation algorithm is proposed to estimate cooperative members’ longitude and latitude errors of INS. We analyze the effect of cooperative navigation measurements on localization errors in theory. We compare the proposed algorithm with the sequentially processing EKF algorithm and TDMA datalink WLS algorithm without cooperative navigation measurements, and we can draw the conclusion from the analysis of positioning results, namely that the algorithm we propose performs better in the same situation. We show the accuracy of the positioning results with different numbers of cooperative members that participate in cooperative navigation and analyze the effect of random error of TOA measurements and clock calibration accuracy. This provides a theoretical basis for datalink location of the TDMA system.