ieee transactions on intelligent...

This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS 1

High-Precision Vehicle Navigation in UrbanEnvironments Using an MEM’s IMU and

Single-Frequency GPS ReceiverSheng Zhao, Yiming Chen, and Jay A. Farrell

Abstract—Many applications demand high-precision navigationin urban environments. Two frequency real-time kinematic (RTK)Global Positioning System (GPS) receivers are too expensive forlow-cost or consumer-grade projects. As single-frequency GPSreceivers are getting less expensive and more capable, more peopleare utilizing single-frequency RTK GPS techniques to achievehigh accuracy in such applications. However, compared with dual-frequency receivers, it is much more difficult to resolve the inte-ger ambiguity vector using single-frequency phase measurementsand therefore more difficult to achieve reliable high-precisionnavigation. This paper presents a real-time sliding-window es-timator that tightly integrates differential GPS and an inertialmeasurement unit to achieve reliable high-precision naviga-tion performance in GPS-challenged urban environments usinglow-cost single-frequency GPS receivers. Moreover, this paperproposes a novel method to utilize the phase measurements, with-out resolving the integer ambiguity vector. Experimental resultsdemonstrate real-time position estimation performance at thedecimeter level. Furthermore, the novel use of phase measure-ments improves the robustness of the estimator to pseudorangemultipath error.

Index Terms—Maximum a posteriori estimation, sensor fusion,inertial navigation, Global Positioning System, localization, stateestimation.

I. INTRODUCTION

R ELIABLE high precision navigation is a core functional-ity for autonomous and safety assisted highway vehicles,

autonomous plowing or mowing robots [1], and unmanned airvehicles (UAVs) [2], [3]. For such applications that requirethe vehicle trajectory for planning and control, navigation in-cludes estimation of the full vehicle state vector (e.g., position,velocity, attitude, angular rate, acceleration). In many suchsystems, a Global Positioning System (GPS) receiver is the

Manuscript received March 10, 2015; revised September 18, 2015; acceptedFebruary 4, 2016. This work was supported in part by a Department ofArmy grant through University of Texas–Pan America with Contract NumberW911NF-12-1-0094-01. The Associate Editor for this paper was Y. Song.

S. Zhao was with the Department of Electrical and Computer Engineering,University of California, Riverside, CA 92521 USA. He is now with GoogleInc., Mountain View, CA 94043 USA (e-mail: [email protected]).

Y. Chen was with the Department of Electrical and Computer Engineering,University of California, Riverside, CA 92521 USA. He is now with QualcommResearch, San Diego, CA 92121 USA (e-mail: [email protected]).

J. A. Farrell is with the Department of Electrical and Computer Engineering,University of California, Riverside, CA 92521 USA (e-mail: [email protected]).

Color versions of one or more of the figures in this paper are available onlineat http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/TITS.2016.2529000

sensor responsible for establishing the global position of thevehicle, as is necessary for maneuvering with respect to digitalmaps referenced in a standard geodetic frame.

GPS receivers can achieve different levels of accuracy. Witha clear view of the sky, a well-designed GPS unit receiving atleast four satellite signals can achieve 3–8 meter positioningaccuracy with the GPS Standard Positioning Service (SPS)[4]. As the mobile communication networks (4G or WiFi)becomes ubiquitous, real-time DGPS techniques can be usedin most urban environments to achieve 1 to 3 meter positioningaccuracy using GPS pseudorange measurements [1], [5]–[7].Real-Time Kinematic (RTK) techniques using the carrier phasemeasurements are capable of achieving centimeter accuracyafter resolving the integer ambiguities. Such RTK methodsusing two-frequency receivers have been demonstrated in realtime [8], [9]; however, reliable, real-time integer resolution forsingle frequency receivers is still quite challenging [10]. Dualfrequency RTK GPS receivers are currently too expensive forlow-cost/consumer-grade applications and projects. Alterna-tively, single frequency (L1-only) GPS receivers have becomereadily available in the market at prices very low in comparisonto dual frequency receivers.

Using GPS alone has certain limitations. First, especiallyin urban environments, GPS signals can be blocked by treesand tall buildings; therefore, using GPS alone cannot reliablyprovide continuously available, accurate global positioning so-lutions. Fig. 1 shows an example of typical GPS coverage inan urban environment. This route around the UCR campus ischallenging for GPS based navigation because GPS signalsare frequently blocked, often there are fewer than four satel-lites available, sometimes for extended durations. Second, GPSreceivers provide a low bandwidth estimate of position andvelocity. Using multiple antennae allows GPS to also estimateattitude [11]. However, GPS cannot provide an accurate, high-bandwidth estimates of the full vehicle state vector.

An inertial measurement unit (IMU) provides signals (spe-cific force and angular rate) useful for tracking the sensorsego-motion at a high bandwidth, but produce position, velocity,and attitude estimates that slowly drift from their true values.Micro-Electro-Mechanical IMUs are getting much cheaper, somuch so that they can now be found in many consumer-gradedevices (e.g., mobile phones) and are enabling new applica-tions, e.g., [1].

This paper focuses on the design of high precision stateestimation combining a low-cost single-frequency GPS receiver

1524-9050 © 2016 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.

mailto: [email protected]





2 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Fig. 1. Test trajectory marked with colors to represent the number of satellitesavailable to the receiver along the trajectory.

and IMU. This is a very popular sensor combination because theGNSS and IMU sensors are complementary. The resulting real-time systems, typically implemented with an extended Kalmanfilter (EKF) [8], [12]–[15], can achieve high-bandwidth, high-sample rate full state estimates with error state covariance thatallows on-line assessment of the system accuracy which isdependent on the satellite availability. These typical approachesuse GNSS pseudorange and either Doppler or triple-differencedphase measurements.

This paper is the first literature report proposing a novelsliding-window, Bayesian estimation method using only IMUand DGPS L1-only pseudorange and phase measurements,without resolving the integers ambiguities. The full nonlinearestimation problem is solved in real-time yielding a high perfor-mance and highly reliable tightly-coupled navigation system.The results, reliably achieving submeter accuracy, are demon-strated in a GPS-challenged urban environment.

The paper is organized as follows. Section II reviews therelated literature. Section III introduces the various sensors.Section IV presents a brief review of the real-time slidingwindow estimator. Section V derives the integer-free phasemeasurement. Section VI illustrates the software architectureof the system. Section VII presents the experimental results.Section VIII discusses conclusions and future work.

II. COMPARISON WITH EXISTING LITERATURE

A. GPS Carrier Phase Approaches

Use of GPS carrier phase measurements can yield positionestimation accuracy at the centimeter level when the integerambiguities are resolved [16]. Integer ambiguity resolution isa well-researched topic with many solutions, particularly fordual-frequency GPS receivers [8], [17], [18]. The drawback ofsuch systems is the high cost of the two-frequency receiver.

For a single-frequency GPS receiver, resolving the integerin real time is very challenging [1], [10], [19]–[21]. The most

serious challenge is the inability to use the multi-frequencycombinations [10], [16], [22] that greatly reduce the size ofthe relevant integer searching space. Moreover, the numberof measurements from a single frequency receiver is half ofthat from a dual frequency receiver. In urban environments,the intermittent signal reception caused by signal blockagenecessitates frequent integer search, because each time thereceiver reacquires each satellite signal, the integer in the carrierphase measurement is different and must be estimated again.Without the correctly resolved integer, the phase measurementdoes not provide absolute range information and the positionaccuracy is limited to the pseudorange accuracy cited above.

One alternative is the triple difference technique in which thephase measurement is differenced between pairs of consecutivetimes to eliminate the unknown integer [23], [24]. In both[20], [23] using triple differencing, the authors report achievingsubmeter accuracy after several hundred seconds of stationaryoperation. In both [1], [23], the authors proposed an integersearch and validation method based on the triple differenceestimation results. The time required to reliably resolve theintegers was not reported. In [21], the authors utilize a modifiedtriple difference technique in an EKF to track the relativepositions of the receivers, when starting from a perfectly knowninitial configuration. The global positions are not estimated inthis approach.

