Introduction

Relative navigation is purposed to provide relative position, velocity, and attitude between separately moving vehicles accurately and reliably. It enables highly complex missions such as formation flying, vehicle-to-vehicle collision avoidance, cooperative positioning, and accident monitoring. In addition, combined with the recently introduced networked vehicles based on wireless communication, it can produce many innovative applications in the near future (Dar et al. 2010; Gerla and Kleinrock 2011).

As in the case of conventional single-vehicle navigation, the integration of the global positioning system (GPS) and the inertial navigation system (INS) is one of the most effective ways for relative navigation between multiple vehicles (Yun et al. 1999; Fosbury and Crassidis 2006). The most representative GPS/INS integration methods are the loosely coupled (LC) and tightly coupled (TC) methods. Among the two methods, the LC method is advantageous regarding implementation simplicity but bears theoretically undesirable problems since two filters are arranged in cascaded form (Groves 2008; Kaplan and Hegarty 2006). The TC method offers theoretical soundness but imposes heavy computational burden since a large number of system states need to be considered by a single integration filter.

Related to the mechanization of relative navigation, several notable methods were introduced recently. Alonso et al. (2000) proposed a method utilizing multiple line of sight (LOS) vectors to estimate the relative position and velocity between multiple satellites in formation. Bever et al. (2002) compared the characteristics of three relative navigation approaches; independent separation measurement system, formation needles, and formation flight instrumentation system.

Since many sensors are used in relative navigation, time synchronization is very important. Related to the time synchronization in multi-sensor fusion, an analytic calibration method was proposed by Lee et al. (2002), a GPS-slaved time synchronization method was introduced by Li et al. (2006), a method using a counter and two latching registers was employed by Ding et al. (2008), and a method to estimate the constant time synchronization error by time stamp filtering was proposed by Nilsson and Handel (2010).

Since sensors are mounted on different vehicles moving independently, sensor errors are more variable in relative navigation than in conventional single-vehicle navigation due to different vehicle dynamics and signal environments. For the reason, a critical factor for the accuracy improvement is the treatment of variable error characteristics. To resolve this problem, adaptive estimation methods have been used traditionally to improve estimation accuracy in the presence of time-varying errors (Magill 1965; Mehra 1970, 1971). In adaptive filtering, time-varying process and measurement error covariances are estimated based on residual vectors. The most representative methods in this category are the multiple-model-based adaptive estimation (MMAE) method and the innovation-based adaptive estimation (IAE) method (Mohamed and Schwarz 1999).

In MMAE, a bank of local filters with different statistical models runs simultaneously (Maybeck 1989; Hanlon and Maybeck 2000). The overall system states are estimated by combining all the local filters. MMAE imposes heavy computational burden due to the utilization of many filters at the same time. In IAE, the noise covariance matrices are estimated sequentially based on innovation vectors (Kailath 1971; Mehra 1970, 1971). Since IAE utilizes a single filter, it can reduce computational burden as compared with MMAE.

Related to navigation, adaptive estimation methods were applied to attitude and heading reference systems (AHRS) (Li and Wang 2013), GPS/INS integration (Hao et al. 2009; Zhou et al. 2010; Fakharian et al. 2011), and velocity estimation (Chu et al. 2010). Ding et al. (2007) investigated an online stochastic modeling algorithm and proposed a new adaptive scaling algorithm. Almagbile et al. (2010) compared the innovation and residual vectors in adaptive estimation.

Recently, studies on adaptive relative navigation have been published. Li et al. (2013) proposed an adaptive robust Kalman filter (KF) derived for both process and measurement noise uncertainties. This method improves the robustness of the methodology proposed by Garcia-Velo (1997) and can be implemented in GPS-based relative navigation with orbit perturbations and non-Gaussian random measurement errors. Baek and Bang (2013) proposed an adaptive sparse-grid quadrature filter for spacecraft’s relative navigation. This method adjusts the accuracy level of the filter autonomously with a quadrature error adaptation criterion.

To improve the robustness against sensor error variability in relative navigation, we propose an efficient adaptive GPS/INS integration method. Compared with conventional methods, the proposed method estimates the covariances of GPS and inertial measurements separately by the innovations of two fundamentally different filters. One is the position-domain carrier-smoothed-code filter and the other is the velocity-aided KF. By the proposed two-filter adaptive estimation method, covariance estimation procedures for GPS measurement noise and inertial propagation noise can be isolated effectively since each filter estimates its own measurement noise.

First, the relative inertial navigation algorithm is explained and the overall two-filter architecture for GPS/INS integration is introduced. Next, an adaptive position-domain carrier-smoothed-code filter (CSCF) and an adaptive KF are explained. The adaptive CSCF is used for relative positioning and the adaptive KF is used to obtain relative velocity and attitude for improved accuracy. The last section deals with simulation and experiment results to demonstrate the feasibility of the proposed method. Finally, concluding remarks will be given.

