Tracking Architecture Based on Dual-Filter with State ... - MDPI

sensors Article

Tracking Architecture Based on Dual-Filter with State Feedback and Its Application in Ultra-Tight GPS/INS Integration Xi Zhang *, Lingjuan Miao and Haijun Shao School of Automation, Beijing Institute of Technology (BIT), Beijing 100081, China; [email protected] (L.M.); [email protected] (H.S.) * Correspondence: [email protected]; Tel.: +86-134-8868-7572 Academic Editor: Vittorio M.N. Passaro Received: 28 January 2016; Accepted: 26 April 2016; Published: 2 May 2016

Abstract: If a Kalman Filter (KF) is applied to Global Positioning System (GPS) baseband signal preprocessing, the estimates of signal phase and frequency can have low variance, even in highly dynamic situations. This paper presents a novel preprocessing scheme based on a dual-filter structure. Compared with the traditional model utilizing a single KF, this structure avoids carrier tracking being subjected to code tracking errors. Meanwhile, as the loop filters are completely removed, state feedback values are adopted to generate local carrier and code. Although local carrier frequency has a wide fluctuation, the accuracy of Doppler shift estimation is improved. In the ultra-tight GPS/Inertial Navigation System (INS) integration, the carrier frequency derived from the external navigation information is not viewed as the local carrier frequency directly. That facilitates retaining the design principle of state feedback. However, under harsh conditions, the GPS outputs may still bear large errors which can destroy the estimation of INS errors. Thus, an innovative integrated navigation filter is constructed by modeling the non-negligible errors in the estimated Doppler shifts, to ensure INS is properly calibrated. Finally, field test and semi-physical simulation based on telemetered missile trajectory validate the effectiveness of methods proposed in this paper. Keywords: baseband signal preprocessing; dual-filter; state feedback; ultra-tight GPS/INS integration; navigation filter with expanded dimension

1. Introduction The navigation errors of GPS are not divergent with time, but a receiver usually performs less well in highly dynamic situations because there are external signals to be dealt with. On the other hand, though INS has the problem of error accumulation, it realizes autonomous navigation and is robust to vehicle dynamics. Since GPS and INS have complementary advantages, their integration can overcome each other's defects and provide a better navigation performance [1]. Unlike loose or tight GPS/INS integration, in which GPS is just an aid to INS, the ultra-tight GPS/INS integration uses INS to couple with GPS tracking channels. In this sense, the ultra-tight form is especially applicable to high dynamic situations [2], and has gradually become the mainstream approach for designing GPS/INS integrated systems. The ultra-tight GPS/INS integration originates from vector tracking [3]. Its core concept is using the known ephemeris and the corrected inertial navigation information to calculate tracking parameters. Namely, satellite signal locking is assisted by the integrated navigation filter, and GPS tracking channels no longer remain independent of each other. According to the structural design of the filter models, the ultra-tight GPS/INS integration methods can be roughly divided into two types. In one case, a single high-dimensional filter is used to estimate the GPS tracking parameters of each

Sensors 2016, 16, 627; doi:10.3390/s16050627

www.mdpi.com/journal/sensors

Sensors 2016, 16, 627

2 of 22

channel and navigation parameters together [4,5], and in the other case, each channel has a baseband signal pre-filter which estimates the tracking parameters for itself, and the integrated navigation filter just needs to estimate the navigation parameters. The second category is actually a form of federated filtering, and usually has higher computational efficiency [6–8]. The baseband signal preprocessing realizes optimal estimation of signal characteristic quantities by KF. Thus, under the application background of the ultra-tight GPS/INS integration, it can restrain phase noise more effectively than the loop filter of the classical GPS tracking channel [9,10]. For each channel, the baseband signal preprocessing is generally completed by a single pre-filter [11]. Both carrier tracking errors and code tracking errors are included in the state vector, namely, these two kinds of tracking errors are tightly coupled through the state and observation equations. However, the precision of carrier and code loops do not have the same order of magnitude. The code loop has great tolerance for tracking errors due to the long wavelength of CA code, while the carrier loop is much more sensitive [12,13]. As preprocessing continues, the code tracking errors would be delivered to the carrier loop and cause the degradation of carrier tracking precision or even loss of lock. In addition, even if the baseband signal preprocessing is applied in a GPS receiver, the Doppler shifts estimated under highly dynamic conditions may still contain significant errors. In the ultra-tight GPS/INS integration, these incorrect GPS outputs eventually lead the estimation of INS errors to be contaminated [14]. That generally means the whole system suffers performance degradation, or even completely crashes. To solve these problems, this paper makes creative contributions in both the tracking and navigation domain. Two independent pre-filters with state feedback are constructed to replace the classical loop filters. The advantages of decoupling between carrier tracking and code tracking are analyzed in detail. Since the single-filter and dual-filter preprocessing models are constructed based on the same principle, their comparison can be free from the influence of other factors. In fact, compared with a popular error-state pre-filter, even the single-filter model has better tracking performance due to the benefits of state feedback. In the ultra-tight GPS/INS integration, local carrier generation is still controlled by state feedback, and the carrier frequency derived from the external navigation information is just used to correct the state feedback value. Meanwhile, a specific integrated navigation filter is presented by taking the pseudorange rate estimation error in each channel as the additional state variable. This scheme ensures the estimation of original state variables, especially the INS errors, is immune to these GPS tracking errors, and hence enables the integrated system to maintain high accuracy even when GPS receiver operates under harsh conditions. The rest of this paper is organized as follows: first, a baseband signal preprocessing model is designed based on a dual-filter structure, while the state feedback control values of carrier and code loops are derived in detail. Then, the specific integrated navigation filter is proposed, and a concrete approach to aid the tracking channel is elaborated. Furthermore, the results of field tests and semi-physical simulation are given and discussed. Finally, the work in this paper is summarized. 2. Construction of Baseband Signal Preprocessing Model Based on Dual-filter Structure The classical GPS tracking channel, which typically adopts second-order or third-order phase control technology, has fixed loop bandwidth. Although a narrower bandwidth is helpful to reduce the thermal noise, in highly dynamic situations, the tracking channel may not retain useful high frequency information and suffers from significant dynamic stress error. These usually lead to signal distortion and tracking loss. Thus, a trade-off between noise resistance and response to dynamics is required while setting the loop bandwidth. In view of the abovementioned fact, the classical control approach cannot cope well with high dynamics, and hence KF, which belongs to the modern control category, is taken into account. KF is a kind of optimal estimator, which allows precise modeling of the tracking loop. With its help, baseband signal preprocessing can achieve accurate control of the local signal generation, even in highly dynamic situations [15].

Sensors 2016, 16, 627

3 of 22

The baseband signal preprocessing model constructed in this paper contains two pre-filters. Sensors 2016, 16, 627 3 ofIn 22 this model, the estimation of carrier and code tracking errors are relatively separate, the same as the manner adopted by classical GPS receiver. Figure 1 just takes carrier loop for example to show the details of this model. It is observed that the loop filter in the the classical classical tracking tracking channel has been replaced by a pre-filter equipped with state feedback. Obviously, designing a suitable KFKF is the keykey to replaced by a pre-filter equipped with state feedback. Obviously, designing a suitable is the accurate carrier tracking. to accurate carrier tracking.