Integer resolution can also use multiple measurement epochs[9], [25], which increases the measurement redundancy, toresolve the integers. The existing (GNSS-only) multiple epochapproaches do not use an IMU to constrain the state vectorestimates across the multiple times.

The methods proposed herein are related to, but distinctform, the triple difference technique. Instead of differencingpairs of consecutive measurements to eliminate the integerand implementing an EKF, we propose a systematic methodto eliminate the integer for a time window of measurements.We present a novel algorithm wherein the IMU measurementsand system kinematics provide strong constraints on the statevector estimates across multiple GNSS epochs, the integers arenot resolved, yet decimeter accuracy is achieved. The proposedmethod is optimal in that it preserves all the information fromthe IMU and GNSS measurements and correctly captures thetime correlation of the resulting GNSS kinematic constraints.

B. Estimator

To account for the strong nonlinearities present in the nav-igation system and to take full advantage of the correlatedinformation in the IMU and GNSS sensors, this paper utilizesa real-time sliding window estimator to achieve better perfor-mance. Related algorithms have received significant attentionin the SLAM community [26]–[30]. However, none of thesepapers report the performance for tightly-coupled carrier-phaseDGPS/IMU systems. The most closely related approach is[30]. However, that paper used a simplified GPS model with aloosely-coupled approach. They reported similar performancebetween their approach and an EKF. The approach herein willdemonstrate a significant performance improvement comparedto the EKF, especially in GPS-challenged urban environments.


ZHAO et al.: NAVIGATION IN URBAN ENVIRONMENTS USING AN MEM’s IMU AND GPS RECEIVER 3

The navigation system designed in this paper is similar inconcept to the one proposed in [28], [29]. However, [28], [29]are focused on the Vision/IMU integration and they have notreported any tightly-coupled GPS/IMU results. Article [31]utilized a sliding-window estimator with pseudorange measure-ments only. To the best of our knowledge this is the first reportof a real-time sliding window tightly-coupled DGPS/IMU es-timation system that achieves high performance using carrierphase without integer resolution.

III. SYSTEM OVERVIEW

This sections briefly overviews Differential GPS, inertialmeasurements, and inertial navigation.

A. Inertial Navigation System

Let x ∈ Rn denote the rover state vector:

x =[Gp�

IGv�

IIGq

� Ib�g

Ib�a

]�(1)

where n = 16, GpI , and GvI are the rover position and velocityrepresented in the global frame, I

Gq is the unit quaternion thatrepresents the rotation from the global frame to the IMU bodyframe, and Ibg and Iba are the IMU gyro and accelerometerbiases represented in the IMU frame.

The kinematic equations for the rover state are

x(t) = f (x(t),u(t)) (2)

where f : Rn × Rm �→ Rn and u ∈ Rm is the vector of spe-cific forces and angular rates. The function f is accuratelyknown (see [15, Chapter 11]).

Given a distribution for the state vector initial conditionx(0) ∼ N (x(0),P(0)) and measurements u of u, an InertialNavigation System (INS) propagates the estimate of the vehiclestate x(t) between aiding measurement times by solving

˙x(t) = f (x(t), u(t)) . (3)

The integral operation propagating the state estimate betweentwo time steps using the IMU measurements Ui = {u(t) | t ∈[ti, ti+1]} is denoted as

x(ti+1) = F (x(ti),Ui) . (4)

Due to initial condition errors, system calibration errors, andmeasurement noise, a state estimation error x(t) = x(t)− x(t)develops over time:

x =[Gp�

IGv�

IGδθ� I b�

gI b�

a

]�∈ R

dx . (5)

where dx = 15. The dynamic model and stochastic propertiesof this estimation error are well understood [15].

The INS constraint eiΔ between state x(ti+1) and x(ti) is

eiΔ (x(ti+1),x(ti)) = x(ti+1)− F (x(ti),Ui) . (6)

The uncertainty of eiΔ is assumed to be Gaussian with covari-ance matrix denoted by Qd

i . The linearization of F and thecomputation of Qd

i is discussed in [15].

B. Differential-GPS

Two types of measurements are provided by GPS receivers:pseudorange (code) and carrier phase measurements. DGPShas advantages over stand-alone GPS in that the common-mode errors (e.g., ionosphere, troposphere, satellite clock andephemeris errors) can be essentially removed by differencingmeasurements between the rover receiver and the GPS basestation receiver. This step is referred as the single differencestep. It is assumed in this paper that the (single difference)DGPS approach completely removes all common-mode errors.Receiver clock errors are not removed by DGPS. To avoid themodeling of the receiver clock error, which would involve twoadditional filter states, double differenced GPS measurementsare considered herein. The approach easily extends to the casewhere the designer chooses to use single differences and modelsthe clock error in the state vector.

The notation used follows several conventions:

• The overhead bar is used to denote the variable obtainedfrom the single differencing step. For example, the singledifferenced pseudorange and phase measurements aredenoted by ρ and φ, respectively.

• The superscript c on a variable is used to denote the com-mon satellite chosen in the double differencing method.

1) Pseudorange Measurement: The double differencedpseudorange measurements for the i-th satellite vehicle (SV)at time t can be modeled as

ρic(t) = γic (x(t)) + nicρ (t) (7)

where γic = γi − γc, γi = ‖pr − pisv‖2 is the geometric dis-

tance between the vehicle antenna position pr ∈ R3 and thei-th SV antenna position pi

sv ∈ R3, ρic = ρi − ρc, ρi is thesingle differenced pseudorange measurement of the i-th SV,nicρ = ni

ρ − ncρ, and ni

ρ is the single differenced measurementnoise. The double differenced noise standard deviation is typi-cally σρ ∈

√2 · [0.1, 2.0] meter [32]. The noise ni

ρ includes themulti-path error which can be several meters [33]. In this paper,the time correlation of the multi-path is ignored. In future work,it could be accommodated by augmenting additional states foreach satellite.

Thus, the residual function of the double differenced codemeasurement can be formed from eqn. (7) as:

eiρ (x(t)) = ρic(t)− γic (x(t)) . (8)

The uncertainty of eiρ(x(t)) is assumed to be Gaussian with thevariance denoted by σ2

ρ .2) Carrier Phase Measurement: The double differenced

carrier phase measurement model for the i-th SV is:

λφic(t) = γic (x(t)) + λN ic(t) + nicφ (t) (9)



where φic = φi − φc and nicφ = ni

φ − ncφ, φi is the single dif-

ferenced phase measurement of i-th SV, N ic is an unknowninteger ambiguity, λ is the carrier signal wavelength (19.05 cmfor L1 signal), and ni

φ is the single differenced measurementnoise. The phase noise nic

φ includes the multi-path error which

has a standard deviation in the centimeter range (σφ ≈√

2 ·0.02 meter) [34]. Its time correlation is not modeled herein.

The integer N ic is constant over time intervals when thereceiver has phase lock for both SV’s i and c. The receiverindicates this lock with a flag and lock time counter. When theunknown integer N ic is resolved, the double differenced phaseobservable measures the range with centimeter accuracy. Thus,the residual of the double differenced phase measurement afterresolving the integer can be formed as:

eiφ (x(t)) = λ(φic −N ic)− γic (x(t)) . (10)

The uncertainty of eiφ(x(t)) is assumed to be Gaussian with thevariance denoted by σ2

φ.

IV. SLIDING-WINDOW ESTIMATOR