Relative inertial navigation algorithm

Figure 1 shows the configuration of different frames used in this research. The inertial-frame (i-frame) is a reference frame in which Newton’s laws of motion apply with non-accelerating but possibly uniform linear motion. The earth-centered, earth-fixed (ECEF) frame (e-frame) is fixed to the center of the earth. The x-axis intersects 0 degree latitude and 0 degree longitude, the z-axis is pointing toward the north, and the y-axis follows the right-handed rule. The navigation-frame (n-frame) is aligned to the local north, east, and downward directions. The body-frame (b-frame) is fixed to the center of gravity of the vehicle and aligned to the vehicle’s forward, right, and downward directions. In relative navigation, the master and slave vehicles correspond to different body frames. To discriminate the body frames related to different vehicles, the master frame (m-frame) and the slave frame (s-frame) are utilized.

Fig. 1
figure 1

Configuration of different frames

Full size image

To obtain high rate relative velocity and attitude, relative angular velocity and specific force are formed by differencing between the master and slave vehicles,

$$\omega_{\text{rel}} = \omega_{\text{is}}^{\text{s}} - C_{\text{m}}^{\text{s}} \omega_{\text{im}}^{\text{m}}$$
(1)
$$f_{\text{rel}} = f^{\text{s}} - C_{\text{m}}^{\text{s}} f^{\text{m}}$$
(2)

where f indicates the specific force, ω is indicates the angular rate of s-frame from i-frame, \(C_{\text{m}}^{\text{s}}\) indicates the transformation matrix from m-frame to s-frame, respectively. Based on the inertial velocity and attitude equations (Lee et al. 1998), relative velocity and attitude are computed as follows,

$$\dot{V}_{\text{rel}} = C_{\text{s}}^{\text{n}} f_{\text{rel}} - \left( {2\varOmega + \rho } \right) \times V_{\text{rel}}$$
(3)
$$\dot{Q}_{\text{rel}} = \frac{1}{2}U(Q_{\text{rel}} )\,\omega_{\text{rel}}$$
(4)
$$Q_{\text{rel}} = \left[ {\begin{array}{*{20}c} {q_{0} } & {q_{1} } & {q_{2} } & {q_{3} } \\ \end{array} } \right]^{\text{T}}$$
(5)
$$U(Q_{\text{rel}} ) = \left[ {\begin{array}{*{20}c} { - q_{1} } & { - q_{2} } & { - q_{3} } \\ {q_{0} } & { - q_{3} } & {q_{2} } \\ {q_{3} } & {q_{0} } & { - q_{1} } \\ { - q_{2} } & {q_{1} } & {q_{0} } \\ \end{array} } \right]$$
(6)

where V rel indicates the relative velocity expressed in n-frame, Ω indicates the earth rate expressed in the n-frame, ρ indicates the transport rate expressed in n-frame, and Q rel indicates the relative quaternion representing the rotation from m-frame to s-frame.

It is assumed in (3) that the master and slave vehicles are under practically the same local gravity field since they are located closely. By applying the similar procedure outlined in Lee et al. (1998), the error dynamics model for relative navigation can be derived as follows,

$$\dot{X} = FX + GW$$
(7)
$$X = \left[ {\begin{array}{*{20}c} {X_{\text{INS}} (s)^{\text{T}} } & {\delta r_{\text{LAE}} (s)^{\text{T}} } & {\delta t_{\text{SE}} (s)} \\ \end{array} } \right]^{\text{T}}$$
(8)
$$X_{\text{INS}} (s)^{\text{T}} = \left[ {\begin{array}{*{20}c} {\delta V_{\text{rel}} } & {\psi_{\text{rel}} } & {\nabla_{\text{s}} } & {\varepsilon_{\text{s}} } \\ \end{array} } \right]$$
(9)
$$W = \left[ {\begin{array}{*{20}c} {w_{\text{rela}}^{\text{T}} } & {w_{\text{relg}}^{\text{T}} } \\ \end{array} } \right]^{\text{T}}$$
(10)
$$F = \left[ {\begin{array}{*{20}c} {F_{11} } & {F_{12} } & {O_{6 \times 4} } \\ {O_{6 \times 6} } & {O_{6 \times 6} } & {O_{6 \times 4} } \\ {O_{4 \times 6} } & {O_{4 \times 6} } & {F_{33} } \\ \end{array} } \right]$$
(11)
$$F_{11} = \left[ {\begin{array}{*{20}c} { - \left( {2\omega_{\text{ie}}^{\text{n}} + \omega_{\text{en}}^{\text{n}} } \right) \times } & {\left( {C_{\text{s}}^{\text{n}} f_{\text{rel}} } \right) \times } \\ {0_{3 \times 3} } & { - \left( {\omega_{\text{in}}^{\text{n}} } \right) \times } \\ \end{array} } \right]$$
(12)
$$F_{12} = \left[ {\begin{array}{*{20}c} {C_{\text{s}}^{\text{n}} } & {O_{3 \times 3} } \\ {O_{3 \times 3} } & { - C_{\text{s}}^{\text{n}} } \\ \end{array} } \right]$$
(13)
$$F_{33} = \left[ {\begin{array}{*{20}c} {O_{3 \times 3} } & {O_{3 \times 1} } \\ {O_{1 \times 3} } & { - 1/\tau } \\ \end{array} } \right]$$
(14)
$$G = \left[ {\begin{array}{*{20}c} {C_{\text{s}}^{\text{n}} } & {O_{3 \times 3} } \\ {O_{3 \times 3} } & { - C_{\text{s}}^{\text{n}} } \\ {O_{10 \times 3} } & {O_{10 \times 3} } \\ \end{array} } \right]$$
(15)

