Development of a Deeply-Coupled GPS/INS Integration Algorithm · PDF file 2014. 4....

Click here to load reader

  • date post

  • Category


  • view

  • download


Embed Size (px)

Transcript of Development of a Deeply-Coupled GPS/INS Integration Algorithm · PDF file 2014. 4....

Microsoft Word - Yang_Fusion.docYuhong Yang, Junchuan Zhou, Holger Nies, Otmar Loffeld
Center for Sensor Systems (ZESS) University of Siegen
Siegen, Germany
Im Reihersbruch 3 St. Ingbert, Germany
Abstract—A deeply-coupled Global Positioning System (GPS)/ Inertial Navigation System (INS) integration algorithm is proposed in this paper. The mathematical system process and observation models are provided. Due to the nonlinearity of the system models, an Extended Kalman Filter (EKF) is employed, which uses quaternions as the representation of attitude. The integrated algorithm is tested using the IFEN GPS radio frequency (RF) signal simulator. Both static and dynamic scenarios are simulated. Numerical results are compared and analyzed.
Keywords—Deeply-coupled; GPS/INS; Quaternions
Deeply-coupled GPS/INS integration research has drawn more attention in the recent few years. Traditional GPS/INS integration employs a loosely or a tightly-coupled architecture. One example of different implementations of GPS/INS integration systems is illustrated in Fig.1.
RF Front End Acquisition Tracking loops (DLL, FLL/PLL)
Pseudorange & Delta Range
Tightly PR., DPR.
Loosely P. ,V.
P. V. tP. V. t
Fig. 1. GPS/INS integration architectures.
The loosely-coupled integration has a decentralized estimation architecture, which uses the output information of the navigation solutions from a GPS receiver and an INS. The main advantages of using loosely-coupled integration are: a) the system observation model is simpler, and accordingly it requires much less computational burden; b) the number of measurement inputs (i.e., position and velocity) for the Kalman filter is fixed; c) redundant GPS navigation solutions are available. However, the disadvantages are: a) in case of using two separate KFs (i.e., one for GPS navigation processing, and the other for integration purpose), it opens the possibility of presenting instable navigation solutions caused by mutual feedbacks of estimation errors, which is coined as cascaded filtering problem; b) usually more than 4 satellites are required to maintain a GPS navigation solution.
Unlike the loosely-coupled manner, in a tightly-coupled integration, only a centralized KF is used. The pseudorange and delta range (or Doppler) measurements are directly used in the filter. The advantages of applying the tightly-coupled integration are mainly in the following aspects: a) the cascaded filtering problem arising through the mutual feedbacks of the estimation errors between two separate KFs is eliminated; b) the system does not require a GPS navigation solution to aid the INS. Therefore, even with less than 4 satellites in view, the remaining satellite based measurements can still be used in the algorithm, which promotes the robustness of the navigation system; c) all systematic errors and noise sources of the distributed sensors are modeled in the same filter, which ensures that all error correlations are accounted for. Nevertheless, the disadvantage of tightly-coupled approach arises in the increased dimension of the observation vector with respect to the loosely-coupled manner. And the number of GPS measurements as inputs to the Kalman filter is varying in practical applications, which depends on the signal environments.
It is known that in a tightly-coupled system, all GPS tracking channels are working independently. In this work, we move one step forward to implement the deeply-coupled integration. The algorithm we proposed uses quaternions as the representation of attitude. This work is based on a software- defined GPS receiver we developed. The former work on this topic can be found in [1-3]. In this system, an EKF is employed to fuse the outputs (i.e., code phase error and frequency error) from the receiver tracking loops and Inertial Measurement Unit (IMU) observables to enhance GPS tracking performance, which gives robust and smoothed navigation results.
A. System Architecture
The realization of a deeply-coupled integration can be mainly divided into two categories [4]. One way directly uses the GPS receiver’s baseband correlator outputs (i.e., Is and Qs) as the measurements to fuse with the output of IMU. The other way is based on the design of a vector tracking loop, which was first proposed by Spilker [5]. He used nonlinear discriminator outputs as measurements. In this paper, our development is based on the second approach. The high level architecture of the algorithm is illustrated in Fig. 2. We employ an IMU, which contains 3-axis gyroscope and 3-axis accelerometer.
16th International Conference on Information Fusion Istanbul, Turkey, July 9-12, 2013
978-605-86311-1-3 ©2013 ISIF
RF Front End
carrier frequency
NCO Code/Carrier
I, Q Code Phase errors & Frequency errors
Fig. 2. The architecture of the deeply-coupled system.
In Fig.2, the acquisition block (at the left side of the plot) is used to acquire the incoming signals from the available satellites, and to provide code phase and coarse Doppler frequency to the tracking loop. In the tracking loop, the carrier frequency is refined to several Hz precision. Unlike the traditional GPS receiver, our deeply-coupled system uses a global integration Kalman filter for all channels. For this reason, the channels are no longer working independently, but are coupled to a common navigation solution. Thus, with the assistance from the IMU, the channels can principally help each other in the case of GPS signal attenuation conditions.
, , , ,
, , , ,
T j k j k k b k j k
f T f j k j k k d k j k
z LOS t w
z LOS t w
where ,j kz and ,
f j kz are the code phase error [m] and carrier
frequency errors [m/s] at epoch k for certain tracking loops; ˆ
kp and ˆ k v are the estimated position error and velocity
error at epoch k; ,j kLOS represents the unit line of sight
vector from the receiver to the j-th satellite; ,b kt is the
receiver clock bias in distance, and ,d kt denotes the clock
drift error [m/s]; ,j kw and , f j kw represent the white Gaussian
noise errors.
B. INS Principle
Inertial navigation is based on Newtonian physics and is affected by gravity. That is, the object will remain in uniform motion unless disturbed by an external force. It involves a blend of inertial measurements, mathematics, control system design and geodesy [7]. The external force generates acceleration on the object, which can be measured by the inertial sensor. After the integration of the measured
accelerations, under consideration of measurements from gyroscopes, the change in velocity and position with respect to the initial conditions can be determined. A conventional IMU consists of three gyroscopes for measuring angular rates and three accelerometers for measuring accelerations. They are mounted in triads so that the sensitive axes of sensors are mutually orthogonal, setting up a Cartesian reference frame. For a strapdown inertial sensor platform, the inertial sensor is rigidly mounted to the structure of the vehicle. The sensor raw data are processed to yield navigation solutions (i.e., position, velocity, and attitude over time), and this process is named strapdown processing. The INS strapdown processing models in continuous and discrete time domains are given in the next section using quaternions as the representation of attitude.
C. INS Strapdown and System Process Models
We will not use Euler angles as representation of attitude due to the inherent problem of singularity [7]. In this paper, the quaternion vector is denoted as 1[ ]T Tq q q
, where
. It is used to represent the rotation from the
navigation frame to the body frame. The attitude differential equation in terms of the
quaternion vector q is given in (2). For a detailed derivation,
the reader is referred to [8, 9].
01 ,
Tb bn
ω ω
bnω is the angular rate measurement vector from the
navigation frame to the body frame, expressed in the body frame, which is equal to:
b b b bn in ib ω ω ω (3)
where b
ibω is the rotational rate vector of the body frame
relative to the inertial frame, expressed in the body frame (i.e., IMU gyroscope raw measurements); b
inω represents the sum
of the rotation of the Earth with respect to the inertial frame plus the turn rate of the navigation frame with respect to the
Earth, expressed in the body frame, i.e., b b n n in n ie en ω R ω ω .
Using a low-cost MEMS-based IMU, the Earth rotation is usually buried in sensor errors, and cannot be detected. The Coriolis and centrifugal terms are not considered in the following sections. Moreover, for short distance applications, the transport rate is negligible. Considering these effects, we have b
in ω 0 . Thus, (3) turns to be:
b b b b bn ib bn ib ω 0 ω ω ω (4)
where , , ,
Tb b b b ib ib x ib y ib z ω is the gyroscope raw data,
resolved in the body frame.
b b b b bn bn ib ib