A sliding-window estimator, referred to as a ContemplativeReal-Time (CRT) estimator in [31], is used to combine theGPS and IMU data. The CRT approach is inspired by recentresearch in the field robotics literature [26]–[29], [35]. TheCRT approach has both real-time and contemplative aspects.The real-time state estimate is required for control and planningpurposes, without latency. The contemplative aspects over mul-tiple epochs are intended to enhance accuracy and reliability byremoving bad measurements and fixing inaccurate linearizationpoints.

A. Full MAP Estimation

Let X denotes the vehicle trajectory over a time window

X =[x(t0)

�, . . . ,x(tk)�]� (11)

where x(t) denotes the vehicle state at time t.Assume there is a prior for the first state: x(t0)∼N (x0,P0).

The corresponding residual function is

e0 (x(t0)) = x(t0)− x0. (12)

Assume there are ns aiding sensors where the j-th sensorprovides a measurement yij � yj(ti) ∈ Rmj . Each sensor hasthe model:

yij = hj (x(ti)) + nyj(ti). (13)

The covariance of nyj(ti) is Rij . The corresponding residual

equation is

ejyi(x(ti)) = yij − hj (x(ti)) . (14)

Estimation of the vehicle trajectory X can be formulated asa Maximum A Posteriori (MAP) problem:

X = argmaxX

{p(X,U,Y)} (15)

where U = {u(t) | t ∈ [t0, tk]} is the high sampling rate IMUdata, and Y = {yij | i = 0, . . . , k, and j = 1, . . . , ns}. TheAppendix shows that p(X,U,Y) can be decomposed as

p(X,U,Y)

= p (x(t0))

k−1∏i=0

p (x(ti+1)|x(ti),Ui)∏

(i,j)∈Yp (yij |x(ti))

(16)

where p(x(ti+1)|x(ti),Ui) is the distribution of the kinematicconstraint in eqn. (6), p(x(t0)) is the distribution of the initialcondition defined in eqn. (12), and p(yij |x(ti)) is the distribu-tion of the measurement constraint in eqn. (14).

Assuming that the noise terms have Gaussian distributions,using log-likelihood, the MAP estimation problem can be con-verted to an equivalent nonlinear least squares problem:

X = argminX

{∑s∈S

‖es(X)‖2Rs

}(17)

where es(X) is the vector residual function defined for all theinformation available (denoted by the set S). The set S contains{eiΔ|i=0,...,k−1} defined in eqn. (6), {ejyi

|(i,j)∈Y} defined in(14) and the prior e0 defined in (12). The matrix Rs is thecovariance matrix and ‖ · ‖2Rs

denotes the squared Mahalanobisnorm: ‖x‖2A = x�A−1x for positive definite A.

This nonlinear optimization problem can be solved iter-atively (e.g., by Gauss-Newton method). At each iteration,the residual function es(X) is linearized around the currenttrajectory estimate X. Let

Js =∂es∂X

∣∣∣∣X

(18)

Λs =J�s R

−1s Js (19)

ηs =J�s R

−1s es(X). (20)

The linearized normal equation is:

ΛδX = η (21)

where Λ =∑

s∈S Λs is the trajectory information matrix, η =∑s∈S ηs is the trajectory information vector. Eqn. (21) can be

solved for δX, for example by the Cholesky factorization asdiscussed in [26]. For a long vehicle trajectory, the sparsity of Λcan be exploited to speed up the Cholesky factorization. Aftersolving for δX, the trajectory is corrected by X = X+ δX.The iteration repeats starting from eqn. (18) by re-computingΛ and η using the new linearization trajectory X. The iterativeprocess is terminated when ‖η‖2 < ε, where ε > 0 is a userdefined termination threshold.



Solving the full MAP problem over the entire measurementhistory yields in the optimal estimate of the vehicle trajectoryX. When the structure of Λ is sparse, the computation loadincreases approximately linearly with the length of trajectory.When the structure of Λ is dense, the computation is cubic withthe length of trajectory. The techniques from iSAM [27] canbe used to speed up the computation by fixing the linearizationpoint for a short period of time and re-evaluatingΛ periodically.However, iSAM requires the storage of all the measurementsover the entire trajectory and the periodic re-linearization ofthe entire trajectory is still computationally intense. The iSAM2[36] avoids the periodic re-linearization of the entire trajectoryby performing fluid relinearization.

For applications that run indefinitely (or for long durations),even a computational load growing linearly with time eventu-ally becomes excessive relative to the computation and mem-ory requirement of low cost embedded processors. Therefore,solving the full MAP problem for the entire trajectory in real-time is infeasible. In fact, optimization over the entire trajectoryis not required to achieve enhanced accuracy and reliabilityrelative to a standard (single epoch) EKF implementation, aswe demonstrate herein. In this paper, we use a sliding windowestimator, with computational load determined by the length Mof the window.

In sliding window approaches, care must be taken to ensurethe consistency of the estimate trajectory [29], [37]. Marginal-ization of old states to ensure consistency is discussed in thefollowing subsection.

B. Marginalization

At time tM−1, after solving the MAP estimation problem,the oldest state x(t0) needs to be removed, to keep the CRTwindow size constant when the next state x(tM ) is added. Tocorrectly account for the information lost with the removedstate, a marginalization step is required.

Let X = [X�m,X�

r ]�

where Xm are the states that are goingto be removed and Xr are the states that will be retained. Thismarginalization process can be performed by the following pro-cedure [29]. Let Sm ⊂ S such that for any s ∈ Sm the residualfunction es depends on Xm and for any s �∈ Sm, es is not afunction of Xm. Define Λ =

∑s∈Sm Λs and η =

∑s∈Sm ηs.

Thus, the information matrix Λ and information vector η canbe partitioned accordingly:

Λ =

[Λmm Λmr

Λ�mr Λrr

](22)

η =

[ηm

ηr

](23)

where Λmm and ηm correspond to the state that will bemarginalized out.

The Schur complement is employed to marginalize out Xm:

Λα =Λrr −Λ�mrΛ

−1mmΛmr (24)

ηα =ηr −Λ�mrΛ

−1mmηm (25)

where Λα and ηα are the trajectory information matrix andvector for Xr after marginalization of Xm.

After solving the MAP problem by iterations of eqns.(18)–(21), we have an estimate of the trajectory X =

[X�m, X�

r ]�

. The estimate Xr can be used as the prior for Xr

for the next CRT window after sliding. Define Xα � Xr. Theresidual function for this prior is

eα(Xr) = Xr − Xα. (26)

The uncertainty of eα(Xr) is specified by the informationmatrix Λα. The measurement residual projected on the statespace is stored in ηα.

The estimator with marginalization is consistent [29]. In ad-dition, the computational cost is bounded due to the fixed lengthwindow, which is critical for the implementation in a low-costconsumer device, intended to run potentially forever, whereineven linear growth in computational load would eventuallyoverload the processor.

C. MAP Estimation in a CRT Window

For each CRT window, the vehicle trajectory X is

X =[x(tk−M+1)

�, . . . ,x(tk)�]� . (27)

The MAP estimation problem in the CRT window can bereformulated as [29]:

X = argminX

{∑s∈S

‖es(X)‖2Rs− 2η�

αeα(X)

}(28)

where the es(X) is the residual function defined for all theinformation available in the CRT window (denoted by S).The set S contains {eiΔ|i=k−M+1,...,k} defined in eqn. (6),{ejyi

|(i,j)∈Sy} defined in (14) where Sy is the set of measure-ments available in the window, and the prior eα is definedin (26).

To analyze the performance of the CRT estimator inSection VII, it is convenient to define several variables. Startingfrom eqn. (17), the cost C is defined as:

C(X) =∑s∈S

‖es(X)‖2Rs− 2η�

αeα(X) (29)

where the second term accounts for the information in theprior trajectory after marginalization, see [29]. Then, the costreduction ΔCi at each iteration can be defined as ΔCi =C0 − Ci where Ci for i = 0, . . . , ζ denotes the cost after thei-th iteration and ζ is the total number of iterations.