where \(\delta r_{\text{LAE}}^{\text{T}}\) is the relative lever arm error expressed in the n-frame, \(\delta t_{\text{SE}}^{\text{T}}\) is the time synchronizing error between GPS and inertial measurements, δV rel is the relative velocity error expressed in the n-frame, \(\psi_{\text{rel}}\) is the relative attitude error expressed in m-frame, ∇ s is the relative accelerometer bias expressed in s-frame, ε s is the relative gyro drift expressed in s-frame, w rela is the relative accelerometer white noise expressed in s-frame, and w relg is the relative gyro white noise expressed in s-frame.

It is assumed in (7) that the inertial sensor biases ∇ m and ε m of the master INS are negligibly small compared to inertial sensor biases ∇ s and ε s of the slave INS due to self-compensation of the master INS, pre-filtering with a stand-alone algorithm, or utilizing a higher-grade IMU for the master vehicle. To obtain feasible accuracy with minimum hardware, both the lever arm errors and the time synchronization error between GPS and INS are also considered as shown in (8).

Configuration of relative GPS/INS integration

For GPS/INS integration, the LC and TC methods have been widely utilized. Among the two representative methods, the LC method is advantageous in terms of implementation simplicity. However, in the LC method, the output of a GPS filter may be used as the input to the integration KF. In this case, the errors in GPS position solutions used in the KF measurement updates are not white but temporally correlated. For this reason, cascaded filtering problem may occur in the LC method (Groves 2008; Kaplan and Hegarty 2006). In this case, the basic KF assumption is not satisfied since the KF assumes that measurement and process errors are white Gaussian for theoretical optimality. Even when snapshot methods are utilized in computing position solutions, temporally correlated error also appears due to the ionospheric and tropospheric errors.

In comparison with the LC method, the TC integration utilizes raw GPS measurements. Due to the utilization of raw measurements, the cascaded filtering problem can be avoided. However, when the number of visible GPS satellites increases, the total number of filter states increases considerably. The increased number of states may cause a significant computational burden.

To combine the advantages and avoid the disadvantages of the traditional LC and TC methods, a two-filter GPS/INS integration method was proposed recently (Park et al. 2011). The configuration of the GPS/INS integration based on the two filters is shown in Fig. 2. The figure shows that the CSCF estimates accurate position and the KF estimates the remaining INS error states. The CSCF is advantageous in obtaining relative positions with centimeter-level accuracy, and the KF can provide high-speed velocity and attitude outputs. The high-speed relative positions can be obtained by combining the CSCF-provided position and the KF-provided velocity. It should be noted that, since the KF utilizes accurate GPS velocity formed by carrier phase measurements instead of a pre-filter, the cascaded filtering problem does not occur in the proposed method since raw measurements are utilized as the input to each filter.

Fig. 2
figure 2

Configuration of GPS/INS integration based on two filters

Full size image

Extending the previous two-filter method, the proposed adaptive GPS/INS integration method also implements two heterogeneous filters, the adaptive single differencing (SD) position-domain (PD) CSCF and the adaptive GPS/INS KF. The architecture of the proposed adaptive method is shown in Fig. 3, where the adaptive SD PD CSCF is applied to the GPS measurements and the adaptive GPS/INS KF is applied to the relative INS. As shown in the figure, the relative inertial measurements are utilized as input to the relative INS algorithm to obtain the relative attitude and velocity at high output rate. GPS measurements are utilized as input to the adaptive SD PD CSCF for relative positioning. Also, GPS carrier phase measurements are utilized to compute the GPS relative velocity. Finally, the relative velocities and estimated noise covariance are utilized as the input to the adaptive KF for accurate relative velocity and attitude.

Fig. 3
figure 3

Configuration of proposed relative GPS/INS Integration

Full size image

Adaptive estimation methods

In order to improve accuracy by estimating noise covariances, two filters are proposed, an adaptive single differencing position-domain CSCF and an adaptive GPS/INS KF. They utilize innovation sequences representing differences between estimated and actual measurements. The noise covariances of GPS measurements are estimated by the adaptive CSCF, and the noise covariances of inertial measurements are estimated by the adaptive KF.