The simplified strapdown mechanization model for the
( )

q Q q
where np stands for position and nv is velocity in navigation
frame; ng represents gravity indicated in the navigation
frame, which is assumed to be approximately constant in the region of interest.
The rotational transformation matrix ( )n bR q from the body
frame to the navigation frame is expressed using quaternions:
2 2 2 2 1 2 3 4 2 3 1 4 1 3 2 4
2 2 2 2 2 3 1 4 1 2 3 4 3 4 1 2
2 2 2 2 2 4 1 3 1 2 3 4 1 2 3 4
2( ) 2( )
( ) 2( ) 2( )
2( ) 2( )
n b

The IMU specific force and angular rate measurement errors (e.g., sensor biases) are modeled as constants superseded by random walk, which are given in (8):
bias b f
where fw and w are assumed to be zero mean, Gaussian
distributed. It is worth mentioning that using (6) as the GPS/INS
system propagation model, the IMU incoming measurements should be compensated by the current estimate of the sensor
biases before further processing, i.e., b b bias ib ib b f f f f n ,
b b bias ib ib b w ω ω ω n ( fn and wn are the remaining noises
n k n k n k
n b n k n k b k ib k n
k k k
bias bias b k b k k
q q Q q
where “T” is system propagation time interval.
In (9), a part of the model is nonlinear, e.g., ( )n
bR q
3 3 3 3 3 4 3 3 3 3 3 1 3 1 , 1
3 3 3 3 23, 3 3 3 1 3 1 , 1
1 4 3 4 3 4 4 4 3 4 1 4 1
, 1 3 3 3 3 3 4
, 1
k bias b k

I I O O O 0 0p O I F R q O 0 0v
q O O I Q O 0 0 f
O O O I ω

3 3 3 3 3 1 3 1 ,
3 3 3 3 3 4 3 3 3 3 3 1 3 1
, 3 1 3 1 4 1 3 1 3 1
, 3 1 3 1 4 1 3 1 3 1
T T T T T b k
T T T T T b k
c t T
ω O O O O I 0 0
0 0 0 0 0
0 0 0 0 0
3 2 1
and 23,kF is computed as:
1 4 3 2 3 4 3 2 1 1 4 2
23, 1 4 2 3 2 1 2 3 4 1 4 3
3 2 1 1 4 2 1 4 3 2
ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ2
ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
x y z x y z x y z y x z
k y x z x y z x y z x y z
x y z y x z x y z
q f q f q f q f q f q f q f q f q f q f q f q f
q f q f q f q f q f q f q f q f q f q f q f q f

k f q f q f

In (12), , , ˆ ˆb bias x ib x b x
k f f f , , ,
k f f f and
, , ˆ ˆb bias z ib z b z
k f f f are the IMU raw data compensated by the
current estimate of the sensor biases expressed in the body frame.
D. System Measurement Model
1, 1, 3 1
d k
tz LOS
f j kz are code phase and carrier frequency
discriminator error outputs. Equation (13) is the matrix representation of (1).
For the code phase error, we employ a delay lock loop (DLL) discriminator, which is a non-coherent early minus late envelope normalized by E+L, to remove the amplitude sensitivity [10].
where 2 2 2 2,E E L LE I Q L I Q . EI and LI represent sum
of the in-phase early and late correlator outputs respectively between time epoch k and k+1; EQ and LQ represent the sum of quadrature early and late correlation results between k and k+1 epoch.
For the carrier frequency error, we employ a normalized decision directed frequency lock loop (FLL) discriminator to eliminate the data bit transition sensitivity [10].
2 1
where 1 2 1 2P P P Pdot I I Q Q , 1 2 2 1P P P Pcross I Q I Q .
The 1PI and 2PI denote the in-phase prompt accumulated
correlation results over time epoch k-1 to k and k to k+1; 1PQ
and 2PQ denote the quadrature prompt correlation results over
time periods.
A. Experiment Setup
The experiment setup is shown in Fig. 3. The simulation is conducted using the trajectories generated from the IFEN hardware GPS RF signal simulator NavX®-NCS. The high frequency GPS signals are sampled, down converted and collected by the hardware front end GN3Sv3 from Sparkfun at the sampling frequency of 38.184 kHz. Then the intermediate frequency (IF) signals are handled over to the integration algorithm as the GPS signal input for post processing. The IMU data is generated using INS Toolbox 2.0 (2005) for Matlab developed by GPSoft Ltd. The free Flight Dynamic Control Toolbox is used to calculate the initial attitudes of the platform based on the simulated dynamic trajectory.
Fig. 3. Simulation experiment setup.
The IMU data is simulated based on the parameters from a LandmarkTM 20 eXT MEMS-IMU. The main specification parameters are given in Table 1.
Gyroscope (Angular rates)
(Specific forces) Bias in-run
20 [μg] 40 [μg/√Hz] ≤1000
B. Straight Line Path
In the first experiment, a straight line trajectory is generated by the signal simulator with constant velocity of 110 m/s toward east. For comparision puporse, red curve of Fig. 4 is the navigation results from a deeply-coupled sytem. The blue dots are the standalone GPS solution using the least- squares estimator.
scalar tracking loop + least−squares INS−aided tracking loop
Fig. 4. 3-D position navigation results. As shown in Fig. 4, the plot is given in ENU (east-north-
up) frame. The deeply-coupled system presents much better navigation solution than the standalone GPS solution. This is because that the IMU estimates can smooth the noisy code phase and frequency errors, which stabilizes the tracking loop. The prompt in-phase components from 6 tracking loops are plotted in Fig. 5 to show the internal working of the tracking.
0 1000 2000 3000 4000 −1
Epoch [1ms] 0 1000 2000 3000 4000
Epoch [1ms]
Epoch [1ms]
Epoch [1ms] 0 1000 2000 3000 4000
Epoch [1ms]
Epoch [1ms]
Fig.5. In phase component of tracking loops in a deeply-coupled system.
Fig. 5 shows that with the aiding from IMU observable, the tracking loops work fine. In this way, all the channels are connected to each other by the navigation filter, and each channel is able to correctly track the GPS satellite signals and decode the navigation data.
The similar smoothed behaviour can also be observed in the velocity solution. Fig. 6 shows that the velocity of the object is also correctly estimated. The estimated east velocity is about 110 m/s, where in the north and up directions, the estimates are around zero.
0 500 1000 1500 2000 2500 3000 3500 4000 109
0 500 1000 1500 2000 2500 3000 3500 4000 −1
0 500 1000 1500 2000 2500 3000 3500 4000 −2
Fig. 6. Velocity estimation comparison in ENU navigation frame.
The results of straight line test show that the deeply- coupled integration system gives smoothed results in constant acceleration scenario. This is because a Kalman filter is used inside which fuses information from all the channels by estimating the common state, and the inertial sensors can provide dynamic measurements (in this test, they are zeros plus sensor errors), which are used in the system process model. Practically, using true dynamic measurements turns out to be better than using dynamic models. Because in reality, the true dynamic of the platform may often not be described by any model due to the lack of enough a priori knowledge on the trajectory.
For testing the algorithm with changing accelerations, a clockwise circle path is employed in the next section.
C. Clockwise Circle Path
In this test, a clockwise circle path is simulated. The platform flies with constant norm velocity of 30 m/s. The path radius is 1 km, with circle center point 48.1815° (latitude), 11.808° (longitude), 525 m (height). In this case, in the horizontal plane, the accelerations (i.e., the output of x and y- axis accelerometers) are changing over time. For the attitude, the heading (i.e., the output of gyro-z axis) is changing constantly over time. The position and velocity navigation results for 20 seconds are compared with GPS alone solution
(using a least-squares estimator) in Fig. 7 and Fig. 8. In both figures, the red curves are the results from the deeply-coupled integration, while the blue curves are the results from GPS stand-alone system. The figures show that with INS aiding, the integration solution is less noisy and much smoother as compared with a GPS only solution.
0 5.000 10.000 15.000 20.000 −600
Fig.7. Navigation solution comparisons in ENU navigation frame.
0 2.000 4.000 6.000 8.000 10.000 12.000 14.000 16.000 18.000 20.000 −35
scalar tracking loop + least−squares INS−aided tracking loop
0 2.000 4.000 6.000 8.000 10.000 12.000 14.000 16.000 18.000 20.000 0
/s ]
0 2.000 4.000 6.000 8.000 10.000 12.000 14.000 16.000 18.000 20.000 −5
Fig. 8. Velocity estimation comparison in ENU navigation frame.
In this test, the advantage of the deeply-coupled system comes from the fact that the IMU can measure the true dynamics of the platform, and this information is used in the tracking loops which can improve the tracking behavior. Besides, in the case that the platform encounters challenging signal environments (e.g., GPS…