D. Covariance Recovery

For outlier detection, the covariance matrix of the latest statex(tk) is required and can be efficiently recovered using themethods in [27], [38].

Let the Cholesky decomposition of Λ be Λ = R�R whereR is an upper triangular matrix. Let P = Λ−1 be the covariance



matrix of the trajectory and

B =

[0(d−dx)×dx

Idx×dx

](30)

where d = dxM is the size of Λ and dx is defined in eqn. (5).Hence by solving

(R�R)Pck = B (31)

where Pck = [P�1k, . . . ,P

�kk]

�contains the last fifteen

columns of P, the covariance of the latest vehicle state(Pkk) can be obtained. The solution employs the forward andbackward substitution:

R�K =B (32)

RP =K (33)

where K can be easily obtained by solving eqn. (32):

K =

[0

R−�kk

](34)

where Rkk is the right bottom portion of R that corresponds tothe latest vehicle state. From K, eqn. (33) can be solved forPkk . Other elements in arbitrary positions of the covariancematrix P can also be recovered by the method proposed in [38]at extra computational cost.

E. Comparison of CRT With EKF

The tradeoffs of the CRT estimator relative to the EKF are:

1) CRT has the ability to change the linearization point ofthe trajectory within the CRT window. For the EKF thelinearization point is the prior. Errors in the prior if largerelative to the higher order terms of the linearization cancause the EKF to diverge.

2) In the CRT approach multiple iterations, each with relin-earization, are possible to fully address the nonlinearitiesin the MAP optimization. The standard EKF performs asingle iteration per measurement.

3) With a window of sensor data, the CRT redundancy(prior, INS state transitions, measurements for all Mmeasurement times) is extensive allowing detailed outlierdetection. Also, since all raw data over the window isretained, fault decisions at one time can be reconsideredat future times, as long as the data is still in the win-dow. The effects of a previous incorrect decision can befully removed by the MAP optimization process. Withthe standard EKF, an incorrect fault decision can becatastrophic and the redundancy available (prior and themeasurements at a single time) is often insufficient tomake confident fault decisions.

4) The computation effort of the CRT is larger than EKF,increasing with window size and number of iterations.

Fig. 2. An illustration of the measurement time-line and CRT window. Thered dots represent the vehicle states at the GPS measurement times. The greenlines represent the IMU constraints between two consecutive states. At tk theCRT window is formed and the optimization problem begins its computation.The optimization solution is available at t∗ = tk + δk . As in an EKF, for t ∈[tk, t

∗], the prior state is propagated by the INS until the optimal solution isobtained at t∗ . Then, the optimal state estimate is propagated from tk to t ≥ t∗

using all the IMU measurements in that time interval. At t = tk+1 the windowadvances, and the process repeats.

F. Application to GPS/INS

For GPS/INS, the measurement equation (13) is redefinedfor the pseudorange and phase measurements in eqn. (7) and(9), and the integer-free phase measurements that are going tobe defined in (42). A typical measurement scenario is depictedin Fig. 2. The red dots on the time-line represent the state atthe GPS measurement times tk. The state transition betweenthese times is constrained by the kinematic model of eqn. (3)and the IMU data U. Additional constraints are imposed bythe initial condition (x0,P0), and GPS measurements Y. Theinitial condition constraint is shown above the time-line. TheGPS measurement constraints (y) are depicted below the time-line. The GPS measurements are not synchronized with theIMU measurement time. The unaligned measurements can beaddressed by interpolation, and unknown latencies could becalibrated by the methods in [39].

A typical CRT process starts when t = tk, using all IMUand GPS measurements that are available for the time interval[tk−M+1, tk]. Starting with the prior defined in (26) and inte-grating forward through time using the IMU data and kinematicmodel provides an initial trajectory across the CRT window.Starting from this initial trajectory, the CRT algorithm willconsider all the available information to reliably and accu-rately compute the state trajectory over the CRT window usingthe optimization process discussed above and fault detectionmethods (which are left for future work). This CRT processends at a time t∗ = tk + δk, where δk > 0 is a small fractionof a second, ideally providing an optimal trajectory estimatefrom which the effects of sensor faults have been removed.Within the computation time interval t ∈ [tk, t

∗], the real-timestate estimate x(t) is maintained by the INS using the IMUdata and starting from the initial estimate of x(tk). At t = t∗,x(tk) is corrected to the result of the CRT optimization processand propagated through time using the IMU data and eqn. (3)to provide an improved estimate of x(t) at the present time.Because the computation time δk = (t∗ − tk) is small, signifi-cantly less than the time between GPS measurements, the INSis integrating the state until the next measurement, as is thecase with an EKF implementation. At some time tk+1 ≥ t∗,the CRT window is redefined and the process repeats indefi-nitely. For the GPS/INS case, the CRT process happens at eachGPS epoch.



V. INTEGER-FREE PHASE MEASUREMENT

A. Motivation

For a single-frequency receiver, the integer ambiguity isdifficult to resolve reliably in real time. Nonetheless, thereare at least two reasons why we still want to use the phasemeasurements, even when the integers cannot be resolved:

1) Multi-path introduces only a few centimeters of error inthe phase measurement while the code measurement canbe affected by a few meters.

2) Phase measurements over a time window provide localposition change constraints on the trajectory with cen-timeter accuracy even when the correct integer cannot beresolved.

When the correct integer are not resolved, there are twostandard ways to utilize the phase measurements:

1) The triple difference technique [20] creates an integer-

free measurement: φic

k at tk for i-th SV defined as:

λφic

k = γic (x(tk))− γick−1 (x(tk−1)) + nic

φ (tk) (35)

where φic

k =φic(tk)−φic(tk−1) and nicφ (tk)=nic

φ (tk)−nicφ (tk−1). This equation is derived by subtracting eqn.

(9) at tk−1 from that at tk. The benefit of this approach

is that the resulting quantity φic

k is independent of theinteger and thus does not require integer estimation. How-ever, the triple difference only considers two consecutivemeasurements, losing the strong time correlation betweenall the phase measurements (sharing a constant integer)over a time window. Note also that even when nic

φ (t) iswhite, nic

φ (t) is time correlated and that the measurementmodeled in eqn. (35) depends on the state at two distincttimes, which defies the standard EKF model.

2) Alternatively, the integers can be treated as real variablesand estimated together with the vehicle state vector [19].The major drawback of this approach is that the integerconstraint is not respected in the estimation process,resulting in information loss. These approaches require20 minutes for the real “integer” estimates to converge tosingle integer accuracy. Moreover, this approach requiresadding all the integers into the estimator which increasesthe state vector size and thus increases the computationalcomplexity.

To fully utilize the phase measurements when the correctintegers cannot be resolved, this paper proposed a new way ofusing phase measurements over a CRT window. In the proposedframework, an integer-free phase measurement is constructedthat is independent of the integer and that correctly captures thetime correlation of phase measurements over a time window.

B. Phase Track

This section defines the concept of a phase track.

Fig. 3. An illustration of the phase measurement availability pattern in a CRTwindow with M = 8. The y-axis is the SV ID number. Each thick blue linesrepresents a set Ti

j .

Fig. 4. An illustration of the phase tracks obtained by choosing the first SVas the common SV for all time steps in the CRT window shown in Fig. 3. Thethick blue lines represent the set Tic

j .

Let Ck = {tk−M+1, . . . , tk} be the set of times in the presentCRT window. Within Ck each satellite may have and losephase lock several times. Let Ti

j ⊆ Ck be the j-th set of timesover which the i-th satellite is available and has phase lock.Fig. 3 illustrates an example of a phase measurement availabil-ity pattern within the CRT window.

Let Ticj ⊆ Ck be the j-th set of times over which the satellite

pair i− c both have continuous phase lock. Note that Ticj ∩