Adaptive single differencing position-domain CSCF

The CSC filtering approach was proposed by Hatch (1983) for high-accuracy GPS applications. Compared with Kalman filtering, CSC filtering does not require any dynamics model for the time propagation of system states. Instead, it utilizes high-accuracy carrier phase measurements to account for the incremental positions between two successive time instants. Since the CSCF does not utilize any dynamics model, the carrier phase measurement noise becomes the main error source for time propagations. For the reason, accurate time propagation in CSC filtering requires accurate covariance estimation of carrier phase measurements. Related to this problem, the adaptive double differencing (DD) PD CSCF was recently proposed (Lee et al. 2011). However, since the structure of DD measurements causes additional computational complexity due to the concept of reference satellite, the DD form is not suitable for relative navigation. For the reason, we focus on the adaptive SD PD CSCF.

A representative algorithm of the PD CSCF is summarized in Table 1. More details on the PD CSC principle can be found in Lee and Rizos (2008) and Lee et al. (2005). The variables utilized in Table 1 are summarized as follows,

$$X_{k} = \left[ {\begin{array}{*{20}c} {x_{u,k} } \\ {b_{u,k} } \\ \end{array} } \right],\;H_{k} = \left[ {\begin{array}{*{20}c} {h_{1,k} } & {h_{2,k} } & \cdots & {h_{j,k} } \\ \end{array} } \right]^{\text{T}}$$
(16)
$$Z_{k} = H_{k} \delta \bar{X}_{k} + \varepsilon_{p,k}$$
(17)
$$\varOmega_{k} = H_{k} \Delta X_{k - 1} + W_{k}$$
(18)
$$W_{k} = - \Delta H_{k - 1} \delta \hat{X}_{k - 1} - \varepsilon_{\phi ,k} + \varepsilon_{\phi ,k - 1}$$
(19)
$$\Delta X_{k - 1} = X_{k} - X_{k - 1}$$
(20)
$$\delta \bar{X}_{k - 1} = \bar{X}_{k - 1} - X_{k - 1} ,\quad \delta \hat{X}_{k - 1} = \hat{X}_{k - 1} - X_{k - 1}$$
(21)
$$\Delta H_{k - 1} = H_{k} - H_{k - 1} ,\quad h_{j,k} = \left[ {\begin{array}{*{20}c} {e_{j,k}^{\text{T}} } & { - 1} \\ \end{array} } \right]^{\text{T}}$$
(22)

where \(\tilde{p}_{k}^{{}}\) and \(\tilde{\phi }_{k}^{{}}\) are the pseudorange and carrier phase measurements, respectively, H k is the observation matrix, e j,k is the difference of LOS vector between the reference and the j-th satellites, k is the time index, j is the satellite index, \(\bar{X}_{k}\) is a priori relative position estimate, \(\hat{X}_{k}\) is a posteriori relative position estimate, \(\delta \bar{X}_{k}\) is a priori relative position error, \(\delta \hat{X}_{k}^{{}}\) is a posteriori relative position error, \(\Delta X_{k}\) is the relative position increment, \(\bar{P}_{k}\) is a priori relative position error covariance, \(\hat{P}_{k}\) is a posteriori relative position error covariance, r p and \(r_{\phi }\) are the noise of pseudorange and carrier phase measurements, respectively, \(\varepsilon_{p,\,k}\) and \(\varepsilon_{\phi ,\,k}\) are pseudorange and carrier phase measurement noise, respectively, and \(\varGamma_{k}\) is the satellite channel selection matrix with elements of 0 and 1.

Table 1 PD CSC filter algorithm
Full size table

In PD CSC filtering, the dimension of the measurement vector changes from time to time depending on the number of visible satellites. The estimate \(\Delta \hat{X}_{k}^{{}}\) can be obtained based on the carrier phase measurement vector \(\varPi_{k + 1}^{{}}\) for time propagation as follows,

$$\Delta \hat{X}_{k - 1} = H_{k}^{ + } \varPi_{k} = \Delta X_{k - 1} - H_{k}^{ + } \Delta H_{k - 1}^{ + } \delta \hat{X}_{k - 1} + H_{k}^{ + } \left( {N_{k} - N_{k - 1} } \right)$$
(23)
$$H_{k}^{ + } = \left( {H_{k}^{\text{T}} H_{k}^{{}} } \right)^{ - 1} H_{k}^{\text{T}}$$
(24)

where N k is the carrier phase measurement noise vector. The innovation sequence \(Z_{\phi ,k}^{{}}\) can be obtained by differencing the indirect measurement \(\varPi_{k}^{{}}\) and the estimated value \(H_{k}^{{}} \Delta \hat{X}_{k - 1}^{{}}\) as follows,