Figure 1. The carrier loop using pre-filter. Figure 1. The carrier loop using pre-filter. 𝑇 𝑐𝑎𝑟𝑟 𝑐𝑎𝑟𝑟 ̇ 𝑐𝑎𝑟𝑟 This paper sets 𝑋𝑘𝑐𝑎𝑟𝑟 =” [∆θcarr 𝑓𝑘 . 𝑓𝑘 ıT ] as the state vector of pre-filter 1, where 𝑘 is the 𝑘 carr carr carr This paper sets Xk “ ∆θk f k fk as the state vector of pre-filter 1, where k is the ̇epoch, epoch, ∆θ𝑐𝑎𝑟𝑟 is the carrier phase error (unit: rad), 𝑓𝑘𝑐𝑎𝑟𝑟 is the input carrier frequency, and 𝑓𝑘𝑐𝑎𝑟𝑟 is 𝑘 . carr thek changing rate ofphase the input carrierrad), frequency. These state variables have the f carr ∆θ is the carrier error (unit: f kcarr is the input carrier frequency, andfollowing is therecurrence changing k relations: rate of the input carrier frequency. These state variables have the following recurrence relations:

. r TTcoh r Tcoh carr 1 1 Tcoh carr carr carr 22 carr “ 11 carr ` 2π f carr carr coh p∆θ t ` π f t qdt ´ 2πu ∆θ  kk,k  (    2  f t   f t ) dt  2ku´kcarr 0 T T ´ 1 k ´ 1 k ´ 1 k ´ 1 1tdt , k 1 k 1 k 1 . k 1 1 tdt coh  coh 0  0 0 T T 1 coh carr ` π f carr T “ ∆θ ` π f carr T 2 ´ πucarr T coh k ´1

k´1 coh

3

1

k´1 coh

k´1 coh

(1) (1)

 integration Tcoh ucarrisf kthe  uk 1 Tfrequency where Tcoh is the coherent local at epoch k ´ 1. k 1   f k 1time, 1 T coh carrier coh k´ 31 carr The output of carrier phase discriminator at epoch k, notated as ∆θk,k´1 , is the average carrier 𝑐𝑎𝑟𝑟 whereerror 𝑇𝑐𝑜ℎ over is the integration is the local carrier frequency at this epoch 𝑘 − can 1. be phase ancoherent integration interval,time, due to𝑢the of Doppler shift. Thus, output 𝑘−1 presence 𝑐𝑎𝑟𝑟 The output of carrier phase discriminator at epoch k, notated as ∆θ , is the average carrier expanded as: 𝑘,𝑘−1 phase error over an integration interval, due to the presence of Doppler shift. Thus, this output can . r Tcoh r Tcoh carr “ 1 carr ` 2π f carr t ` π f carr t2 qdt ´ 1 carr tdt be expanded as: ∆θk,k p∆θ 2πu Tcoh 0 Tcoh 0 ´1 k ´1 k´1. k ´1 k ´1 (2) 1 carr ` π f carr T carr T 2 ´ πucarr T “ ∆θ ` π f T T coh coh k ´1carr 3 carr k´1 coh k´11 coh coh k´11 carr carr 2 carr k ,k 1  (k 1  2f k 1 t  f k 1 t )dt  2uk 1 tdt Tcoh 0 Tcoh 0 Unfortunately, if Equation (2) is directly used to generate the observation model for pre-filter (2)1, 1 carr carr carr carr 2 carr there will be a time lag between [16]. Hence, by moving ∆θk´1 , f kcarr  k 1the  state f k 1 Tvector and f k 1 observation Tcoh  uk 1 Tcoh ´1 coh  . 3 carr and f k´1 to the left side of the equal sign, Equation (1) is rewritten as follows (more details are available Unfortunately, if Equation (2) is directly used to generate the observation model for pre-filter 1, in Appendix A): there will be a time lag between the state vector and observation [16]. Hence, by moving ∆θ𝑐𝑎𝑟𝑟 𝑘−1 , . $ 𝑐𝑎𝑟𝑟 carr carr carr carr 2 carr ̇ 𝑐𝑎𝑟𝑟 to the 𝑓𝑘−1 and 𝑓𝑘−1 left side of the equal sign, Equation (1) is rewritten as follows (more details ’ ∆θk´1 “ ∆θk ´ 2π f k Tcoh ` π f k Tcoh ` 2πuk´1 Tcoh ’ & . are available in Appendix A): carr ´ f carr T f kcarr (3) coh ´1 “ f k k ’ ’ . % . carr carr carr carr carr 2 carr carr fk´k 11 “f   2 f k Tcoh   f k Tcoh  2 uk 1 Tcoh k k carr

carr

carr

2

carr

  carr carr f kcarrTcoh (2) results in:  f k 1 (3)f k intoEquation Substitution of Equation  carr carr 1  f k   f kcarr 1 . carr carr carr

2 ∆θk,k´1 “ ∆θk ´ π f k Tcoh ` π f k Tcoh ` πucarr k´1 Tcoh 3 Substitution of Equation (3) into Equation (2) results in:

1 carr 2  kcarr   f kcarrTcoh   f kcarrTcoh   ukcarr , k 1   k 1 Tcoh 3

(3)

(4)

(4)

Then, in terms of Equations (1) and (4), the state equation and observation equation of pre-filter 1 can be expressed as:

Sensors 2016, 16, 627

4 of 22

Then, in terms of Equations (1) and (4), the state equation and observation equation of pre-filter 1 can be expressed as: # carr carr carr carr Xk “ Φcarr k,k´1 Xk´1 ` Mk´1 ` Wk´1 (5) Zkcarr “ Hkcarr Xkcarr ` Nkcarr ` Vkcarr where:

»

1

2πTcoh

— Φcarr k,k´1 “ – 0

1

0

0

Mkcarr ´1 “

”

Hkcarr “

”

2 πTcoh

ffi Tcoh fl

´πTcoh

(6)

1

´2πTcoh ucarr k ´1 1

fi

0

0

1 2 3 πTcoh

ıT

(7)

ı

Nkcarr “ πTcoh ucarr k ´1

(8) (9)

Zkcarr is the observation, and can be directly obtained from carrier phase discriminator, namely: carr Zkcarr “ ∆θk,k ´1 “ arctan

Qp Ip

(10)

carr represent the system noise and observation noise, respectively. Assume these two Wkcarr ´1 , Vk noises have the following characteristics:

$ ” ı T “ Qcarr δ ’ E rW s “ 0, E W W ’ k k j kj ’ k ’ ” ı & T carr E rVk s “ 0, E Vk Vj “ Rk δkj ’ ’ ” ı ’ ’ % E Wk V T “ 0 j

(11)

where δkj is the Kronecker delta. Then, the corresponding iterative algorithm for pre-filter 1 is given below: carr carr ˆ carr ˆ carr Xˆ k,k (12) ´1 “ Φk,k´1 Xk´1 ` Mk´1 carr carr carr carr,T carr Pk,k ´1 “ Φk,k´1 Pk´1 Φk,k´1 ` Qk´1

´ ¯´1 carr,T carr,T carr carr carr carr Kkcarr “ Pk,k H H P H ` R ´1 k k k,k´1 k k ˘ carr ` carr carr carr Pk “ I ´ Kk Hk Pk,k´1 ´ ¯ carr carr carr Xˆ kcarr “ Xˆ k,k Zkcarr ´ Nkcarr ´ Hkcarr Xˆ k,k ´1 ` K k ´1

(13) (14) (15) (16)

The carrier loop is in essence a Phase Lock Loop (PLL), and aims to achieve the elimination . ` ˘ carr 2 of the carrier phase error. Since ∆θcarr ` 2π f kcarr ´ ucarr Tcoh ` π f carr k`1 “ ∆θk k k Tcoh , by assuming ∆θcarr k`1 “ 0, the feedback at epoch k can be obtained as: ucarr “ k

∆θˆkcarr 1 ˆ. ` fˆkcarr ` f carr Tcoh 2πTcoh 2 k

(17)

It can be seen that ucarr is not exactly equivalent to the estimated input carrier frequency fˆkcarr . k And further discussions about ucarr and fˆkcarr can be found in Sections 3 and 4.1. k

Sensors 2016, 16, 627

5 of 22

” ıT . code f code , where ∆θcode is the Similarly, the state vector of pre-filter 2 is set to Xkcode “ ∆θcode f k k k k .

code phase error (unit: chip), f kcode is the input code frequency, and f code is the changing rate of the k input code frequency. Meanwhile, in consideration of the following formula: f d “ f carr ´ f IF “

f L1 code pf ´ f CA q f CA

(18)

where f d is the Doppler shift, f IF is the Intermediate Frequency (IF), f L1 (1575.42 MHz) is the L1 carrier frequency, f CA (1.023 MHz) is the nominal code frequency, the state equation and observation equation of pre-filter 2 can be written as: #

code code code Xkcode “ Φcode k,k´1 Xk´1 ` Mk´1 ` Wk´1

Zkcode “ Hkcode Xkcode ` Nkcode ` Vkcode where:

»

1

Tcoh

1 2 2 Tcoh

(19)

fi

— ffi — Φcode 1 Tcoh ffi k,k´1 “ – 0 fl 0 0 1 ” ıT code 0 0 Mkcode “ ´T u coh ´1 k´1 » fi 2 1 ´ 21 Tcoh 16 Tcoh fl Hkcode “ – f L1 0 0 f

(20)

(21) (22)

CA

Nkcode “

”

1 code 2 Tcoh uk´1

“

ıT

code ∆θk,k ´1

« Zkcode

0

(23)

ff (24)

f kcarr ´ f IF ` f L1

code are respectively the system and observation noises of pre-filter 2, while the Wkcode ´1 , Vk observation ∆θcode k,k´1 obtained from code phase discriminator can be denoted by:

b b IE2 ` Q2E ´ IL2 ` Q2L 1 code b ∆θk,k ´1 “ 2 b I 2 ` Q2 ` I 2 ` Q2 E

E

L

(25)

L

ucode k´1 is the local code frequency at epoch k ´ 1. Moreover, at epoch k, the local code frequency, namely the feedback for code tracking, can be expressed as: ucode “ k

∆θˆkcode 1 ˆ. ` fˆkcode ` f code Tcoh Tcoh 2 k

(26)

Since code estimation is processed after carrier stripping, its errors have no effect on carrier tracking. At the same time, once these two pre-filters are skillfully tuned, they can accurately track GPS signals in high dynamic situations. Taking pre-filter 1 for example, its equivalent bandwidth can p1, 1q, Qcarr p2, 2q be calculated from the elements of the gain matrix Kkcarr and is proportional to Qcarr k k carr carr carr carr and Qk p3, 3q [17]. Herein, Qk p1, 1q, Qk p2, 2q and Qk p3, 3q are the diagonal elements of Qcarr k . Qcarr p1, 1q and Qcarr p2, 2q (the epoch k is omitted to reveal these two values are constant) can be p3, 3q is related to the vehicle dynamics. If obtained from the receiver clock model [11], while Qcarr k carr Qk p3, 3q is estimated in real-time by any efficient adaptive algorithm, the loop bandwidth will vary in a time-varying optimal manner. This paper uses another more practical method: Qcarr p3, 3q is

Sensors 2016, 16, 627

6 of 22

elements of 𝑄𝑘𝑐𝑎𝑟𝑟 . 𝑄𝑐𝑎𝑟𝑟 (1,1) and 𝑄𝑐𝑎𝑟𝑟 (2,2) (the epoch 𝑘 is omitted to reveal these two values are constant) can be obtained from the receiver clock model [11], while 𝑄𝑘𝑐𝑎𝑟𝑟 (3,3) is related to the vehicle dynamics. If 𝑄𝑘𝑐𝑎𝑟𝑟 (3,3) is estimated in real-time by any efficient adaptive algorithm, Sensors 2016, 16, 627 6 ofthe 22 loop bandwidth will vary in a time-varying optimal manner. This paper uses another more practical method: 𝑄𝑐𝑎𝑟𝑟 (3,3) is set to a large constant value based on the general knowledge of set to a large constant value𝑐𝑎𝑟𝑟 based on the general knowledge of vehicle dynamics. Since Qcarr p1, 1q ̂ 𝑐𝑎𝑟𝑟 (1,1) and 𝑄𝑐𝑎𝑟𝑟 (2,2) are vehiclecarrdynamics. Since 𝑄 unchanged, the low variances of ∆θ 𝑘 and Q 𝑐𝑎𝑟𝑟p2, 2q are unchanged, the low variances of ∆θˆkcarr and fˆkcarr can be maintained [3]. In other and 𝑓̂𝑘 can be maintained [3].ˆ. In other words, although the variance of 𝑓̂̇ 𝑐𝑎𝑟𝑟 increases, phase words, although the variance of f carr increases, phase and frequency estimations which normally and frequency estimations which normally deserve concern can hardly be affected. Additionally, carr is set according 𝑐𝑎𝑟𝑟 𝑐𝑎𝑟𝑟 deserve concern can hardly be affected. Additionally, R to the knowledge of 𝑅 is set according to the knowledge of the phase noise variance, while 𝑃0 is usually larger carr carr 𝑐𝑎𝑟𝑟 the variance, P0 corrections. is usually larger to leave space for settles state corrections. thanphase 𝑄 noise to leave spacewhile for state In thisthan case,Qpre-filter 1 gradually down to a In this case, pre-filter 1 gradually settles down to a wide bandwidth, and hence is robust to high wide bandwidth, and hence is robust to high dynamics. Benefiting from the above mentioned dynamics. Benefiting from the above mentioned advantages, this baseband signal preprocessing advantages, this baseband signal preprocessing model can be appropriate for highly dynamic model canand be appropriate for highly dynamicand situations andperformance. make GPS have better tracking and situations make GPS have better tracking navigation navigation performance. 3. Realization of the Specific Ultra-tight GPS/INS Integration 3. Realization of the Specific Ultra-tight GPS/INS Integration The carrier tracking loop employing pre-filter 1 is in essence equivalent to a third-order loop. The carrier tracking loop employing pre-filter 1 is in essence equivalent to a third-order loop. However, a N-th order phase locked loop cannot correctly track the signal whose phase varies by However, a N-th order phase locked loop cannot correctly track the signal whose phase varies time to the 𝑁th or higher-order power (the estimates of signal parameters can still have low by time to the Nth or higher-order power (the estimates of signal parameters can still have low variance, but not low bias) [18]. That indicates, even improved by baseband signal processing, an variance, but not low bias) [18]. That indicates, even improved by baseband signal processing, an independent GPS receiver is vulnerable to extreme high dynamics, and may give inaccurate independent GPS receiver is vulnerable to extreme high dynamics, and may give inaccurate tracking tracking and navigation results. and navigation results. On the contrary, INS is robust to vehicle dynamics. Based on this fact, INS can be used to assist On the contrary, INS is robust to vehicle dynamics. Based on this fact, INS can be used to assist GPS, which usually means baseband signal processing is connected closely with the estimation of GPS, which usually means baseband signal processing is connected closely with the estimation of navigation parameters. In the ultra-tight GPS/INS integration shown in Figure 2, the integrated navigation parameters. In the ultra-tight GPS/INS integration shown in Figure 2, the integrated navigation filter is responsible for obtaining corrected inertial navigation information, and navigation filter is responsible for obtaining corrected inertial navigation information, and delivering delivering them back to GPS tracking channels. Because the feedback from the corrected inertial them back to GPS tracking channels. Because the feedback from the corrected inertial navigation navigation information has the advantage of high accuracy in a short time, GPS tracking channel information has the advantage of high accuracy in a short time, GPS tracking channel can perform better can perform better under highly dynamic conditions. The detailed work procedure of this under highly dynamic conditions. The detailed work procedure of this ultra-tight GPS/INS integration ultra-tight GPS/INS integration is as follows: in each GPS tracking channel, in-phase and is as follows: in each GPS tracking channel, in-phase and quadrature signals are processed by phase quadrature signals are processed by phase discriminators and pre-filters to estimate tracking discriminators and pre-filters to estimate tracking parameters, such as carrier frequency and code parameters, such as carrier frequency and code phase; the observations of the integrated navigation phase; the observations the integrated can be filter can be convertedoffrom tracking navigation parametersfilter as well as converted deduced from from tracking inertial parameters navigation as well as deduced from inertial navigation information and satellite ephemeris, and then the corrected information and satellite ephemeris, and then the corrected inertial navigation information can be inertial navigation can be obtained; finally, auxiliary tracking parameters, obtained; finally, information auxiliary tracking parameters, carrier frequency, for example, carrier can befrequency, deduced for example, can be deduced from these external navigation information and satellite ephemeris, and from these external navigation information and satellite ephemeris, and then be applied to assist then applied to assist signal tracking. Through the be above it can be seen that identifying signalbetracking. Through the above analysis, it can seenanalysis, that identifying conversion relations conversion relations between tracking parameters and navigation parameters as well as finding a between tracking parameters and navigation parameters as well as finding a rational way to assist rational to assist tracking are the keysultra-tight to designing a goodintegration. ultra-tight GPS/INS integration. trackingway are the keys to designing a good GPS/INS

Figure 2. The structure of the ultra-tight GPS/INS integration. Figure 2. The structure of the ultra-tight GPS/INS integration.

Sensors 2016, 16, 627

7 of 22

In the ideal situation, pseudorange rate and variation of pseudorange output by GPS tracking channel can be respectively derived from: .

ρG “ ´ f d

c

(27)

f L1

∆ρG “ ∆θ code

c f CA

(28)

where c is the speed of light. But in fact, under highly dynamic conditions, the carrier phase of satellite signal may be proportional to time to the power of N or higher value, and hence the estimated carrier frequency may contain a large error δ f carr . Since the incorrect GPS outputs could destroy the estimation of INS errors, it is necessary to analyze this error in detail. Therefore Equation (27) is rewritten as: ´ ¯ . ρG “ ´ fˆcarr ´ f IF “ ´ p f d ` δ f carr q

c f L1 c

(29)

f L1

On the other hand, Equation (28) can remain unchanged due to the fact that the code loop dynamic stress error is usually small. ρG , which represents the current pseudorange output by the GPS tracking channel, can be calculated from ∆ρG and the previous pseudorange. Then, in Earth Centered Earth Fixed (ECEF) coordinate system, the observation equation of integrated navigation filter can be expressed as: # δρ “ ρ I ´ ρG “ e1 δx ` e2 δy ` e3 δz ´ δtu ` vρ . . . . . . (30) δρ “ ρ I ´ ρG “ e1 δ x ` e2 δy ` e3 δz ´ δtru ` δvG ` vρ. .

where ρ I , ρ I respectively represent the pseudorange and pseudorange rate deduced from inertial . . . navigation information and satellite ephemeris, δx, δy, δz, δx, δy, δz are vehicle position errors and velocity errors in ECEF coordinate system, e1 , e2 , e3 are unit vectors in the x, y, and z direction, δtu , δtru represent clock bias and drift respectively, δvG is the error of pseudorange rate output by the GPS tracking channel and equals to δ f carr c{ f L1 , vρ , vρ. are observation noises. It is noted that Equation (30) should usually be transformed into the geodetic coordinate system to facilitate INS error correction. r can be Then, taking all the available tracking channels into consideration, the observation matrix H obtained naturally, where the superscript "„" denotes the navigation filter designed in this paper. If there are m satellites being locked by tracking channels, the state vector of the integrated navigation filter can be expressed as: ıT

” r“ X

X

δvG,1

δvG,2

¨¨¨

δvG,m

(31)

where X is the transpose of the state vector adopted by a traditional integrated navigation filter, namely ı ” X “ ϕ E ϕ N ϕU δv E δv N δvU δL δλ δh ε bx ε by ε bz ∇bx ∇by ∇bz δtu δtru

(32)

and the first 15 variables of X represent three INS error states each in attitude, velocity, position, gyro bias, accelerometer bias. Accordingly, the state transition matrix Φ and the noise matrix G of the traditional integrated navigation filter can be expanded to: « r “ Φ « r“ G

Φ17ˆ17 0mˆ17

017ˆm Imˆm

ff

G17ˆ8 0 m ˆ8

017ˆm Imˆm

ff

(33)

(34)

Sensors 2016, 16, 627

8 of 22

r can also be written as: In addition, H « r H “ H2mˆ17

«

0mˆm Imˆm

ff ff (35)

where H is the traditional observation matrix. The specific information of Φ, G and H can be easily found in many references [19–22] and will not be discussed in detail here. According to the conversion relations indicated by Equations (27) and (28), the corrected inertial navigation information can in turn feed back into GPS tracking channels. The corresponding carrier frequency estimate is usually accurate enough, and can be used to directly control local carrier generation. However, in the new baseband signal preprocessing model, it is hoped that the phase error can return to 0 in the shortest amount of time, so the external information assists in local carrier generation through correcting the state feedback value, and the input carrier frequency estimated output by pre-filter 1 can be expected to remain relatively stable and accurate. In other words, instead of being given by Equation (17), the local carrier frequency ucarr is calculated from: k ucarr “ k

∆θˆkcarr ` 2πTcoh

" *ˇ ´ ¯ ı ˇ 1 ˆ. f ” Tcoh ´ L1 e1 pvsx ´ v x q ` e2 vsy ´ vy ` e3 pvsz ´ vz q ` f IF ˇˇ ` f carr c 2 k k

(36)

where v x , vy , vz are the corrected vehicle velocity components in ECEF coordinate system, vsx , vsy , vsz are the satellite velocity components. Meanwhile, because vehicle dynamics have less effect on the code loop, generation of local code is still based upon the state feedback value ucode given by Equation (26), namely, the structure of the code loop is unchanged. 4. Results and Discussion This paper carried out static field tests and semi-physical simulations based on a telemetered missile trajectory to verify the validity of the proposed methods. In the static field test, the integration of GPS and INS is not involved, and only GPS is utilized to compare the performance of the baseband signal preprocessing models based on signal-filter and dual-filter structures. In the simulation, the navigation results of the ultra-tight GPS/INS integration are analyzed, and special attention is given to the further improvement of carrier frequency tracking accuracy. 4.1. Static Field Test Although the baseband signal preprocessing model and the ultra-tight GPS/INS integration ˝ can both restrain noise better, the field test is carried out in an open-sky area (N : 39 571 36.16” , ˝ E : 116 191 1.58” , Hgt : 32.9m) and does not involve the weak signal problem. After collecting signals with a radio frequency front-end, seven satellites are chosen according to their high Carrier-to-Noise Ratio pC{N0 q listed in Table 1. The sky plot of these satellites is shown in Figure 3, and the elevation of ˝ each satellite is always greater than 10 during the test. Table 1. C{N0 of selected satellites. Satellite

C{N0 (dB¨ Hz)

Satellite

C{N0 (dB¨Hz)

PRN22 PRN31 PRN25 PRN18

48.881 48.025 45.987 48.086

PRN14 PRN32 PRN12

45.612 45.693 45.202

Sensors 2016, 16, 627

9 of 22

Sensors Sensors2016, 2016,16, 16,627 627

9 9ofof2222

Figure 3. Satellites Figure 3. Satellites Satellites sky sky plot plot of of the thefield fieldtest. test. Figure 3. sky plot of the field test.

Since weak weak signals signals are are beyond beyond the Since the scope scope of of this this contribution, contribution,there thereisisno noneed needtotoemploy employINS INStoto Since weak signals are beyond the scope ofthis thispart contribution, there the is noperformance need to employ INS aid GPS tracking in static field tests. Hence just compares of aid GPS tracking in static field tests. Hence this part just compares the performance ofGPS GPS toreceivers aid GPSequipped tracking with in static field tests. Hence this part just compares the performance of GPS single pre-filter pre-filter (pre-filter-S) receivers equipped with single (pre-filter-S) and and double double pre-filters pre-filters(pre-filter-D). (pre-filter-D).First Firstofof receivers equipped with single pre-filter (pre-filter-S) and double pre-filters (pre-filter-D). First of all, all, in in the the tracking tracking domain, domain, the the reference reference Doppler all, Doppler shifts shifts can can be be provided provided by by cubic cubic spline splinefitfit ininterpolation. the tracking domain, the reference Doppler shifts can be provided by cubic spline fit interpolation. Then, taking taking PRN25 PRN25 for interpolation. Then, for example, example, Figure Figure 44 shows shows the the difference differencebetween betweenDoppler Doppler Then, taking PRN25 for example, Figure 4aforesaid shows the difference between Doppler shiftanalysis, tracking shift tracking errors obtained by these two methods. According to the statistical shift tracking errors obtained by these two aforesaid methods. According to the statistical analysis, errors obtained these(RMS) two aforesaid methods. According to theerrors statistical analysis, the Root Mean the Root Mean by Square value of the Doppler shift tracking corresponding to pre-filter-S the Root Mean Square (RMS) value of the Doppler shift tracking errors corresponding to pre-filter-S Square (RMS) value of the Doppler shift tracking errors corresponding to pre-filter-S is 0.487 Hz. For is 0.487 Hz. For pre-filter-D, this value is 0.198 Hz. Obviously, pre-filter-S has a bad Doppler shift is 0.487 Hz. For pre-filter-D, this value is 0.198 Hz. Obviously, pre-filter-S has a bad Doppler shift pre-filter-D, value is 0.198 has a error, bad Doppler estimation outcome estimation this outcome due to Hz. the Obviously, influence pre-filter-S of code phase while shift pre-filter-D achieves a estimation outcome due to the influence of code phase error, while pre-filter-D achieves a due to the influence of code phase error, while pre-filter-D achieves a significantly higher degree significantly higher degree of precision. The decoupling between carrier tracking and code trackingof significantly higher degree of precision. The decoupling between carrier tracking and code tracking precision. decoupling between carrier tracking code tracking proposes a 59.3 percentexample, reduction proposes The a 59.3 percent reduction in Doppler shiftand tracking error. Staying with the PRN25 proposes a 59.3 percent reduction in Doppler shift tracking error. Staying with the PRN25 example, inFigure Doppler shift tracking error. Staying with the the PRN25 example, Figureerrors 5 shows the difference between 5 shows the difference between code phase tracking obtained by these two Figure 5 shows the difference between the code phase tracking errors obtained by these two the code phase tracking that errors by these two in methods. It has is observed that the new model methods. It is observed theobtained new model proposed this paper slightly better performance. methods. It is observed that the new model proposed in this paper has slightly better performance. proposed in this slightly betterphase performance. the RMS values of code phase Specifically, the paper RMS has values of code tracking Specifically, errors corresponding to pre-filter-S and Specifically, the RMS values of code−2 phase tracking errors corresponding to pre-filter-S and ´ 2 chip and tracking errors to pre-filter-S and pre-filter-D are respectively 1.55 ˆ10 pre-filter-D arecorresponding respectively 1.55 × 10−2 chip and 1.43 × 10−2 chip. The decoupling reduces the pre-filter-D are respectively 1.55 × 10 chip and 1.43 × 10−2 chip. The decoupling reduces the ´ 2 1.43 ˆ10 The error decoupling reduces the code phase tracking error by 7.7 percent. code phasechip. tracking by 7.7 percent. code phase tracking error by 7.7 percent.

Figure 4. 4. Doppler Doppler shift shift tracking Figure tracking error error for for PRN25 PRN25 in in field field test. test. Figure 4. Doppler shift tracking error for PRN25 in field test.

Sensors 2016, 16, 627

10 of 22

Sensors 2016, 2016, 16, 16, 627 627 Sensors

10 of of 22 22 10

Figure 5. Code phase tracking error for forPRN25 PRN25inin infield fieldtest. test. Figure error for PRN25 field test. Figure5.5.Code Codephase phase tracking tracking error

To analyze analyze the impact impact of tracking tracking parameters on on position position and and velocity velocity detection, detection, receiver receiver To To analyzethe the impact of of tracking parameters parameters on position and velocity detection, receiver implements navigation through Least Squares (LS). The position estimation errors in the ECEF implements The position positionestimation estimationerrors errorsininthethe ECEF implementsnavigation navigationthrough through Least Least Squares Squares (LS). (LS). The ECEF coordinate system system are are shown shown in Figure Figure 6, 6, while while their their corresponding corresponding RMS RMS values are are listed listed in in coordinate coordinate system are shown in in Figure 6, while their corresponding RMS values arevalues listed in Table 2. It Table 2. It can be seen that pre-filter-D makes positioning more precise. Then, Figure 7 illustrates Table 2. It canthat be pre-filter-D seen that pre-filter-D makes positioning precise. Figurethe 7 velocity illustrates can be seen makes positioning more precise.more Then, Figure Then, 7 illustrates the velocity estimation errors in ECEF frame, and Table 3 lists their RMS values. Obviously, there theestimation velocity estimation errors in ECEF frame, and their TableRMS 3 lists their Obviously, RMS values. Obviously, there errors in ECEF frame, and Table 3 lists values. there are multiple areglitch multiple glitch impulses in the velocity velocity estimation result output by the thepre-filter-S, receiver employing employing are multiple glitch impulses the result output by receiver impulses in the velocity in estimation result estimation output by the receiver employing which is pre-filter-S, which is quite similar to the situation in Doppler shift tracking. Sometimes the velocity quite similar to the in Doppler shift tracking. Sometimes velocitySometimes errors in the and z pre-filter-S, which is situation quite similar to the situation in Doppler shiftthe tracking. they velocity errors in the 𝑦 and 𝑧 direction are even close to 0.3 m/s. On the contrary, the other receiver direction are even close to 0.3 m/s. On the contrary, the other receiver achieves comparatively stable errors in the 𝑦 and 𝑧 direction are even close to 0.3 m/s. On the contrary, the other receiver achieves comparatively stable and accurate accurate outcomes. The absolute maximum valuesthan of the the velocity and accurate outcomes.stable The absolute maximum values of theabsolute velocity maximum errors are smaller 0.1, 0.13 achieves comparatively and outcomes. The values of velocity errors are smaller than 0.1, 0.13 and 0.12 m/s, respectively. and 0.12 m/s, respectively. errors are smaller than 0.1, 0.13 and 0.12 m/s, respectively.

Figure 6. 6. Position estimation estimation errors errors with with field field data. data. Figure Figure 6.Position Position estimation errors with field data. Table 2. 2. RMS RMS of of position position estimation estimation errors. errors. Table

Method Method Pre-filter-S Pre-filter-S Pre-filter-D Pre-filter-D

RMS of of Position Position Error Error (m) (m) RMS 𝑿 𝒀 𝒁 𝑿 𝒀 𝒁 0.359 0.585 0.519 0.359 0.585 0.519 0.171 0.367 0.315 0.171 0.367 0.315

Sensors 2016, 16, 627

11 of 22

Table 2. RMS of position estimation errors. RMS of Position Error (m)

Method Pre-filter-S Pre-filter-D

X

Y

Z

0.359 0.171

0.585 0.367

0.519 0.315

Sensors 2016, 16, 627

11 of 22

Figure 7. Velocity estimation errors errors with with field field data. data. Figure 7. Velocity estimation Table 3. RMS of velocity estimation errors. Table 3. RMS of velocity estimation errors. Method Method Pre-filter-S Pre-filter-D Pre-filter-S Pre-filter-D

RMS of Velocity Error (m/s) Error (m/s) 𝑿 RMS of Velocity 𝒀 𝒁 X 0.044 0.026 0.044 0.026

Y 0.085 0.036 0.085 0.036

0.072Z 0.036 0.072 0.036

It can be drawn from the analysis above that, using the new preprocessing model based on dual-filter makes GPS receiver behave above betterthat, in the static Furthermore, is worth It can be drawn from the analysis using thefield new tests. preprocessing modelit based on mentioning that the frequency difference between input and local carrier is shown in Figure 8. dual-filter makes GPS receiver behave better in the static field tests. Furthermore, it is worth mentioning Although the GPS tracking channel generates a local with a high in fluctuation the input that the frequency difference between input and localcarrier carrier is shown Figure 8. from Although the 𝑐𝑎𝑟𝑟 carrier, it can be interpreted as resulting from the design principle that ∆θ must return in 𝑘+1 GPS tracking channel generates a local carrier with a high fluctuation from the input carrier, it to can0 be the shortest amount of time. The analysis of Figure 4 indicates the precision of Doppler shift interpreted as resulting from the design principle that ∆θcarr k`1 must return to 0 in the shortest amount tracking is not reduced, that 4is, pre-filter is insensitive to theshift wide fluctuation of local carrier of time. The analysis of Figure indicates the1precision of Doppler tracking is not reduced, that is, frequency. pre-filter 1 is insensitive to the wide fluctuation of local carrier frequency.

mentioning that the frequency difference between input and local carrier is shown in Figure 8. Although the GPS tracking channel generates a local carrier with a high fluctuation from the input carrier, it can be interpreted as resulting from the design principle that ∆θ𝑐𝑎𝑟𝑟 𝑘+1 must return to 0 in the shortest amount of time. The analysis of Figure 4 indicates the precision of Doppler shift tracking Sensors 2016,is16,not 627reduced, that is, pre-filter 1 is insensitive to the wide fluctuation of local carrier 12 of 22 frequency.

Figure 8. Frequency difference between local and input carriers in field tests. Figure 8. Frequency difference between local and input carriers in field tests.

Sensors 2016, 16, 627

12 of 22

4.2. 4.2. Semi-Physical Semi-Physical Simulation Simulation Based Based on on Telemetered Telemetered Missile Missile Trajectory Trajectory Highly Highly dynamic dynamic conditions, conditions, which which are are needed needed for for validating validating the the effectiveness effectiveness of of the the ultra-tight ultra-tight GPS/INS integration, are extremely difficult to achieve in practice. To simulate this condition, in GPS/INS integration, are extremely difficult to achieve in practice. To simulate this condition, in this this paper, a missile trajectory lasting 180.23 s is derived from Fiber Optic Gyroscope (FOG) and paper, a missile trajectory lasting 180.23 s is derived from Fiber Optic Gyroscope (FOG) and accelerometer areare telemetered during a missile test, and the reference accelerometerdata, data,which which telemetered during a missile test,is treated and is as treated as the trajectory. reference The position of the missile relative to the launch site in geodetic coordinate system, and the velocity of trajectory. The position of the missile relative to the launch site in geodetic coordinate system, and the coordinate system are shown in Figure 9. Figure shows (strapdown) the missile velocityinofECEF the missile in ECEF coordinate system are shown in 10 Figure 9. the Figure 10 showsINS the used in the missile test. The statistical characteristics of the FOG and accelerometer are listed in Table 4. (strapdown) INS used in the missile test. The statistical characteristics of the FOG and If the same errors in turn are added to the original FOG and accelerometer data, a set of new data can accelerometer are listed in Table 4. If the same errors in turn are added to the original FOG and be generated. The trajectory new inertial measurements can be used the new INS accelerometer data, a set ofcorresponding new data cantobethese generated. The trajectory corresponding to as these navigation result, which needs to be calibrated by the ultra-tight GPS/INS integration. In addition, inertial measurements can be used as the INS navigation result, which needs to be calibrated by the based on the reference trajectory as as thebased satellite at the missile launch sitesatellite shown ultra-tight GPS/INS integration. In well addition, on configuration the reference trajectory as well as the in Figure 11, the can be simulated. configuration at IF thesignal missile launch site shown in Figure 11, the IF signal can be simulated.

Figure 9. The reference trajectory for simulation. Figure 9. The reference trajectory for simulation.

Sensors 2016, 16, 627

13 of 22

Figure 9. The reference trajectory for simulation.

Sensors 2016, 16, 627

Figure 10. The (strapdown) INS used in the missile test. Figure 10. The (strapdown) INS used in the missile test.

13 of 22

Table 4. Gyro and accelerometer specifications.

Table 4. Gyro and accelerometer specifications. Gyros Accelerometers In-Run Bias Error Noise (1σ, In-Run smoothing window: Bias Error 10 ms)

Noise (1σ, smoothing window: 10 ms)

0.5˝ {hGyros 15˝ {h0.5°/h

15°/h

2Accelerometers ˆ10´4 g 2 ˆ10 g −4 g 2 ´2 × 10

2 × 10−2 g

Figure Figure 11. 11. Satellites Satellites sky sky plot plot of of the the simulation. simulation.

Figure 12 provides a close look at the absolute acceleration of the missile following the Figure 12 provides a close look at the absolute acceleration of the missile following the reference reference trajectory. It is observed that this value does not remain unchanged during the missile trajectory. It is observed that this value does not remain unchanged during the missile ascent phase. ascent phase. Indeed, this value has abrupt changes at about 4 s and 9 s. That usually means that Indeed, this value has abrupt changes at about 4 s and 9 s. That usually means that even baseband even baseband signal preprocessing cannot ensure the carrier frequency estimation accuracy. signal preprocessing cannot ensure the carrier frequency estimation accuracy. Taking PRN5 as example, Taking PRN5 as example, the Doppler shift tracking error obtained from the independent GPS the Doppler shift tracking error obtained from the independent GPS receiver equipped with pre-filter-D receiver equipped with pre-filter-D is shown in Figure 13. Obviously, the rapid variation of is shown in Figure 13. Obviously, the rapid variation of acceleration introduces a significant error acceleration introduces a significant error into the estimated Doppler shift after 4 s, and the sudden into the estimated Doppler shift after 4 s, and the sudden reversal of jerk at 9 s causes the sign of this reversal of jerk at 9 s causes the sign of this error to change. As a comparison, the same dynamic error to change. As a comparison, the same dynamic simulation is also performed with the ultra-tight simulation is also performed with the ultra-tight GPS/INS integration. The local carrier generation GPS/INS integration. The local carrier generation can be directly controlled by the frequency derived can be directly controlled by the frequency derived from the external navigation information from the external navigation information (GPS/INS-f), but even so, the carrier tracking loop is still (GPS/INS-f), but even so, the carrier tracking loop is still equivalent to a third-order phase locked equivalent to a third-order phase locked loop. Hence, in the integrated system, the Doppler shift loop. Hence, in the integrated system, the Doppler shift tracking error follows the same trend as the tracking error follows the same trend as the absolute acceleration. However, the absolute maximum absolute acceleration. However, the absolute maximum value of this error drops to 5 Hz from 15 Hz, and the whole tracking process goes more smoothly. According to Equation (36), the external navigation information can also be used to correct the state feedback value. In this case, local carrier generation is controlled by state feedback (GPS/INS-u), and the absolute maximum value of the Doppler shift estimation error is 3 Hz. The following discussions focus on GPS/INS-u. It can be seen

Sensors 2016, 16, 627

14 of 22

value of this error drops to 5 Hz from 15 Hz, and the whole tracking process goes more smoothly. According to Equation (36), the external navigation information can also be used to correct the state feedback value. In this case, local carrier generation is controlled by state feedback (GPS/INS-u), and the absolute maximum value of the Doppler shift estimation error is 3 Hz. The following discussions focus on GPS/INS-u. It can be seen from Equation (29) that δvG is proportional to the estimation error of input carrier frequency, so Figure 13 also shows that the estimation of δvG,1 (PRN5 is locked by channel #1) is accurate. In this sense, the proposed navigation filter can ensure INS is properly calibrated. If the traditional 17-dimensional filter is applied, in fact, the significant Doppler shift tracking error caused by missile ascent will lead the navigation results to gradually deviate from the reference trajectory until the whole integrated system cannot work. Sensors 2016, 16, 627 14 of 22 Sensors 2016, 16, 627

14 of 22

Figure 12. The absolute acceleration of the missile.

Figure 12. The absolute acceleration of the missile. Figure 12. The absolute acceleration of the missile.

Figure 13. Doppler shift tracking error for PRN5 and the estimation of 𝛿𝑣𝐺,1 . Figure 13. Doppler shift tracking error for PRN5 and the estimation of 𝛿𝑣𝐺,1 .

Since there13. is no loss of shift lock in dynamic simulation, theand independent GPS receiver equipped Figure Doppler tracking error for PRN5 the estimation of δvG,1 . with Since the new preprocessing implement navigation. However, inGPS navigation if there is no loss of model lock incan dynamic simulation, the independent receiverdomain, equipped KF isthe applied, the large error contained in the estimated Doppler shift in may lead to domain, inaccurate with new preprocessing model can implement navigation. However, navigation if Since positioning there is noresults. loss of lockthe in independent dynamic simulation, the independent GPS receiverresults. equipped with Henceerror still uses LSshift to obtain navigation KF is applied, the large contained inGPS the receiver estimated Doppler may lead to inaccurate the new preprocessing model can implement navigation. However, in navigation domain, if KF is Figures 14 and 15 respectively show its position velocity ECEF coordinate positioning results. Hence the independent GPS and receiver still estimation uses LS to errors obtaininnavigation results. system. And Figures 16 and 17 show the INS navigation errors in the same coordinate system. applied, the large14error theits estimated Doppler shift mayerrors leadintoECEF inaccurate positioning Figures and 15contained respectivelyin show position and velocity estimation coordinate Meanwhile, navigation of ultra-tight GPS/INS the coordinate difference between system. And Figures 16errors andGPS 17 the show the INS navigation innamely the navigation same system.Figures 14 results. Hence the independent receiver still usesintegration, LSerrors to obtain results. the correctednavigation inertial navigation information reference values are shownthe in difference Figures 18between and 19. Meanwhile, of the ultra-tightand GPS/INS integration, namely and 15 respectively show itserrors position and velocity estimation errors in ECEF coordinate system. Obviously, theinertial velocity components estimated byreference the independent GPS receiver have large errors, the corrected navigation information and values are shown in Figures 18 and 19. And Figures 16are and 17 show the INSduring navigation inphase. the same system. Meanwhile, which on the order of 3 m/s, the missile ascent This iscoordinate consistent with the errors, carrier Obviously, the velocity components estimated byerrors the independent GPS receiver have large tracking result. For the case of INS, there is no sudden change in navigation errors. That is because which are on the order of 3 m/s, during the missile ascent phase. This is consistent with the carrier INS is robust vehicle dynamics. has the problem of error accumulation. 180 s, tracking result.toFor the case of INS, However, there is noINS sudden change in navigation errors. That isAt because the position are 76.3, −21.1 and −60.4 m, INS while the velocity errors are 0.6, 0.2 and −0.5At m/s. INS is robusterrors to vehicle dynamics. However, has the problem of error accumulation. 180On s, the position errors are 76.3, −21.1 and −60.4 m, while the velocity errors are 0.6, 0.2 and −0.5 m/s. On

Sensors 2016, 16, 627

15 of 22

navigation errors of the ultra-tight GPS/INS integration, namely the difference between the corrected inertial navigation information and reference values are shown in Figures 18 and 19. Obviously, the velocity components estimated by the independent GPS receiver have large errors, which are on the order of 3 m/s, during the missile ascent phase. This is consistent with the carrier tracking result. For the case of INS, there is no sudden change in navigation errors. That is because INS is robust to vehicle dynamics. However, INS has the problem of error accumulation. At 180 s, the position errors are 76.3, Sensors 2016, 16, 627 15 of 22 ´21.1 and ´60.4 m, while the velocity errors are 0.6, 0.2 and ´0.5 m/s. On the other hand, employing Sensors 2016, 16, 627 15 of 22 the navigation filter with expanded dimension, the ultra-tight GPS/INS integration can provide more the other hand, employing the navigation filter with expanded dimension, the ultra-tight GPS/INS accurate navigation results. The absolute maximum values of absolute position errors velocity errors are the other hand, employing theaccurate navigation filter with expanded dimension, theand ultra-tight integration can provide more navigation results. The maximum values ofGPS/INS position smaller than 0.5 m and 0.1 m/s, respectively. integration can provide more accuratethan navigation results. Therespectively. absolute maximum values of position errors and velocity errors are smaller 0.5 m and 0.1 m/s, errors and velocity errors are smaller than 0.5 m and 0.1 m/s, respectively.

Figure14. 14.GPS GPSposition position estimation estimation errors Figure errors with withsimulation simulationdata. data. Figure 14. GPS position estimation errors with simulation data.

Figure 15. GPS velocity estimation errors with simulation data. Figure 15. GPS velocity estimation errors with simulation data. Figure 15. GPS velocity estimation errors with simulation data.

Sensors 2016, 16, 627

16 of 22

Sensors 2016, 16, 627 Sensors 2016, 16, 627

16 of 22 16 of 22

Figure estimationerrors. errors. Figure 16. 16. INS INS position position estimation Figure 16. INS position estimation errors.

Figure 17. INS velocity estimation errors. Figure estimationerrors. errors. Figure 17. 17. INS INS velocity velocity estimation

Sensors 2016, 16, 627

17 of 22

Sensors Sensors 2016, 2016, 16, 16, 627 627

17 17 of of 22 22

Figure 18. GPS/INS Figure18. 18. GPS/INS GPS/INS position position estimation estimation errors. errors. Figure

Figure 19. GPS/INS Figure Figure19. 19. GPS/INS GPS/INS velocity velocity estimation estimation errors. errors.

As mentioned before, the traditional navigation filter cannot maintain the estimation accuracy As mentioned mentioned before, before, the the traditional traditional navigation navigation filter filter cannot cannot maintain maintain the the estimation estimation accuracy accuracy As of INS errors when the GPS receiver operates under harsh conditions. Once the incorrect navigation of INS INS errors errors when when the the GPS GPS receiver receiver operates operates under under harsh harsh conditions. conditions. Once Once the the incorrect incorrect navigation navigation of information is utilized to generate local carrier frequency, the Doppler shift tracking error will information is utilized to generate local carrier frequency, the Doppler shift tracking error will information is utilized to generate local carrier frequency, the Doppler shift tracking error will further further increase, so it can be concluded that the proposed navigation filter elegantly avoids this further increase, so it can be concluded that the proposed navigation filter elegantly avoids this increase, so it can be concluded that the proposed navigation filter elegantly avoids this vicious circle. vicious circle. Here, a tight GPS/INS integration based on the traditional navigation filter is realized vicious circle.GPS/INS Here, a tight GPS/INSbased integration on the traditionalfilter navigation filter realized Here, a tight integration on the based traditional navigation is realized toisshow the to show the harmful effect of carrier tracking errors in detail. The corresponding navigation errors to show the harmful effect of carrier tracking errors in detail. The corresponding navigation errors harmful effect of carrier tracking errors in detail. The corresponding navigation errors are shown are shown in Figures 20 and 21. It observed that the gradually achieves areFigures shown 20 in and Figures andbe 21.observed It can can be bethat observed that the tight tight integration integration gradually achieves in 21. 20 It can the tight integration gradually achieves acceptable acceptable navigation accuracy as time goes on, but during the missile ascent phase, the velocity acceptable navigation accuracy as time goes on, but during the missile ascent phase, the navigation accuracy as time goes on, but during the missile ascent phase, the velocity errorsvelocity of this errors of this integration are on the order of 2 m/s, and are much larger than those of INS. errors of this areofon2 the order m/s, and arethan much larger integration areintegration on the order m/s, and of are2 much larger those of than INS. those of INS.

Sensors 2016, 16, 627

18 of 22

Sensors Sensors 2016, 2016, 16, 16, 627 627

18 18 of of 22 22

Figure (tight integration, integration, traditional traditional navigation navigation filter) filter) position Figure 20. GPS/INS Figure20. 20. GPS/INS GPS/INS (tight (tight integration, traditional navigation filter) position estimation estimation errors. errors.

Figure Figure 21. 21. GPS/INS GPS/INS (tight (tight integration, integration, traditional traditional navigation navigation filter) filter) velocity velocity estimation estimation errors. errors. Figure 21. GPS/INS (tight integration, traditional navigation filter) velocity estimation errors.

5. 5. Conclusions Conclusions 5. Conclusions This This paper paper proposes proposes aa baseband baseband signal signal preprocessing preprocessing model, model, which which has has two two pre-filters pre-filters and and This paper proposes a baseband signal preprocessing model, which has two pre-filters and uses state feedback values to control the generation of local carrier and code. Compared with uses state feedback values to control the generation of local carrier and code. Compared with the the uses state feedback values to control the generation ofthis local carrier and code. Compared with conventional conventional approach approach based based on on aa single-filter single-filter structure, structure, this model model limits limits the the impact impact of of code code phase phase the conventional approach based on a single-filter structure, this model limits the impact of code error error on on carrier carrier tracking, tracking, and and hence hence enables enables the the receiver receiver to to achieve achieve better better navigation navigation precision. precision. phase error on carrier tracking, and hence enables the receiver to achieve better navigation precision. Furthermore, this model can ensure the stationarity of the estimation of input carrier frequency, Furthermore, this model can ensure the stationarity of the estimation of input carrier frequency, Furthermore, this model can ensure the stationarity of the estimation of input carrier frequency, even in even even in in the the case case of of local local carrier carrier frequencies frequencies with with wide wide fluctuation. fluctuation. In In fact, fact, compared compared with with aa popular popular the case of local carrier frequencies with wide fluctuation. In fact, compared with athe popular error-state error-state error-state pre-filter, pre-filter, even even pre-filter-S pre-filter-S proposed proposed in in this this paper paper performs performs better better in in the tracking tracking domain domain pre-filter, even pre-filter-S proposed in this paper performs better in the tracking domain due to the due to the benefits of state feedback. due to the benefits of state feedback. benefits of state feedback. To To further further promote promote the the tracking tracking and and navigation navigation performance, performance, aa specific specific ultra-tight ultra-tight GPS/INS GPS/INS To further promote the tracking and navigation performance, a specific ultra-tight GPS/INS integration is designed on the basis of the new preprocessing model. Since the pseudorange integration is designed on the basis of the new preprocessing model. Since the pseudorange rate rate integration is designed on the basis of the new preprocessing model. Sincevariable, the pseudorange rate estimation estimation error error in in each each channel channel has has been been treated treated as as the the additional additional state state variable, the the proposed proposed estimation error ensures in each channel has been INS treated as will the additional state variable, the proposed navigation navigation filter filter ensures the the estimation estimation of of INS errors errors will not not be be contaminated. contaminated. Moreover, Moreover, in in this this integration, although the corrected inertial navigation information is used to estimate the carrier integration, although the corrected inertial navigation information is used to estimate the carrier frequency, frequency, the the tracking tracking channel channel still still aims aims to to make make the the phase phase error error return return to to 00 in in the the shortest shortest amount amount

Sensors 2016, 16, 627

19 of 22

navigation filter ensures the estimation of INS errors will not be contaminated. Moreover, in this integration, although the corrected inertial navigation information is used to estimate the carrier frequency, the tracking channel still aims to make the phase error return to 0 in the shortest amount of time. The results of the semi-physical simulation based on a telemetered missile trajectory indicate that navigation solutions with higher accuracy can be achieved. Meanwhile, because tracking is assisted by external navigation information, the error contained in the estimated Doppler shift can be reduced substantially. Acknowledgments: The authors wish to acknowledge the financial support of the National Natural Science Foundation of China (Grant No. 61153002). Author Contributions: Xi Zhang provided insights in formulating the ideas and wrote the paper. He also performed the main part of the experiments. Lingjuan Miao offered the experimental data telemetered in a missile test, and critically reviewed the paper. Haijun Shao contributed greatly to experimental data evaluation. All authors read, revised and approved the final manuscript. Conflicts of Interest: The authors declare no conflict of interest.

Appendix A First, the items in Equation (1) are numbered as follows: .

carr carr 2 carr ∆θkcarr “ ∆θkcarr ´1 ` 2π f k´1 Tcoh ` π f k´1 Tcoh ´ 2πuk´1 Tcoh .

carr f kcarr “ f kcarr ´1 ` f k´1 Tcoh .

(A1) (A2)

.

f carr “ f carr k k ´1

(A3)

Obviously, Equation (A3) can be rewritten as: .

.

carr f carr k ´1 “ f k

(A4)

Substitution of Equation (A4) into Equation (A2) results in: .

carr f kcarr “ f kcarr ´1 ` f k Tcoh

then:

.

carr f kcarr ´ f carr ´1 “ f k k Tcoh

(A5)

and substitution of Equations (A4) and (A5) into Equation (A1) results in: ∆θkcarr

´ ¯ . . carr ´ f carr T carr 2 carr “ ∆θkcarr ` 2π f coh Tcoh ` π f k Tcoh ´ 2πuk´1 Tcoh ´1 k k .

carr carr 2 carr “ ∆θkcarr ´1 ` 2π f k Tcoh ´ π f k Tcoh ´ 2πuk´1 Tcoh

then:

.

carr 2 carr ∆θkcarr ´ 2π f kcarr Tcoh ` π f carr ´1 “ ∆θk k Tcoh ` 2πuk´1 Tcoh

(A6)

Finally, Equation (3) can be obtained by combining Equations (A4)–(A6) together. Appendix B ıT ” . The filter, whose state vector is ∆θcarr ∆ f kcarr ∆ f carr ∆θcode , is a popular baseband signal k k k .

.

carr “ f carr ´ 0. To focus on analyzing preprocessing model. Herein, ∆ f kcarr “ f kcarr ´ ucarr k , ∆fk k

Sensors 2016, 16, 627

20 of 22

the benefits of decoupling between carrier tracking and code tracking, this filer should be further improved. First, consider the equations derived from Equation (18): f code “ β f carr ` β p f L1 ´ f IF q .

f

code

.

“ βf

(B1)

carr

(B1) .

f

carr carr and ∆θcode have the following where β “ fCA . The new state variables ∆θcarr k , fk , f k k L1 recurrence relations: $ . carr T carr T 2 ´ 2πucarr T ’ ∆θkcarr “ ∆θkcarr ` 2π f ` π f coh ’ ´1 k ´1 k´1 coh k´1 coh ’ ’ . ’ ’ & f carr “ f carr ` f carr Tcoh k k´1 k ´1 (B3) . . ’ carr carr ’ f “ f ’ k k ´1 ’ ’ ’ . % 1 code carr T carr T 2 ´ ucode T ∆θk “ ∆θkcode ` β f ` β f coh 2 ´1 k ´1 k´1 coh k´1 coh ` β p f L1 ´ f IF q Tcoh

The relation between the observation ∆θcarr k,k´1 and state variables can still be represented by Equation (4), while ∆θcode is written as: k,k´1 1 . 1 1 1 2 code code Tcoh ` ucode Tcoh ´ β p f L1 ´ f IF q Tcoh ∆θk,k ´ β f kcarr Tcoh ` β f carr k k ´ 1 ´1 “ ∆θk 2 6 2 2

(B4)

Then, the state equation and observation equation of pre-filter-S can be expressed as: #

caco caco caco Xkcaco “ Φcaco k,k´1 Xk´1 ` Mk´1 ` Wk´1

(B5)

Zkcaco “ Hkcaco Xkcaco ` Nkcaco ` Vkcaco where:

» Φcaco k,k´1

Mkcaco ´1 “

”

«

Nkcaco “

”

2πTcoh

2 πTcoh

1

Tcoh

0

1

βTcoh

1 2 2 βTcoh

— — 0 “— — 0 – 0

´2πTcoh ucarr k ´1 Hkcaco

1

“

πTcoh ucarr k ´1

0

0

0

fi

ffi 0 ffi ffi 0 ffi fl 1

β p f L1 ´ f IF q Tcoh ´ Tcoh ucode k ´1

1

´πTcoh

0

´ 21 βTcoh

1 2 3 πTcoh 1 2 6 βTcoh

0

« “

carr ∆θk,k ´1

ıT

(B7)

ff (B8)

1

´ 21 β p f L1 ´ f IF q Tcoh ` 12 Tcoh ucode k ´1

Zkcaco

(B6)

ıT

(B9)

ff

code ∆θk,k ´1

(B10)

while the feedback for carrier tracking is still represented by Equation (17), the feedback for code tracking is expressed as: ucode “ k

1 1 ˆ. ∆θˆkcode ` β fˆkcarr ` β f carr Tcoh ` β p f L1 ´ f IF q Tcoh 2 k

(B11)

Sensors 2016, 16, 627

21 of 22

carr Furthermore, it is noted that ∆θcode k,k´1 is uncorrelated with ∆θk,k´1 [15]. In fact, based on statistical code Sensors 2016, 16, 627 21 of 22 analysis, it can be drawn that the correlation coefficient between ∆θcarr k,k´1 and ∆θk,k´1 is usually smaller caco than 0.01. The measurement covariance matrix Rk , therefore, is set as: caco caco k

R

Rk

« 2  σ12 “  01  0

0 ff  σ222  2 0

(B12) (B12)

where σ12 and σ22 are the variances at the outputs of the carrier and code discriminators where σ21 and σ22 are the variances at the outputs of the carrier and code discriminators respectively. In respectively. In general, σ12 and σ22 can be calculated as follows [10]: general, σ21 and σ22 can be calculated as follows [10]:

 1 1 ˆ ˙ 1 1 1  σ1 “ 2 C N T 1 ` 2C N T  2C{N00T coh  2C{N0 0T coh 

 122 

coh

σ222 “ 2

„ 11  `

dd 44C{N C N00TTcoh coh 

(B13) (B13)

coh

 22  C N0 T p22 ´ ddqC{N 0T coh  coh

(B14) (B14)

where d is the early-late correlator spacing. where 𝑑 is the early-late correlator spacing. Pre-filter-S usually has better performance than the popular error-state filter (pre-filter-E). Taking Pre-filter-S usually has better performance than the popular error-state filter (pre-filter-E). PRN25 for example, Figure A1 shows the differences between tracking errors obtained by pre-filter-E Taking PRN25 for example, Figure B1 shows the differences between tracking errors obtained by and pre-filter-S. According to statistic analysis, the RMS values of Doppler shift and code phase pre-filter-E and pre-filter-S. According to statistic analysis, the RMS values of Doppler shift and tracking errors corresponding to pre-filter-E are respectively 1.028 Hz and 1.62 ˆ10´ 2 chip. For code phase tracking errors corresponding to pre-filter-E are respectively 1.028 Hz and 1.62 × 10−2 pre-filter-S, these two values are 0.487 Hz and 1.55 ˆ10´2 chip. −2 chip. For pre-filter-S, these two values are 0.487 Hz and 1.55 × 10 chip.

Figure A1. B1. Tracking errors of pre-filter-E pre-filter-E and and pre-filter-S. pre-filter-S.

References 1. Gao, Y.B.; Liu, S.F.; Atia, M.M.; Noureldin, A. INS/GPS/LiDAR Integrated Navigation System for Urban References 1. 2.

3. 2. 4.

5.

and Indoor Environments Using Hybrid Scan Matching Algorithm. Sensors 2015, 15, 23286–23302. Gao, Y.B.; Liu, S.F.; Atia, M.M.; Noureldin, A. INS/GPS/LiDAR Integrated Navigation System for Urban and Jwo D.J.; Yang C.F.; Chuang C.H.; Lin K.C. A Novel Design for the Ultra-Tightly Coupled GPS/INS Indoor Environments Using Hybrid Scan Matching Algorithm. Sensors 2015, 15, 23286–23302. [CrossRef] Navigation System. J. Navig. 2012, 65, 717–747. [PubMed] Tang, X.H.; Falco, G.; Falletti, E.; Presti, L.L. Theoretical analysis and tuning criteria of the Kalman Jwo, D.J.; Yang, C.F.; Chuang, C.H.; Lin, K.C. A Novel Design for the Ultra-Tightly Coupled GPS/INS filter-based tracking loop. GPS Solut. 2014, 19, 489–503. Navigation System. J. Navig. 2012, 65, 717–747. [CrossRef] Buck, T.M.; Wilmot, J.; Cook, M.J. A High G, MEMS Based, Deeply Integrated, INS/GPS, Guidance, Navigation and Control Flight Management Unit. In Proceedings of the 2006 IEEE/ION Position Location and Navigation Symposium, Coronado, CA, USA, 25–27 April 2006; pp. 772–794. Landis, D.; Thorvaldsen, T.; Fink, B.; Sherman, P; Holmes, S. A Deep Integration Estimator for Urban Ground Navigation. In Proceedings of the 2006 IEEE/ION Position Location and Navigation Symposium, Coronado, CA, USA, 25–27 April 2006; pp. 927–932.

Sensors 2016, 16, 627

3. 4.

5.

6.

7.

8.

9. 10. 11.

12. 13. 14.

15. 16. 17. 18. 19. 20.

21. 22.

22 of 22

Tang, X.H.; Falco, G.; Falletti, E.; Presti, L.L. Theoretical analysis and tuning criteria of the Kalman filter-based tracking loop. GPS Solut. 2014, 19, 489–503. [CrossRef] Buck, T.M.; Wilmot, J.; Cook, M.J. A High G, MEMS Based, Deeply Integrated, INS/GPS, Guidance, Navigation and Control Flight Management Unit. In Proceedings of the 2006 IEEE/ION Position Location and Navigation Symposium, Coronado, CA, USA, 25–27 April 2006; pp. 772–794. Landis, D.; Thorvaldsen, T.; Fink, B.; Sherman, P.; Holmes, S. A Deep Integration Estimator for Urban Ground Navigation. In Proceedings of the 2006 IEEE/ION Position Location and Navigation Symposium, Coronado, CA, USA, 25–27 April 2006; pp. 927–932. Ohlmeyer, E.J. Analysis of an Ultra-Tightly Coupled GPS/INS System in Jamming. In Proceedings of the 2006 IEEE/ION Position Location and Navigation Symposium, Coronado, CA, USA, 25–27 April 2006; pp. 44–53. Dyrad, L.; Woessner, B.; Jovancevic, A.; Ganguly, S. Ultra Tightly Coupled GPS/INS Receiver for TSPI Applications. In Proceedings of the 20th International Technical Meeting of the Satellite Division of the Institute of Navigation, Fort Worth, TX, USA, 25–28 September 2007; pp. 2563–2572. Lashley, M.; Bevly, D.M.; Hung, J.Y. Analysis of Deeply Integrated and Tightly Coupled Architectures. In Proceedings of the 2010 IEEE/ION Position, Location and Navigation Symposium, Indian Wells, CA, USA, 3–6 May 2010; pp. 382–396. Patapoutian, A. On Phase-Locked Loops and Kalman Filters. IEEE Trans. Commun. 1999, 47, 670–672. [CrossRef] O'Driscoll, C.; Petovello, M.G.; Lachapelle, G. Choosing the Coherent Integration Time for Kalman Filter-Based Carrier-Phase Tracking of GNSS Signals. GPS Solut. 2011, 15, 345–356. [CrossRef] Petovello, M.G.; Lachapelle, G. Comparison of Vector-Based Software Receiver Implementations with Application to Ultra-Tight GPS/INS Integration. In Proceedings of the 19th International Technical Meeting of the Satellite Division of the Institute-of-Navigation, Fort Worth, TX, USA, 26–29 September 2006; pp. 1790–1799. Crane, R.N. A Simplified Method for Deep Coupling of GPS and Inertial Data. In Proceedings of the 2007 Institute of Navigation, National Technical Meeting, San Diego, CA, USA, 22–24 January 2007; pp. 311–319. Luo, Y.; Babu, R.; Wu, W.Q. Double-Filter Model with Modified Kalman Filter for Baseband Signal Pre-processing with Application to Ultra-Tight GPS/INS Integration. GPS Solut. 2012, 16, 463–476. [CrossRef] Chiang, K.W.; Duong, T.T.; Liao, J.K. The Performance Analysis of a Real-Time Integrated INS/GPS Vehicle Navigation System with Abnormal GPS Measurement Elimination. Sensors 2013, 13, 10599–10622. [CrossRef] [PubMed] O’Driscoll, C.; Lachapelle, G. Comparison of Traditional and Kalman Filter Based Tracking Architectures. In Proceedings of the 13th European Navigation Conference, Naples, Italy, 3–6 May 2009. Miao, J.F. Research on Key Technologies for Robust and Adaptive GPS Software Receiver. Ph.D. Thesis, Nanjing University of Aeronautics and Astronautics, Nanjing, China, December 2009. Won, J.H.; Pany, T.; Eissfeller, B. Characteristics of Kalman filters for GNSS signal tracking loop. IEEE Trans. Aerosp. Electron. Syst. 2012, 48, 3671–3681. [CrossRef] Xie, G. Principles of GPS and Receiver Design; Publishing House of Electronics Industry: Beijing, China; pp. 276–277. Titterton, D.; Weston, J. Strapdown Inertial Navigation Technology; The Institution of Engineering and Technology: Reston, VA, USA, 2004; pp. 377–418. Gao, G.J.; Lachapelle, G. INS-Assited High Sensitivity GPS Receivers for Degraded Signal Navigation. In Proceedings of the 19th International Technical Meeting of the Satellite Division of The Institute of Navigation, Fort Worth, TX, USA, 26–29 September 2006. Kim, K.H.; Lee, J.G.; Park, C.G. Adaptive Two-Stage Extended Kalman Filter for a Fault-Tolerant INS-GPS Loosely Coupled System. IEEE Trans. Aerosp. Electron. Syst. 2009, 45, 125–137. Hong, S.P.; Lee, M.H.; Chun, H.H. Observability of error states in GPS/INS integration. IEEE Trans. Veh. Technol. 2005, 54, 731–743. [CrossRef] © 2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).