Ticj+1 is always an empty set. For all t ∈ Tic

j ,

λφic(t) = γic (x(t)) + λN icj + nic

φ (t) (36)

where N icj is a constant integer. Define

Ξicj =

{φic(t) | t ∈ T

icj

}(37)

which will be referred to as a phase track. The length of thephase track Ξic

j is the cardinality of Ticj . Fig. 4 illustrates the

set of times Ticj in the phase track pattern Ξic

j resulting fromchoosing the first SV as the common SV throughout the CRTwindow. In each CRT window, the designer will choose oneSV to serve as the common satellite. For each such choice adifferent set of phase tracks results. The choice of the commonsatellite affects both the number and cardinality of the sets Tic

j .This choice is further discussed below.



C. Integer-Free Measurement

To fully utilize the phase measurements without resolvingthe integer vector, an integer-free measurement is constructedby adapting the technique proposed in [40].

Starting from eqn. (36), for any phase track Ξicj that has

length greater than one, stack all the double difference mea-surements from the phase track Ξic

j to define a vector:

λφicj = h

(XTic

j

)+ λGic

j Nicj + nic

φj(38)

where XTicj= {x(t) | t ∈ Tic

j } and

φicj =

⎡⎢⎢⎢⎣φic(t1)φic(t2)

...φic(tl)

⎤⎥⎥⎥⎦ , h

(XTic

j

)=

⎡⎢⎢⎢⎣γic (x(t1))γic (x(t2))

...γic (x(tl))

⎤⎥⎥⎥⎦ (39)

Gicj =

⎡⎢⎢⎢⎣

11...1

⎤⎥⎥⎥⎦ , nic

φj=

⎡⎢⎢⎢⎣nicφ (t1)

nicφ (t2)

...nicφ (tl)

⎤⎥⎥⎥⎦ . (40)

The covariance of nicφj

is a diagonal matrix Ricφ . In this paper,

we ignore the correlation between satellites, so that nicφ is

assumed to be uncorrelated with ndcφ for i �= d to reduce the

computational complexity (see Remark 1).The integer can be eliminated from the equation by the

following procedure. Select a unitary matrix Ai = [Ai1, A

i2]

such that the columns of Ai2 form the basis of the left null space

of Gicj (i.e., Ai

2�Gic

j = 0). Multiplying Ai2� on both sides of

(38) gives:

λφic

j = h(XTic

j

)+ nic

φj(41)

where φicj = Ai

2�φic

j , h = Ai2�h and nic

φj= Ai

2�nicφj

. Theinteger-free phase measurement induced residual equation is

eiφj(X) = λφic

j − h(XTic

j

). (42)

This integer-free phase residual function is used in eqn. (28)instead of the carrier phase residual.

Note that eqns. [(41), (42)] are independent of the integer.Eqn. (42) expresses the relative kinematic constraints betweenvehicle poses along the trajectory. The constraint is strongbecause the noise nic

φ ∼ N (0,Ai2�Ric

φAi2) has component

level standard deviations at the centimeter level with verystrong cross-correlations that are known and correctly modeled.Therefore, the optimization will correctly accommodate therelative kinematic constraints between all the vehicle poses atthe times in Ξic

j . In contrast, the triple difference technique onlycaptures the pairwise kinematic constraint between two con-secutive vehicle poses, but neglects time correlation betweensubsequent measurements.

Remark 1: Due to the double differencing procedure definedin eqn. (9), the double difference noise terms nic

φ (t) and ndcφ (t)

are correlated with each other: E(nicφ , n

dcφ ) = σ2

φ. Therefore,the vectors nic

φ and ndcφ concatenating these quantities are also

correlated. For computational reasons, in this paper, we neglectthe correlation between nic

φ and ndcφ .

Remark 2: It is easy to see that the triple difference measure-ment can be obtained from eqn. (41) by setting

Ai2�=

⎡⎢⎢⎢⎣−1 1 0 · · · · · · 00 −1 1 0 · · · 0...

. . ....

0 · · · · · · 0 −1 1

⎤⎥⎥⎥⎦ (43)

and removing the off-diagonal elements in the covariance ma-trix of nic

φ (effectively ignoring the time correlation of the phasemeasurements). Ignoring of time correlation in the triple differ-ence procedure yields a set of independent pairwise constraintsinstead of allowing window length constraints.

D. Phase Track Construction

Let ScΞ denotes the union of Ξicj over all i’s and j’s in the CRT

window. Let Vk denotes the sequence of choices of commonsatellite vehicles for each time step in Ck. The choice of Vk

affects the structure of ScΞ. The problem of choosing the set Vk

can be formulated as an optimization problem [41]:

V∗k = argmax

Vk

det

⎛⎝ ∑

(i,j)∈ScΞ

Λij(X)

⎞⎠ (44)

where Λij is the information matrix formed by the phase trackΞicj using eqn. (18). We state this theoretical optimal solution

because we have not seen it stated previously; however, theoptimal solution method for V∗

k is not currently known.In this article, we use the following the standard, though non-

optimal, approach where at time tk, if Vk−1 is available, thenthe common SV at tk will remain the same, as long as it is stillavailable. When it becomes unavailable, the SV with the highestelevation is selected as the new common SV. Because SV’s withhigh elevation are infrequently blocked, this approach tends toprovide phase tracks with high cardinality.

E. Marginalization of Integer-Free Measurements

The marginalization approach of Section IV can be usedfor the integer-free measurements. However, direct applica-tion of the marginalization described in Section IV leads toa dense prior information matrix Λα for the remaining tra-jectory Xr. This leads to complications during optimizationof eqn. (28) wherein the Jacobean cannot be relinearized forx(tk−M+2), . . . ,x(tk), see [29]. It would also eliminate theability to extend a phase track from Ck to Ck+1.

Therefore, after obtaining all the phase tracks in the CRTwindow, they are divided into two sets: SA and SB . The set SAcontains all the tracks that involve the rover states x(tk−M+1)that is going to be marginalized out at time tk. The set SB

contains all the remaining tracks. For the phase tracks in SB ,the integer-free residual function (42) is used.



Fig. 5. Software architecture.

For the phase tracks in SA, the corresponding integers areaugmented into the vector X and estimated as real variables.Hence, the new CRT estimator state vector is defined as:

X =[x(tk−M+1)

�, . . . ,x(tk)� | N1, . . . , Nh

]�(45)

where Ns for s = 1, . . . , h are the integers corresponding to thephase tracks in SA. The residual function for phase tracks in SA

becomes:

eiφN(x(t), N ic

)= λ(φic −N ic)− γic (x(t)) . (46)

In this method the marginalization can be carried out in thesame manner shown in Section IV. The prior informationmatrix Λα remains sparse and the full information matrix canbe maintained properly even if the phase track extends to thenext CRT window. Note that, if the length of a phase trackbecomes 0 after removing one state from the CRT window, thecorresponding integer will also be removed from the vector Xin the marginalization process.

VI. SOFTWARE ARCHITECTURE

To meet the real-time performance requirements and fa-cilitate the software development, the navigation system isdesigned in a multi-threading framework as shown in Fig. 5.Each thread is assigned a priority to ensure the high prioritythreads (e.g., INS propagation thread) use the CPU first and thelow priority threads (e.g., data logging) use the CPU only whenthe system would otherwise be idle. The system runs on Ubuntu10.04 with a real-time kernel.

There are 3 running modes in the system: Realtime,Rerun_TrueTime and Rerun_FastTime. In all three modes, thesystem solves the trajectory estimation problem over a windowof length M while maintaining an estimate of the currentvehicle state. The three modes differ in where they source thedata from and whether they operate at or faster than real time.

• Realtime mode: In this mode, the system obtains thesensor data from the physical sensors (the GPS basemeasurements are received via Internet).

• Rerun modes: To enable fast algorithm verification andreal-time performance tuning and debugging, two rerun