$$\begin{aligned} Z_{\phi ,k} & = \varPi_{{_{k} }} - H_{k} \Delta \hat{X}_{k - 1} \\ & = \left( {I - H_{k} H_{k}^{ + } } \right)\left( {N_{k} - N_{k - 1} } \right) - \left( {I - H_{k} H_{k}^{ + } } \right)\Delta H_{k - 1} \delta \hat{X}_{k - 1} \\ \end{aligned}$$
(25)

Based on (25), the covariance of the innovation vector can be modeled by

$$\begin{aligned} E\left\{ {Z_{\phi ,k} Z_{\phi ,k}^{\text{T}} } \right\} & = \left( {I - H_{k} H_{k}^{ + } } \right)R_{\phi ,k} \left( {I - H_{k} H_{k}^{ + } } \right)^{\text{T}} \\ & \quad + \left( {I - H_{k} H_{k}^{ + } } \right)\Delta H_{k - 1} \hat{P}_{k - 1} \Delta H_{k - 1}^{T} \left( {I - H_{k} H_{k}^{ + } } \right)^{\text{T}} \\ \end{aligned}$$
(26)
$$R_{\phi ,k} = E\left\{ {\left( {N_{k} - N_{k - 1} } \right)\left( {N_{k} - N_{k - 1} } \right)^{\text{T}} } \right\}$$
(27)
$$\hat{P}_{k - 1} = E\left\{ {\delta \hat{X}_{k - 1} \delta \hat{X}_{k - 1}^{\text{T}} } \right\}$$
(28)

For further simplification, it is assumed that the following approximation is valid between two successive time instants. This assumption can be justified since the observation matrix consists of LOS vectors which are making nearly two revolutions (6.28 rad) each day based on the orbital characteristics of the navigation satellites. The assumption is modeled by the following approximation,

$$\left( {I - H_{k} H_{k}^{ + } } \right)\Delta H_{k - 1} \hat{P}_{k - 1} \Delta H_{k - 1}^{\text{T}} \left( {I - H_{k} H_{k}^{ + } } \right)^{\text{T}} \cong O$$
(29)

where O represents the zero matrix of appropriate dimension. It should be noted that (29) is easily satisfied in most cases when GPS measurements are sampled at higher than 1 Hz. With this assumption, the estimated carrier phase noise covariance \(\hat{R}_{\phi ,k}\) can be modeled as,

$$\hat{R}_{\phi ,k} = E\left\{ {Z_{\phi ,k} Z_{\phi ,k}^{\text{T}} } \right\}\left\{ {\left( {I - H_{k} H_{k}^{ + } } \right)\left( {I - H_{k} H_{k}^{ + } } \right)^{\text{T}} } \right\}^{ - 1}$$
(30)

In order to estimate the pseudorange noise covariance, the proposed method computes the innovation sequence similar to the case of carrier phase measurements. The pseudorange noise covariance is required in measurement updates. The innovation sequence \(Z_{p,k}^{{}}\) can be obtained as follows before each measurement update,

$$Z_{p,k} = H_{k} \bar{X}_{k} - Y_{k}$$
(31)

where Y k indicates the real pseudorange measurements. Similar to (26), the instantaneous covariance matrix of the innovation vector can be modeled by

$$\bar{M}_{p,k} = E\left\{ {Z_{p,k} Z_{p,k}^{\text{T}} } \right\}$$
(32)

For more reliability, the smoothed covariance matrix \(\hat{M}_{p,k}^{{}}\) can be obtained by averaging within the pre-defined time interval as follows,

$$\hat{M}_{p,k} = E\left\{ {\bar{M}_{p} } \right\} = \frac{1}{B}\sum\limits_{j = k - B + 1}^{k} {\bar{M}_{p,j} }$$
(33)

where B indicates the pre-defined averaging interval. Finally, the covariance matrix of pseudorange measurement error is obtained by

$$\hat{R}_{p,k} = \hat{M}_{p,k} - H_{k}^{{}} \bar{P}_{k} H_{k}^{\text{T}}$$
(34)

The proposed adaptive CSCF estimates the noise covariance utilizing actual carrier phase and pseudorange measurements as shown in (30) and (34). Consequently, the adaptive CSCF takes the role of accurate position estimation with estimated measurement noise and provides the noise covariance matrices of pseudorange and carrier phase measurements.

Adaptive GPS/INS KF

In the proposed method, the adaptive velocity-aided GPS/INS KF is closely interlaced with the adaptive CSCF. As explained in the previous section, the carrier phase noise covariance is already estimated by the innovation sequences obtained by CSCF. Thus, for the measurement updates of KF, the previously obtained carrier phase noise can be utilized. This procedure reduces computational burden of the adaptive KF by removing unnecessary covariance estimation for carrier phase measurements. Figure 4 shows the architecture of the proposed adaptive GPS/INS KF.

Fig. 4
figure 4

Architecture of Adaptive GPS/INS Kalman filter

Full size image

At each KF measurement update, the previously estimated covariances \(\hat{R}_{\phi ,k - 1}\) and \(\hat{R}_{\phi ,k}\) assist in computing the covariance \(\hat{R}_{{\varLambda_{\text{GPS}} }}\) of the relative velocity measurement \(\varLambda_{\text{GPS}}^{\text{n}}\) as follows,

$$\begin{aligned} \hat{R}_{{\varLambda_{\text{GPS}} }} & = E\left[ {\delta \varLambda_{\text{GPS}}^{\text{n}} \delta \varLambda_{\text{GPS}}^{\text{nT}} } \right] \\ & = C_{\text{e}}^{\text{n}} H_{k}^{ + } \hat{R}_{\phi ,k} H_{k}^{{ + {\text{T}}}} C_{\text{e}}^{\text{nT}} + C_{\text{e}}^{\text{n}} H_{k - 1}^{ + } \hat{R}_{\phi ,k - 1} H_{k - 1}^{{ + {\text{T}}}} C_{\text{e}}^{\text{nT}} \\ \end{aligned}$$
(35)
$$\varLambda_{\text{GPS}}^{\text{n}} = C_{\text{e}}^{\text{n}} H_{k}^{ + } Y_{k} - C_{\text{e}}^{\text{n}} H_{k - 1}^{ + } Y_{k - 1}$$
(36)

where Y k−1 and Y k indicate the relative carrier phase measurements at the previous and current time instants, respectively. As shown in (36), the covariance is affected by the carrier phase noise covariance. Since the carrier phase noise covariance is already computed by the adaptive CSCF as shown in (30), the measurement noise covariance can be obtained without additional complicated computations.

Before the KF measurement update, the indirect measurement considering lever arm compensation is formed as follows,

$$\begin{aligned} Z_{k} & = H\bar{X}_{k} - \varLambda_{\text{GPS}}^{\text{n}} \\ & = \varLambda_{\text{INS}}^{\text{n}} + \left( {I_{3 \times 3} - \left\langle {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\varPsi }_{\text{b1}}^{\text{n}} } \right\rangle } \right)C_{\text{b1}}^{\text{n}} \left\langle {\hat{\omega }_{\text{im}}^{\text{m}} } \right\rangle \hat{r}^{\text{m}} - \left( {I_{3 \times 3} - \left\langle {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\varPsi }_{\text{s}}^{\text{n}} } \right\rangle } \right)C_{\text{s}}^{\text{n}} \left\langle {\hat{\omega }_{\text{is}}^{\text{s}} } \right\rangle \hat{r}^{\text{s}} - \varLambda_{\text{GPS}}^{\text{n}} \\ \end{aligned}$$
(37)

where \(\left\langle x \right\rangle\) indicates the 3 × 3 skew-symmetric matrix constructed by the 3 × 1 vector x, \(\varPsi^{\text{n}}\) indicates the attitude error, and \(\hat{r}\) indicates the lever arm vector. The a priori INS error state \(\bar{X}_{k}^{{}}\) and its covariance matrix \(\bar{P}_{k}^{{}}\) for the current measurement update are obtained by the following equations,

$$\bar{X}_{k} = \varPhi_{k} \hat{X}_{k - 1}$$
(38)
$$\bar{P}_{k} = \varPhi_{k} \hat{P}_{k - 1} \varPhi_{k}^{T} + \hat{Q}_{k}$$
(39)
$$\varPhi_{k} = I + F\left( {t_{k} } \right)\Delta t + \frac{{F^{2} \left( {t_{k} } \right)\Delta t^{2} }}{2}$$
(40)

where t k indicates the k-th time instant, ∆t indicates the time interval between t k−1 and t k , and F can be found in (11). \(\hat{X}_{k - 1}^{{}}\) and \(\hat{P}_{k - 1}^{{}}\) appearing in (38) and (39) indicate a posteriori INS error state and its covariance matrix after the previous measurement update. The observation matrix for the measurement update can be derived as follows.

$$H = \left[ {\begin{array}{*{20}c} I & {C_{\text{s}}^{\text{n}} \left\langle {C_{\text{m}}^{\text{s}} \left\langle {\hat{\omega }_{\text{im}}^{\text{m}} } \right\rangle \hat{r}^{\text{m}} } \right\rangle } & {0_{3 \times 3} } & {C_{\text{b2}}^{\text{n}} \left\langle {\hat{r}^{\text{s}} } \right\rangle } & { - C_{\text{b2}}^{\text{n}} \left\langle {\hat{\omega }_{\text{is}}^{\text{s}} } \right\rangle } & {\dot{\varLambda }^{\text{n}} } \\ \end{array} } \right]$$
(41)

Finally, the measurement update is performed by the following equations,