modes are designed. Both rerun modes retrieve the sensordata from log files.- Rerun_TrueTime mode: The logged sensor data is fed

into the system at the exact same time (within theaccuracy of the computer clock) relative to the start ofthe program as it was in the real-time mode. This modeis ideal for exactly reproducing the real-time runningsituation and/or debugging real-time errors.

- Rerun_FastTime mode: The logged sensor data isfed into the system immediately after all the systemprocessing is complete for the previous sensor data.This mode is ideal for debugging and/or testing thealgorithm on multiple datasets to generate statistics,as it processes each data set quickly. For example,processing a 500 sec trajectory using M = 10 takesaround 40 sec in this mode.

Therefore, the same code can be re-used for different pur-poses with only the change of desired running modes.

To facilitate the comparison of various estimators, the INScorrection thread is designed as a general estimator interfaceand can be easily replaced with any estimator, such as the EKFor sliding-window estimator. Moreover, the multi-threaded sys-tem architecture can also be extended to include multiple aidingsensors such as vision [42] and Lidar [43].

VII. EXPERIMENTAL RESULTS

This section presents analysis of data accumulated usinga test loop around the campus of University of California,Riverside, see Fig. 1. Along the test path there are many treesand buildings as is representative of a typical urban environ-ment. The test path is challenging for GPS based navigation.Snapshots along the path can be found in [44]. Fig. 1 usescolor coding along the trajectory to indicate the number ofsatellite signals received as a function of position. There aremany places where only a few satellites are available. The effectof tree cover and multipath is more difficult to discern. Anyof these effects (e.g., small number of satellites, tree cover, ormultipath) can make it extremely difficult to maintain integerlock and to resolve the correct integers.

In the experiment, the vehicle is equipped with a dual-frequency GPS receiver and an IMU, but no magnetometer(i.e., compass). The IMU provides measurements at 200 Hz.GPS measurements are taken at 1 Hz. All GPS measurementsare used in a differential mode. For comparison, a groundtruth trajectory with centimeter accuracy is computed via post-processing [44].

To initialize the CRT estimator (obtaining x0), the vehiclestarts with zero velocity. The initial position estimate is ob-tained from GPS-only double differenced pseudorange mea-surements. The initial roll and pitch estimate are obtained byextracting the gravity vector while stationary, with the engineon. To emphasize the ability to correct errors that are largerelative to the curvature of the nonlinearities, we intentionallyinitialize the yaw with the worst case error of 180 degrees.

The CRT estimator uses the same data set as the ground truthpost-processing algorithm, so that we can compare the CRT



Fig. 6. CRT Navigation system error history relative to ground truth trajectory and the number of satellites available along the trajectory. The ±3σ boundis plotted in red. The real-time state is computed using L1 code and integer-free phase. (a) North, east, and down position error (blue) with ±3σ (red).(b) North, east, and down velocity error (blue) with ±3σ (red). (c) Roll, pitch, and yaw attitude error (blue) with ±3σ (red). (d) Body frame gyro bias errors (blue)with ±3σ (red). (e) Body frame accelerometer bias error (blue) with ±3σ (red). (f) The number of satellites as a function of time.

estimation results with the ground truth trajectory. The CRTestimator only uses the L1 GPS data, discarding the L2 data.For the CRT estimator, the window size is M = 10 and thetermination threshold is ε = 10−3.

After each CRT experiment is completed, performance isanalyzed by subtracting the CRT state estimate at each time

from the ground truth state estimate for the same time. Analysisof this error state will demonstrate that the CRT window basedBayesian estimation approach combining pseudorange andphase measurements rapidly calibrates the IMU biases and IMUattitude after vehicle motion makes them observable and mit-igates the effects of code multipath. The fast, accurate, and



reliable estimation of attitude and IMU biases estimates isthe key to maintaining high precision navigation performancein GPS-challenged environments because the INS uses thecorrected IMU data to integrate through GPS time-intervalswhen few (if any) satellite signals are received.

The results of the CRT estimator using code and integer-free phase are shown in Figs. 6 and 7. Fig. 6 shows the INSerrors (position, velocity, attitude and IMU biases) of the CRTestimator with the reported ±3σ bounds. The estimator startsat t = 5 sec. The vehicle is stationary at the beginning withforward acceleration starting at t ≈ 8 sec. Note that the yawwhich is not observable prior to t = 5, has error < 1◦ by t =10 sec. The results show that (typically) the position error iswithin ±0.5 m in the horizontal plane (north and east direction),the velocity error is within ±0.01 m/s and the roll and pitcherrors are within ±0.2◦ and the yaw errors are within ±1◦.The number of satellites versus time is also shown and per-formance is maintained throughout. The reported ±3σ boundsalso correctly reflect the uncertainty of errors. Fig. 7 shows thehistogram of errors (position, velocity and attitude) of the CRTestimator along the trajectory. The first 10 seconds are excludedfrom the histogram to prevent the presentation being skewed bythe errors in the initial state (e.g., yaw).

Fig. 8 displays the CRT position estimation errors in bluewhen only pseudorange is used and in red when both pseudo-range and integer-free phase are used. The estimated trajec-tory using the integer-free phase measurement is in generalsmoother than only using the code measurements. From thiscomparison, we can see that the integer-free phase measure-ments are able to improve the robustness of estimator topseudorange multi-path error and noise. This robustness derivesfrom the kinematic constraints imposed by the integer-freephase measurements, which prevent the large changes in theposition estimates that could otherwise result from code multi-path errors.

To gain insight into the status of the CRT estimator, severalkey variables are plotted in Fig. 9 versus the GPS epoch counter.The figure shows the total number of iterations ζ, the costreduction ΔCζ , the final cost Cζ , the final value of ‖η‖2in the optimization. Immediately after initialization, it takesmore iterations per time step to converge from the initial x0

to an estimate x such that δx lies within the unobservablespace. Due to the initial inaccuracy of x0, the error within theunobservable space may still be large. For example, the yawis still completely unknown, until the vehicle accelerates. Atthis time, when the bias and attitude errors become observable,the effect of nonlinearities can be very significant; this isdemonstrated by the number of iterations again increasing. It isthe fact that the unobservable subspace changes, that allows theattitude and bias vectors to be more accurately estimated. Notefrom Fig. 6 that yaw is accurately estimated, within 1 degree,quickly after the vehicle accelerates, without any form of mag-netometer. After the trajectory estimate becomes accurate, theoptimization needs fewer iterations to converge. The final valueof ‖η‖2 is always less than the termination threshold ε = 10−3.The vector η is a projection of the residual vector onto asubspace with the same dimension as δX, see eqns. [(18)–(21)];therefore because Λ is nonsingular, the final norms of δX and

Fig. 7. Error histograms (position, velocity and attitude) for the entire tra-jectory (10 sec–500 sec). The CRT estimator uses pseudorange and integer-free phase measurements. The y-axis is the percentage. (a) The histogram ofposition errors. (b) The histogram of velocity errors. (c) The histogram ofattitude errors.

η should both be near zero when the trajectory estimate is nearany local minimum of the cost function C(X). The fact thatfinal cost reduction ΔCζ is always positive indicates that theoptimization improves accuracy and does not jump to a worselocal minimum (if one exists).



Fig. 8. North and East position error comparison for CRT estimator with andwithout integer-free phase measurements.

Fig. 9. The total number of iterations ζ , the final cost reduction ΔCζ , the finalcost Cζ , and the final value of ‖η‖2 in the optimization.

The final cost Cζ is also given as a reference. In practice, in-consistent GPS data, possibly caused by multi-path or overheadtrees, could explain the large values of Cζ as is shown near time200 sec and 350 sec. For example, Fig. 10 shows the histogramof the twenty-six integer-free phase residuals [see eqn. (42)]over the 10 second CRT window after optimization convergesat time 205 sec. The measurement that has the residual error

Fig. 10. The histogram of the posterior CRT window phase residuals (eqn.(46)) at time 205 sec.

around −0.025 m is suspicious and could be removed by faultdetection methods. Note that the cost in Fig. 9 defined ineqn. (29) weights the residual from Fig. 10 by its inversecovariance, which accounts for the large difference in thescales of the two plots. The opportunity of removing suspiciousmeasurements and recomputing the trajectory estimate is anadvantage of the CRT estimator. Such outlier detection methodsare an interesting topic for future research.

VIII. CONCLUSIONS AND FUTURE WORK

This paper has proposed a novel DGPS/IMU integrationapproach that significantly improves performance, compared toEKF solutions. Such performance improvement is especiallyneeded in urban environments. The new algorithm performsoptimization in real-time, optimally combining all IMU andGPS measurement within a time window, to provide a real-time state estimate at the current time. The approach leads toimproved performance for a few reasons. First, optimizationover the CRT time window provides the capability to re-linearize the system kinematic and measurement models aroundthe improved trajectory estimate. Multiple iterative optimiza-tion steps converge to the minimum of the nonlinear MAPproblem. This provides the ability to estimate attitude andbiases, even yaw, accurately without a magnetometer, oncethey become observable. Second, the large set of residual datawithin each CRT window provides sufficient redundancy toallow the effects of noise to be significantly reduced in theoptimization. In addition, it allows the detection of anomalousmeasurements, by RAIM type techniques, so that they can beeliminated from the measurement set. Those methods have notbeen presented herein, but are an important topic for futurework. Third, the proposed integer-free phase measurement isable to provide accurate local kinematic constraints, withoutresolving the integers, which helps to improve the robustnessto multipath errors and GPS noise, which are common in urbanenvironments.

In addition to presenting the improved approach, this arti-cle has presented and analyzed data from an application of



this method to a test trajectory using L1-only GPS measure-ments. The experimental data analysis demonstrated severalpoints. The CRT navigation system is able to reliably achievesub-meter (often decimeter) positioning accuracy in a GPS-challenged urban environment. The CRT approach calibratesthe attitude and biases rapidly following initial acceleration,which then allows the approach to maintain state accuracythrough periods with few satellites. Use of the integer-freephase measurements in the CRT approach yields a smoothertrajectory than the pseudorange only solution.

There are a variety of possible directions for future work toimprove the performance of the proposed system:

• Develop and demonstrate reliable methods to check themeasurement residuals to reject outliers.

• Develop real-time algorithms that allow correct model-ing of the correlation between satellites in the integer-free measurements. This was ignored in this paper. Oneapproach is to add the receiver clock bias into the statevector to avoid the double-differencing, thus avoidingintroducing the correlation between satellites.

• Develop CRT methods to reliably resolve the integer am-biguity to achieve centimeter positioning accuracy usingsingle frequency receivers. The CRT framework has thepotential to increase the success rate of real-time integerambiguity resolution.

• Construct the optimal integer-free phase tracks Ξ in theCRT window to maximize the information extracted fromphase measurements.

• Test the algorithm using data from a low-cost, single-frequency receiver to evaluate robustness to the noisysignal, and also reception from a low-cost GPS antenna.

• Augment additional states per satellite to model GNSStime correlate measurement errors such as multipath.

APPENDIX

We prove the eqn. (16) in this appendix. The X is the vehicletrajectory, the U is the set of IMU measurements and the Y isthe set of aiding sensor measurements.

p(X,U,Y) (47)

= p(X,U)p(Y | X,U) (48)

= p(X,U)p(Y | X) (49)

= p(X,U)∏

(i,j)∈Yp (yij |x(ti)) (50)

= p (x(t0))k−1∏i=0

p (x(ti+1)|x(ti),Ui)∏

(i,j)∈Yp (yij |x(ti)) .

(51)

Eqn. (48) is obtained by applying the definition of conditionalprobability. Eqn. (49) is obtained by applying the conditionalindependence property. Eqn. (50) is obtained by assuming the

noise affecting the sequence of measurements is independent.Eqn. (51) is obtained by assuming Markov process and timeindependence of process noise.

ACKNOWLEDGMENT

This research builds on the software of and technical conver-sations with Anastasios I. Mourikis and Mingyang Li. Thesetechnical collaborations are greatly appreciated.

REFERENCES

[1] J.-M. Codol, M. Poncelet, A. Monin, and M. Devy, “Safety roboticlawnmower with precise and low-cost L1-only RTK-GPS positioning,”in Proc. IROS Workshop Perception Nav. Auton. Veh. Human Environ.,San Francisco, CA, USA, 2011, pp. 69–72.

[2] W. Stempfhuber and M. Buchholz, “A precise, low-cost RTK GNSSsystem for UAV applications,” in Proc. Conf. Unmanned Aerial Veh.Geomatics, Zürich, Switzerland, 2011, pp. 289–293.

[3] S. Zhao, W. Dong, and J. A. Farrell, “Quaternion-based trajectory trackingcontrol of VTOL-UAVs using command filtered backstepping,” in Proc.IEEE ACC, 2013, pp. 1018–1023.

[4] “ Global Positioning System Standard Positioning Service PerformanceStandard,” U.S. Dept. Def., Washington, DC, USA, Tech. Rep., 2008.

[5] R. A. Snay and T. Soler, “Continuously operating reference station(CORS): History, applications, and future enhancements,” J. SurveyingEng., vol. 134, no. 4, pp. 95–104, 2008.

[6] G. P. and C. E. Fly, “NDGPS Assessement Final Report,” U.S. Dept.Transp., Washington, DC, USA, Tech. Rep., 2008.

[7] E. Kaplan and C. Hegarty, Understanding GPS Principles and Applica-tions, 2nd ed. Norwood, MA, USA: Artech House, 2006.

[8] J. Farrell, T. Givargis, and M. Barth, “Real-time differential carrier phaseGPS-aided INS,” IEEE Trans. Control Syst. Technol., vol. 8, no. 4,pp. 709–721, Jul. 2000.

[9] A. Chen, D. Zheng, A. Ramanandan, and J. A. Farrell, “Near-real-timeGPS integer ambiguity resolution,” in Proc. IEEE CDC/ECC, 2011,pp. 7281–7286.

[10] M. Bahrami and M. Ziebart, “Doppler-aided positioning-improvingsingle-frequency RTK in the urban environment,” in Proc. GPS World,May 2011, pp. 47–56.

[11] C. E. Cohen, “Attitude determination using GPS,” Ph.D. Dissertation,Stanford Univ., Dept. Aeronaut. Astronaut., Stanford, CA, USA,Dec. 1992.

[12] J. Diesel, “GPS/INS Integration for Civil Aviation,” in Proc. NTC, 1991,pp. 223–228.

[13] J. A. Farrell, H. Tan, and Y. Yang, “Carrier phase GPS-aided INS basedvehicle lateral control,” ASME J. Dyn. Syst., Meas., Cont., vol. 125, no. 3,pp. 339–353, 2003.

[14] D. Bevly, J. Ryu, and J. Gerdes, “Integrating INS sensors with GPSmeasurements for continuous estimation of vehicle sideslip, roll, andtire cornering stiffness,” IEEE Trans. Intell. Transp. Syst., vol. 7, no. 4,pp. 483–493, Dec. 2006.

[15] J. A. Farrell, Aided Navigation: GPS With High Rate Sensors.New York, NY, USA: McGraw-Hill, 2008.

[16] R. Hatch, “The synergism of GPS code and carrier measurements,”in Proc. Int. Geodesy Symp. Sat. Doppler Pos., 1983, vol. 1,pp. 1213–1231.