$$K_{k} = \bar{P}_{k} H_{k}^{\text{T}} \left[ {H_{k} \bar{P}_{k} H_{k}^{\text{T}} + \hat{R}_{{\varLambda_{\text{GPS}} }} } \right]^{ - 1}$$
(42)
$$\hat{P}_{k} = \left[ {I - K_{k} H_{k} } \right]\bar{P}_{k} \left[ {I - K_{k} H_{k} } \right]^{\text{T}} + K_{k} \hat{R}_{{\varLambda_{\text{GPS}} }} K_{k}^{\text{T}}$$
(43)
$$\hat{X}_{k}^{ + } = K_{k} Z_{k}$$
(44)

where K k indicates the Kalman gain. After the measurement update, the instantaneous innovation covariance matrix \(\bar{M}_{k}\) and the smoothed innovation covariance matrix \(\hat{M}_{k}\) are obtained from (37) as

$$\bar{M}_{k} = E\left\{ {Z_{k} Z_{k}^{\text{T}} } \right\}$$
(45)
$$\hat{M}_{k} = E\left\{ {\bar{M}_{j} } \right\} = \frac{1}{B}\sum\limits_{j = k - B + 1}^{k} {\bar{M}_{j} }$$
(46)

Based on the IAE principle, the noise covariance \(\hat{Q}_{k}\) for the next time propagation is obtained combining the innovation covariance matrix \(\hat{M}_{k}\) and the Kalman gain K k as

$$\hat{Q}_{k} = K_{k} \hat{M}_{k} K_{k}^{\text{T}}$$
(47)

In the previous section, it was shown that the noise covariances of GPS measurements are estimated independently of the adaptive KF. The adaptive KF explained in this section needs to estimate the covariance of process noise which is contributed only by inertial sensors. Thus, the roles of CSCF and KF are clearly divided in the proposed method, i.e., the adaptive CSCF estimates the covariances of GPS measurement noise and the adaptive KF estimates the covariances of inertial measurement noise.

Simulation and experiment

A simulation and an experiment were performed to evaluate the feasibility of the proposed method. The simulation evaluates the accuracy improvement of the proposed method compared with the conventional method utilizing pre-defined noise covariances. The experiment evaluates the applicability of the proposed method in actual environment.

Simulation

For the simulation, a land vehicle simulator and a GPS/INS signal generator were utilized. The simulator generated the reference trajectory for a vehicle consisting of a tractor and a trailer. The simulation trajectory consists of two circular turns in opposite directions. Figure 5 shows the trajectory, the baseline between GPS antennas, and the relative velocity generated for the simulation. The signal generator generated GPS/INS measurements based on the reference trajectory.

Fig. 5
figure 5

Trajectory, baseline, and relative velocity of simulation

Full size image

To simulate two vehicles for relative navigation, a master (tractor) and a slave (trailer) running on the same trajectory at a speed of 80 km/h during 6 min were considered. It was assumed that each of the master and slave was equipped with a GPS receiver and an IMU. The stochastic errors of gyro and accelerometer were set as 0.0217\(^{ \circ } /\sqrt {\text{s}}\) and 0.000723 \(({\text{m}}/{\text{s}})/\sqrt {\text{s}}\), respectively. These parameters correspond to low-grade inertial sensors. The inertial sensor sampling rate was set as 100 Hz. The GPS pseudorange and carrier phase errors were modeled as the second-order Gauss-Markov process with the standard deviation of 23 m and 0.28 m/s, respectively. The GPS measurements were generated at 1 Hz. The lever arm was set as zero.

Figure 6 shows relative velocity errors in north, east, and vertical directions with or without the proposed adaptive method. As shown in the figure, the proposed adaptive method generated more accurate relative velocity estimates than the conventional method without adaptive filtering. It can be also seen that the accuracy in the vertical direction is improved much more than in the horizontal direction. The vertical errors are, in general, more influenced by incompletely modeled measurement errors than horizontal errors due to the intrinsic geometry characteristics between GPS satellites and receivers. This means that appropriately estimated measurement errors improve vertical accuracy more than horizontal accuracy. According to the simulation results, the root-mean-square error (RMSE) of the relative velocity estimates by the proposed method in north, east, and vertical directions are 0.0138, 0.0155, and 0.0054 m/s, respectively.

Fig. 6
figure 6

Comparison of relative velocity error magnitudes with and without the proposed adaptive method

Full size image

Figures 7 and 8 show the error magnitudes of the relative attitude and baseline with or without the proposed adaptive method. As shown in Fig. 7, yaw accuracy improves by the proposed method. The RMSE of the relative roll, pitch, and yaw by the proposed method are 0.0789°, 0.3326°, and 0.5786°, respectively. It can also be seen that the proposed method generates more accurate relative position estimates than the conventional method without adaptive filtering. The RMSE (3D) of the estimated baseline by the proposed method is 1.96 mm.

Fig. 7
figure 7