[17] T. Takasu and A. Yasuda, “Development of the low-cost RTK-GPS re-ceiver with an open source program package RTKLIB,” in Proc. Int.Symp. GPS/GNSS, Korea, 2009, pp. 4–6.

[18] X.-W. Chang and T. Zhou, “MILES: MATLAB package for solvingmixed integer least squares problems,” GPS Solutions, vol. 11, no. 4,pp. 289–294, 2007.

[19] R. G. Brown and P. Y. Hwang, “A Kalman filter approach to precisionGPS geodesy,” Navigation, vol. 30, no. 4, pp. 338–349, 1983.

[20] J. Codol and A. Monin, “Improved triple difference GPS carrier phase forRTK-GPS positioning,” in Proc. IEEE SSP, 2011, pp. 61–64.

[21] W. Hedgecock, M. Maroti, J. Sallai, P. Volgyesi, and A. Ledeczi,“High-accuracy differential tracking of low-cost GPS receivers,” in Proc.ACM Inter. Conf. Mobile Syst., Appl., Serv., Taipei, Taiwan, 2013,pp. 221–234.

[22] R. Hatch, “Instantaneous ambiguity resolution,” in Kinematic Systemsin Geodesy, Surveying, and Remote Sensing. New York, NY, USA:Springer-Verlag, 1991, pp. 299–308.



[23] X. Gu and A. Lipp, “DGPS positioning using carrier phase for precisionnavigation,” in Proc. IEEE PLANS, 1994, pp. 410–417.

[24] F. van Graas and S.-W. Lee, “High-accuracy differential position-ing for satellite-based systems without using code-phase measure-ments,” NAVIGATION, J. Inst. Navigat., vol. 42, no. 4, pp. 605–618,Winter 1995/1996.

[25] P. Henkel, “Reliable carrier phase positioning,” Ph.D. dissertation, Tech-nische Universitat Munchen, Munich, Germany, 2010.

[26] F. Dellaert and M. Kaess, “Square root SAM: Simultaneous localizationand mapping via square root information smoothing,” Int. J. Rob. Res.,vol. 25, no. 12, pp. 1181–1203, 2006.

[27] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smooth-ing and mapping,” IEEE Trans. Robot., vol. 24, no. 6, pp. 1365–1378,Dec. 2008.

[28] H.-P. Chiu, S. Williams, F. Dellaert, S. Samarasekera, and R. Kumar,“Robust vision-aided navigation using sliding-window factor graphs,” inProc. IEEE ICRA, 2013, pp. 46–53.

[29] T.-C. Dong-Si and A. I. Mourikis, “Motion tracking with fixed-lagsmoothing: Algorithm and consistency analysis,” in Proc. IEEE ICRA,2011, pp. 5655–5662.

[30] V. Indelman, S. Williams, M. Kaess, and F. Dellaert, “Factor graph basedincremental smoothing in inertial navigation systems,” in Proc. IEEE 15thInt. Conf. Inf. Fusion, 2012, pp. 2154–2161.

[31] S. Zhao, Y. Chen, H. Zhang, and J. A. Farrell, “Differential GPS aidedinertial navigation: A contemplative realtime approach,” in Proc. IFAC,2014, pp. 8959–8964.

[32] P. Misra and P. Enge, Global Positioning System: Signals, Measurementsand Performance, 2nd ed. Lincoln, MA, USA: Ganga-Jamuna Press,2006.

[33] T. Takasu and A. Yasuda, “Evaluation of RTK-GPS performance withlow-cost single-frequency GPS receivers,” in Proc. Int. Symp. GPS/GNSS,2008, pp. 852–861.

[34] P. Cederholm, “Statistical characteristics of L1 carrier phase observationsfrom four low-cost GPS receivers,” Nordic J. Surveying Real Estate Res.,vol. 7, no. 1, pp. 58–75, 2010.

[35] M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” Int. J. Rob. Res., vol. 32, no. 6, pp. 690–711, 2013.

[36] M. Kaess et al., “iSAM2: Incremental smoothing and mapping using theBayes tree,” Int. J. Robot. Res., vol. 31, no. 2, pp. 216–235, Feb. 2012.

[37] R. Eustice, H. Singh, and J. Leonard, “Exactly sparse delayed-state filtersfor view-based SLAM,” IEEE Trans. Robot., vol. 8, no. 6, pp. 1100–1114,Dec. 2006.

[38] M. Kaess and F. Dellaert, “Covariance recovery from a square root infor-mation matrix for data association,” Robot. Auton. Syst., vol. 57, no. 12,pp. 1198–1210, 2009.

[39] M. Li and A. I. Mourikis, “Online temporal calibration for camera-IMUsystems: Theory and algorithms,” Int. J. Robot. Res., vol. 33, no. 7,pp. 947–964, Jun. 2014.

[40] A. Mourikis and S. Roumeliotis, “A multi-state constraint Kalman fil-ter for vision-aided inertial navigation,” in Proc. IEEE ICRA, 2007,pp. 3565–3572.

[41] V. Ila, J. M. Porta, and J. Andrade-Cetto, “Information-based compactpose SLAM,” IEEE Trans. Robot., vol. 26, no. 1, pp. 78–93, Feb. 2010.

[42] A. Vu, A. Ramanandan, A. Chen, J. A. Farrell, and M. Barth, “Real-time computer vision/DGPS-aided inertial navigation system for lane-level vehicle navigation,” IEEE Trans. Intell. Trans. Syst., vol. 13, no. 2,pp. 899–913, Jun. 2012.

[43] S. Zhao and J. A. Farrell, “2D LIDAR aided INS for vehicle positioningin urban environments,” in Proc. IEEE Int. Conf. Control Appl., 2013,pp. 376–381.

[44] A. Vu, J. A. Farrell, and M. Barth, “Centimeter-accuracy smoothed vehi-cle trajectory estimation,” IEEE Intell. Transp. Syst. Mag., vol. 5, no. 4,pp. 121–135, winter 2013.

Sheng Zhao received the B.Eng. and M.S. degrees inautomation from Xiamen University, Xiamen, China,in 2005 and the Ph.D. degree in electrical engineer-ing from the University of California, Riverside, CA,USA, in 2014. He is currently a Software Engineerat Google Inc., Mountain View, CA. His currentresearch interests include aided inertial navigationsystems for robots, autonomous vehicles and mobiledevices, state estimation, and simultaneous localiza-tion and mapping.

Yiming Chen received the B.Eng. degree in au-tomation from Tianjin University, Tianjin, China, in2009 and the M.S. degree in electrical engineering,the M.S. degree in mathematics, and the Ph.D. de-gree in electrical engineering from the University ofCalifornia, Riverside, CA, USA, in 2011, 2013, and2014, respectively. He is currently a Senior ResearchEngineer with Qualcomm Research, San Diego, CA.His current research interests include sensor fusion,state estimation, optimization, computer vision, ma-chine learning and automatic control, particularly

their applications in autonomous vehicles and robotics.

Jay A. Farrell received the B.S. degrees in physicsand electrical engineering from Iowa State Univer-sity, Ames, IA, USA, and the M.S. and Ph.D. de-grees in electrical engineering from the University ofNotre Dame, Notre Dame, IN, USA. After workingat Charles Stark Draper Lab (1989–1994), he joinedthe University of California, Riverside, CA, USA,where he is currently a Professor and a two-timeChair of the Department of Electrical and ComputerEngineering. He has authored over 250 technicalarticles and three books. Prof. Farrell is a Fellow

of the IEEE and AAAS and a Distinguished Member of IEEE CSS. He hasserved the IEEE Control Systems Society on their Board of Governors for twoterms (’03–’06, ’12–’14), as Vice President of Finance and Vice President ofTechnical Activities, as General Chair for IEEE CDC 2012, and as President in2014. He was named a GNSS Leader to Watch for 2009–2010 by GPS WorldMagazine in May 2009.

ieee transactions on intelligent...

Documents