Comparison of relative attitude error magnitudes with and without the proposed adaptive method

Full size image
Fig. 8
figure 8

Comparison of relative baseline error magnitudes with and without the proposed adaptive method

Full size image

Field experiment

To evaluate the applicability of the proposed method in real environments, a kinematic experiment with two isolated ground vehicles was performed. The vehicles were stationary for 130 s at a static point and moved in cascaded formation around the trajectory shown in Fig. 9 for 200 s. Each of the front and rear vehicles was equipped with a Septentrio PolaRX2e GPS receiver and an MPU-6050 IMU. The noise specifications of the gyroscope and the accelerometer were 0.05°/s-rms and 400 µg/\(\sqrt {\text{Hz}}\), respectively (InvenSense Inc. 2013). The sampling rate of inertial and GPS measurements was set as 20 and 1 Hz, respectively. The lever arm between the GPS antenna and the IMU was measured as 110.5 cm for the front vehicle and 210.5 cm for the rear vehicle. The GPS receiver provides dual-frequency measurements; however, only single-frequency measurements were utilized for relative GPS/INS. The field-collected dual-frequency measurements were processed to extract reference trajectory based on integer ambiguity resolutions. All the raw measurements were logged in the standard RINEX format.

Fig. 9
figure 9

Experiment environment and reference trajectory

Full size image

Figure 10 shows the estimated standard deviations of carrier phase and pseudorange measurements. In the case of pseudorange measurements, standard deviations were estimated channel-by-channel. However, in the case of carrier phase measurements, a single standard deviation value was estimated and applied to all the channels since carrier phase errors are far less sensitive to signal environments than pseudorange errors. The areas marked by the rectangles in Fig. 10 correspond to the period when either an existing satellite disappears or a new satellite arises. In this case, the previously accumulated information for making innovation is not valid. For the reason, the noise variance was fixed until a sufficient number of measurements were obtained for an updated noise variance.

Fig. 10
figure 10

Estimated standard deviations of carrier phase and pseudorange measurements

Full size image

Figure 11 compares relative velocity errors and one-sigma envelopes, and Fig. 12 compares relative position errors and one-sigma envelopes. The estimation errors were computed by the differences between the solutions generated by the proposed method and the references generated by dual-frequency RTK method. It should be noted that if the covariances were estimated incorrectly, inconsistency between the one-sigma envelopes and the estimation error would be noticeable. The estimation error would be much larger than the magnitude indicated by the one-sigma envelops even after the filter enters into the steady state (Lee et al. 2013). However, it can be seen in Figs. 11 and 12 that the one-sigma envelopes constrain the actual estimation errors reasonably and inconsistency does not occur when the proposed adaptive method is utilized. This result shows that the proposed method estimates the covariance appropriately in actual environment. The RMSE of the relative positions in north, east, and vertical directions were 0.0074, 0.0055, and 0.0147 m, respectively, with respect to the reference RTK solutions. The RMSE of the baseline was 0.0059 m. The RMSE of the relative velocity in north, east, and vertical directions were 0.0130, 0.0121, and 0.0315 m/s, respectively.

Fig. 11
figure 11

Comparison of relative velocity errors (solid lines) and their one-sigma envelopes (dotted lines)

Full size image
Fig. 12
figure 12

Comparison of relative position errors (solid lines) and their one-sigma envelopes (dotted lines)

Full size image

Figure 13 shows the relative yaw and its one-sigma envelopes generated by the proposed method. Since the dual-frequency GPS measurements cannot provide reference attitude information by the RTK processing, it is difficult to obtain relative yaw errors. However, the relative yaw between the two vehicles would be practically zero when the two vehicles move along the same straight road lane. For the reason, the relative yaw can be considered as the relative yaw error during the straight movements. In the right plot of Fig. 13, four areas marked by dashed rectangles correspond to the periods during which the two vehicles are in straight movement so that the relative yaw equals the relative yaw error. In the four areas, it can be seen that the relative yaw errors are well within the one-sigma envelopes. Thus, it can also be considered that relative yaw is appropriately estimated.

Fig. 13
figure 13

Comparison of relative yaw (solid line) and one-sigma envelopes (dotted lines)

Full size image

Conclusions

An efficient GPS/INS integration method was proposed for adaptive relative navigation. The proposed method avoids the cascaded filtering problem by two fundamentally different filters, i.e., the adaptive SD PD CSCF and the adaptive velocity-aided GPS/INS KF. The adaptive SD PD CSCF generates GPS position with the noise covariance estimates of carrier phase and pseudorange measurements. The adaptive KF generates accurate relative velocity and attitude information at high output rate with the noise covariance estimates of the inertial sensors. By simulation and experiment results, it was shown that the proposed method improves accuracy of navigation solution by appropriate noise covariance estimation. For further improvement, covariance estimation techniques for multi-constellation global navigation satellite systems (GNSS) need to be developed with more field experiments.