Proceedings of the 11th International IEEE

Conference on Intelligent Transportation Systems

Beijing, China, October 12-15, 2008

Improved Filtering-Smoothing Algorithm for GPS Positioning

Yi Cao, Xuchu Mao
Department of Instrument Science & Engineering, Shanghai Jiao Tong University
Shanghai, 200240, China

Abstract—The well-known Unscented Kalman Filter (UKF) is
widely applied to nonlinear system, while smoothing algorithm
shows advantage of accuracy improvement in post processing
application. But conventional filter is harassed by roundoff
error due to processor’s finite-wordlength in practice. This
paper proposes an improved filtering-smoothing algorithm
which replaces UKF with Square-Root UKF(SR-UKF) in the
forward filtering pass. Two smoothers, fixed-interval smoother
and fixed-lag smoother, are incorporated in the backward
smoothing pass respectively to form two iterative filter-
ing-smoothing algorithms for GPS positioning estimation. Sys-
tem model is addressed first, then SR-UKF and smoother im-
plementations are described respectively, effectiveness of new
algorithm is evaluated by analyzing experiment results, future
work will also be discussed
I. INTRODUCTION
In GPS positioning application, Extended Kalman Filter
(EKF) [1] and UKF [2] have been proved to be effective
approaches for positioning estimation. A widely used kalman
filter model for GPS positioning estimation has been pro-
posed by [1]. It is necessary to linearize the highly nonlinear
pseudorange equation before applying EKF to it. Unlike EKF,
tem first; then briefly describes SR-UKF and smoother algo-
rithms, two filtering-smoothing combinations are introduced;
finally experimental results are presented and analyzed.
For stand-alone GPS receiver, system noise contains
non-white components. [2] has systematically modeled these
components and given explanations in detail, so following
sections would base discussion on its model and verify the
validity of new algorithms.
II. GPS POSITIONING MODEL
Generally, position and velocity in three-dimensional
format are user’s major concern, so state vector of GPS sys-
tem model basically contains the following components:
position coordinates and velocity coordinates in WGS-84
ECEF coordinate system; GPS time clock offset cδ t and its
shift cδ t ; bias between Doppler shift and rate of change of
pseudorange δ R . Besides, there is non-white error ε k for
each channel. Subscript k indicates kth channel. All errors
are assumed to be independent to each other, so the system
state vector is expressed as
i i i

UKF provides a derivative-free alternative, by carefully xt = [ X t , X t , Yt , Y t , Z t , Z t , cδ tt , cδ tt , δ Rt ,

(1)
choosing a set of sigma points to simulate the distribution ε1t , ε 2t , ε nt ]
characteristics of Gaussian Random Variable (GRV), GRV
where subscript t refers to tth epoch. Vector dimension at
could be transmitted directly by nonlinear function. UKF
each epoch would change in accordance with satellites in
avoids linearization errors and explicit derivation of Jaco-
view. Therefore the dimensions of state transition matrix and
bians, positioning accuracy is consequently improved. An
measurement equation are time variant. The state transition
iterative algorithm employing Unscented Kalman Smoother
equation is given by
(UKS) was addressed in [3], higher accuracy was achieved.
However, the complicated implementation of existent T2
X t +1 = X t + TX t + δ X t
iterative filtering-smoothing algorithm bolsters the risk of 2
computational failure. Two problems need to be solved be- X t +1 = X t + T δ X t
fore it can be used in practice. One is accumulative roundoff
error, which can result in divergence of filter; the other is T2
Yt +1 = Yt + TYt + δ Yt
weak correlation between epochs. Estimation of each epoch 2
only relates to that of previous epoch, the navigation results Yt +1 = Yt + T δ Yt
are thereby sensitive to disturbance at each epoch.
T2
It is a natural idea to build a coherent connection at series Z t +1 = Z t + TZ t + δ Zt (2)
of estimations, which makes the estimations more resistant to 2
disturbance. Smoothing can achieve this goal. Theoretically, Z t +1 = Z t + T δ Z t
smoothing applies backward filtering to forward filtering cδ tt +1 = cδ tt + Tcδ tt + cw1t
results to achieve expectation maximi
cδ tt +1 = cδ tt + cw2t
zation, thus it generally yields better result than applying
forward filtering alone. δ Rt +1 = δ Rt + wδ R
This paper presents an improved algorithm of iterative where T is the interval. δ X t , δ Yt , δ Z t are modeled as sum of
filtering-smoothing. It replaces UKF with SR-UKF in stan-
receiver acceleration and other system disturbances, they are
dard UKS implementation, effectively addressing the roun-
doff error problem. Besides, fixed-lag smoother is also used handled as process noise. w1t and w2t are driving noise for
to replace the fixed-interval smoother for tentative purpose. clock bias. wδ R is the white noise associated with δ R .
The following sections will model the GPS positioning sys-

1-4244-2112-1/08/$20.00 ©2008 IEEE 857

Authorized licensed use limited to: University of Gavle. Downloaded on July 01,2021 at 13:30:08 UTC from IEEE Xplore. Restrictions apply.
 For each satellite, the non-white component could be for t ∈ {1,L ,∞}
modeled using first-order Gaussian-Markov process since 2. Sigma point calculation and time update:
system errors are independent: χ t-1 = [ xˆt −1 xˆt −1 + γ St xˆt −1 − γ St ]
ε i ( t +1) = αε it + β wcit i = 1, 2 , n (3)
χ * = Φ[ χ t-1 , wt −1 ]
α and β are given by [1]: t|t-1

2L

α=
2τ − 1
β=
2τ
(4) xˆt− = ∑ Wi ( m ) χ *t|t-1
2τ + 1 2τ + 1 i =0

where τ =100, wcit is the white noise S = qr{[ W1c ( χ1:2L,t|t-1

− *
− xˆt− )] Q}
t
Measurement equation includes pseudorange observation
S = cholupdate{S , χ
− − * −
− xˆ , W } (c )
and Doppler shift observation. For brevity, we stipulate that t t 0,k t 0

the satellite clock error, ionospheric delay and tropospheric χ t|t-1 = [ xˆt− xˆt− + γ St− xˆt− − γ St− ]
delay are already compensated by preprocessing the raw data, Yt|t-1 = C[ χ t|t-1 ]
so measurement equation can be simplified as:
2L
ρit = rt − rit + cδ t + ε it + vit yˆ t− = ∑ Wi ( m )Yi ,t |t −1
i =0
= ( X t − X it ) 2 + (Yt − Yit ) 2 + ( Z t − Z it ) 2 (5)
3. Measurement update equations:
+ cδ t + ε it + vit
Sŷt = qr{[ W1c [Y1:2 L ,t − yˆ t− ] R ]}
i i i i
Dit = [( X t − X it )( X t − X it ) + (Yt − Yit )(Yt − Yit ) + Syˆ t = cholupdate{Syˆ t Y0,t − yˆ −
W0( c ) }
i i i
(6) t

( Z t − Z it )( Z t − Z it )] / rt − rit + c δ t + δ Rt + ς it 2L
Pxt yt = ∑ Wi ( c ) [ χ i,t|t-1 − xˆt− ][Yi ,t |t −1 − yˆt− ]T
where ρit is the pseudorange for satellite i at tth epoch, i =0

X t , Yt , Z t are receiver position coordinates, X it , Yit , Zit are κ t = ( Px y / STyˆ ) / Syˆ

t t t t

ith satellite position coordinates, rt and rit are position xˆt = xˆ + κ t ( yt − yˆ )

vectors of receiver and satellite respectively, vit is measure-
ment white noise of ith channel. Di is the Doppler shift
caused by relative motion between receiver and ith satellite.
ς it is white noise like vit .
When there are more than 3 visible satellites, the mea-
surement equation can be expressed as:
zt = [ ρ1t , ρ 2t , ρ nt , D1t , D2t Dnt ]T (7)
III. UNSCENTED KALMAN FILTER IN SQUARE ROOT FORM
Standard UKF features direct GRV propagation without
linearizing a nonlinear model [4]. It provides an excellent
solution for nonlinear system, but it is susceptible to possible
divergence of covariance matrices due to accumulative
roundoff errors in the transition process.
To guarantee that posteriori covariance matrix Pt |t is non-
negative-definite, its square root St |t is used in the UKF
transition directly. An important point to note is that
Pt |t = St |t StT|t is always positive-definite, St |t can be obtained
by performing Cholesky factorization. Updating S t |t to cal-
culate sigma points efficiently avoid refactorizing Pt |t at each
measurement update, thus SR-UKF implementation is about
20% faster than standard UKF. For integrality, full imple-
mentation of SR-UKF is listed below. Detailed discussion
could be found in [5].
TABLE I
SQUARE-ROOT UNSCENTED KALMAN FILTER ALGORITHM
1. Initialize with:
xˆ0 = E[ x0 ] S0 = chol{E[( x0 − xˆ0 )( x0 − xˆ0 )T ]}
− −
t t
U = κ t Sŷt
St = cholupdate{St− U −1}
Shorthand notation qr{·}means QR decomposition;
chol{·} means Cholesky factorization;
cholupdate{·} means Cholesky factor update;
Q, R are process noise covariance and measurement
noise covariance.
IV. GPS POSITIONING ESTIMATION BASED ON ITERATIVE
FILTERING AND SMOOTHING
A. Optimal linear smoothing algorithm
For linear system in which system noise complies with
Gaussian-distribution, given a series of observations { y1 , y2
L yN } , optimal smoothing is to determine the optimal linear
estimation of state variable xt that maximizes its likelihood,
where 1 < t < N , N is the length of interval. There are three
smoothers in terms of the way they function:
a) Fixed-Interval smoother: observation range is fixed,
hence N is a constant. The goal is to determine a set of
optimal state xt in terms of the series of observations,
subscript t varies in [1, N − 1] .
b) Fixed-Point smoother: subscript t is fixed, while the
observation range N would changes. This smoother is
especially useful when states at certain epochs are criti-
cal.
c) Fixed-Lag smoother: both t and N are variant. Smooth-
ing point lags behind the observation point at a fixed

858

Authorized licensed use limited to: University of Gavle. Downloaded on July 01,2021 at 13:30:08 UTC from IEEE Xplore. Restrictions apply.
 interval N − t = Δ . The smoother functions as a window Fixed-interval smoother formula roots in linear model but
moving along the time axis, with filtering operated si- is applicable to nonlinear model as well [6]. In order to im-
multaneously with the smoothing but prior to smooth- plement the smoothing algorithm, the state transition matrix
ing Δ epochs,. and state vector estimation need to be saved at forward fil-
Given that civilian GPS receiver generally yields naviga- tering pass. In fixed-interval smoother, filtering and smooth-
tion results once per second, fixed-interval smoother and ing operate in asynchronous manner, backward smoothing
fixed-lag smoother are more suitable for continuous posi- can only be executed after forward filtering. But in fixed-
tioning. Thus the following sections will focus on these two interval smoother, filtering and smoothing are manipulated
smoothers. simultaneously. Filtering is operated at epoch N while
Backward smoothing algorithm could be obtained by smoothing is operated at epoch t . Fig.1 illustrates the way
solving the differential equation: they work.
∂f ( xt , xt +1 | y1 , y2 L y N )
=0 (8) Filtering Pass
∂xt
where f ( xt , xt +1 | y1 , y2 L y N ) is the conditional probability
function complying with Gaussian-distribution. Complexity t-1 t t+1 N-1 N
of resolution could be simplified when dealing with linear Smoothing Pass
discrete-time model. Set the discrete-time dynamic system
model: (a) Fixed-interval smoother
⎧⎪ xt +1 = Φ t +1,t xt + Γ t +1,t wt
⎨ (9)
⎪⎩ yt +1 = Ct +1 xt +1 + vt +1 Smoothing point Filtering point
where t = 0,1, 2,K N − 1 , xt is the state vector, yt +1 is the ob-
∆
servable vector. wt , vt +1 are system process noise and mea-
surement noise respectively and assumed to be Gaussian.
Q , R are covariance matrices of wt and vt +1 respectively, t t+1 t+2 t+N t+1+N
Φ t +1,t is the state transition matrix, Ct +1 is the measurement (b) Fixed-lag smoother

matrix. the recursive formulas of fixed-interval smoother and Fig. 1. Fixed-interval smoother and fixed-lag smoother
fixed-lag smoother are as following:
Fixed-lag smoother’s formula seems more complicated than
(1) fixed-interval smoother
that of fixed-interval smoother. However, fixed-lag smoother
⎧xˆt|N = xˆt|t + Ast [xˆt +1|N −Φt +1,t xˆt|t ] presents superiority when sample points increase. Because it
⎪⎪ T
⎨Pt|N = Pt|t + Ast [Pt +1|N − Pt +1|t ]As t t < N, t = 1,2LN −1 (10) avoid executing smoothing algorithm point by point, there-
⎪ T −1
fore the overall computational cost is less than fixed-interval
⎪⎩ Ast = Pt|t Φ t +1,t P t +1|t smoother. In addition, in fixed-lag smoother implementa-
where xˆt |t , Pt |t , Pt +1|t refer to state vector, covariance matrix tion, Δ + 1 points’ information are incorporated into the solu-
tion when computing xˆt +1 t +1+ N , thus the smoothed estimation
and predicted covariance matrix respectively. Pt +1|t comes
from forward filtering pass. Ast is optimal smoothing gain are expected to be more coherent.
matrix, the boundary condition are xˆ N | N , PN | N . B. Iterative filtering-smoothing algorithm based on
SR-UKF
(2) fixed-lag smoother
⎧ Conventional applications perform one pass forward fil-
⎪ xˆt +1 t +1+ N = Φ t +1,t xˆt t + N + Bt +1,t +1+ N [ xˆt +1+ N t +1+ N − tering and one pass backward smoothing, the outcome is
⎪ thereby more precise than taking filtering alone since many
⎪ xˆt +1+ N |t + N ] + Gt [ xˆt t + N − xˆt t ]
⎪ errors have been removed. If the smoothed estimation is taken
⎪ Pt +1|t +1+ N = Pt +1|t + Ast−1 [ Pt |t + N − Pt |t ] Ast− T − as refined initial value for filter again, then more precise
⎪⎪ (11) output is expected to achieve. Base on the above idea, [3]
⎨ Bt +1, t +1+ N K t +1+ N Ct +1+ N Pt +1+ N |t + N B T t +1, t +1+ N
⎪ t +1 + N
applies UKS to adjacent points in iterative manner, which
⎪B
⎪ t +1, t +1+ N
= ∏ Asi = Ast−1 Bt , t + N Ast + N basically combines UKF and fixed-interval smoother in one
i = t +1 implementation. Fixed-interval smoother is derived from
⎪
⎪ Gt = Γ t +1, t Q Γ Tt +1, k Φ − T t +1, t Pt |t −1 linear model. In case of GPS modeling, measurement equa-
⎪ tions are highly nonlinear, combination of the UKF and
⎪⎩ Ast = Pt |t Φ T t +1, t P −1t +1|t
fix-interval smoother may arouses concern over its validation.
where t = 0,1, 2L , xˆt |t , Pt |t . Pt +1|t , Ast have the same mean- However, [6] proves that such combination is valid by spe-
ing as fixed-interval smoother, boundary condition are cific mathematical proof and experiments.
xˆ0 N , P0| N .

859

Authorized licensed use limited to: University of Gavle. Downloaded on July 01,2021 at 13:30:08 UTC from IEEE Xplore. Restrictions apply.
 SR-UKF with δ X&&t2 = δ Y&&t 2 = δ Z&&t2 = 102 , which are the maximum ac-
xˆt −1|t −1 xˆt|t
celeration. E{wδ R wδ R } = σ wδ R 2 = 20 2 is obtained from statis-
Pt −1|t −1 Iteration Pt |t tics by receiver test. E{wcit wcit } = (0.3URA) 2 , URA is short
for User Range Accuracy, which is obtained from navigation
k-1 data, typically 4 without SA policy [1]. Other parameters are
defined in [3].
xˆt −1|t xˆt |t ⎡σν21 ⎤
Pt −1|t Smoothing Pass Pt|t ⎢ ⎥
⎢ ⎥
Fig. 2. Iterative filtering-smoothing process ⎢ σν21 ⎥
R=⎢ ⎥ (15)
⎢ σ ς21 ⎥
UKF is superior to EKF in positioning accuracy, but both ⎢ ⎥
are susceptible to accumulative roundoff error[7]. In contrast ⎢ ⎥
with UKF and EKF, SR-UKF requires only half of word ⎢⎣ σ ς 1 ⎥⎦
2

length that UKF and EKF need when computing covariance

matrices, thus UKF in square root form is more robust. So we
employ SR-UKF as forward filter and incorporate fixed
-interval smoother and fixed-lag smoother with it respectively
to form two positioning algorithms, denoted as SR-UKFS-
Iterative and SR-UKFS-Window. Fig.2 illustrates the solu-
tion.
Note that in equation (11), measurement matrix Ct +1 is
needed to calculate the covariance matrix, but pseudorange
equation and Doppler shift equation are highly nonlinear, so
it’s impossible to calculate the smoothed covariance matric-
es Pt +1|t +1+ N . Fortunately, calculating xˆt +1 t +1+ N does not need
Ct +1 , thus smoothing could be operated at least one pass.
where E{vi vi } = σν2i = 102 and E{ς iς i } = σ ς21 = 1 , the cova-
riance of measurement noise are obtained by statistics from
experiments.
Fig. 3 and Fig.4 represent the 3-D position estimation of
SR-UKFS-window and SR-UKFS-iterative respectively,
compared with RTK GPS reference and UKF. SR-UKFS-
iterative has performed 20 iterations, SR-UKFS-window
have an interval of 21 (N=21), table 1 presents comparison of
different algorithms’ root mean square root (RMSE).

V. EXPERIMENT RESULTS AND ANALYSIS

This paper verifies proposed algorithms by carrying out
static GPS positioning experiments, which consists of Su-
perstar GPS OEM receiver, a low-cost GPS antenna of CMC,
PC. We took output of Trimble MS750 RTK-GPS receiver as
reference, because it has high positioning precision (in fix
mod, <3cm). Superstar GPS OEM receiver receives raw data
and decodes navigation data, PC collects these data and
performs post processing. Two algorithm results are com- Fig. 3. Comparison of 3-D Position Errors between SR-UKFS-Window
pared with output given by Superstar and estimations given and UKF
by using UKF alone.
In static experiment, there were average 7 satellites in
view. 300 points were collected. The sampling interval was
1s. We use receiver’s position output as the initial position
value for the filter, this could help accelerating convergence
of filter:
Before proceeding, two matrices are critical to initialize, Q
and R. Since all error sources are assumed independent, thus:
⎡δ X&&t2 ⎤
⎢ &&2 ⎥
⎢ δY t ⎥
⎢ &&
δ Zt2 ⎥
⎢ ⎥
⎢ Q11 Q12 ⎥ (14)
Q = E{wwT } = ⎢ Q21 Q22 ⎥
⎢ ⎥
⎢ σwδR 2
⎥
⎢ 2 ⎥
⎢ (0.3URA) ⎥
⎢ O ⎥ Fig. 4. Comparison of 3-D position errors between SR-UKFS-Iterative and
⎢ ⎥ UKF
⎢⎣ (0.3URA)2 ⎥⎦

860

Authorized licensed use limited to: University of Gavle. Downloaded on July 01,2021 at 13:30:08 UTC from IEEE Xplore. Restrictions apply.
 TABLE II
COMPARISON OF DIFFERENT METHOD’S POSITION RMSE
RMSE/m
X Y Z 3-D
Superstar Receiver 2.3052 3.4641 2.9314
UKF 1.8275 3.3948 3.0701
SR-UKFS-Window 1.8067 3.2479 2.8219
SR-UKFS-Iterative 1.3792 2.7111 2.0325
Fig.3 shows that the trend of SR-UKFS-window is similar
to that of UKF. But SR-UKFS-window involves in 21 points’
information at each sample, the curve is smoother than that of
UKF or SR-UKFS-Iterative. By carefully examining the
curve, we found that a ‘faultage’ presents at Z direction. It
actually indicates a change on visible satellite number. The
cause of ‘faultage’ is due to inconsistent matrix dimension.
Equation (10) requires all points involving in should have
identical dimension. But matrix dimensions change in re-
sponse to visible satellite number change. So the formula
could not be implemented any more, conventional SR-UKF
will be used instead. Hence the curve would ‘jump’ to that of
UKF. A pragmatic strategy is to skip the particular sample
and continue to use estimation of previous epoch until the
point move out the window interval, but it does not guarantee
accuracy in case the satellite number change frequently.
Fig.4 shows that the curve of SR-UKFS-Iterative is closer
to RTK GPS reference than that of UKF. Table 1 indicates
that SR-UKFS-Iterative achieves least RMSE among other
algorithms. It’s an intuition to increase the iterations and
window interval to pursue higher accuracy, Fig.5 and Fig.6
present the results of the tentative suggestion.
Fig. 5. Comparison as iteration number increase
Fig. 6. 3-D RMSE vary with different iteration and window interval
Authorized licensed use limited to: University of Gavle. Downloaded on July 01,2021 at 13:30:08 UTC from IEEE Xplore. Restrictions apply.
Positioning precision is conspicuously improved when
iteration number increase from 10 to 40. However, it is ob-
vious that a trade-off exists between precision and computa-
tional cost. Besides, precision is not improved constantly as
5.0899 iteration number or window interval increase. This is partly
due to the averaging effect of smoother. For each individual
4.9285
sample, estimation is partly refined from information of its
4.6665 adjacent points, the more frequently the smoothing is im-
3.6583 plemented, the more influence it would get from adjacent
points. Although the process helps to reduce white noise as a
whole, it possibly causes divergence from true position for
individual sample.
VI. CONCLUSION AND FUTURE WORK
This paper has presented implementations of SR-UKF and
two smoothers, combining them to form two improved fil-
tering-smoothing algorithm for GPS positioning estimation.
Experimental results show that proposed algorithms achieve
higher accuracy than applying UKF alone, they effectively
reduce the risk of divergence. Appropriate iteration number
and window interval are also suggested in diagram. Moreover,
their limits are also discussed. The SR-UKFS-iterative are
suitable for real-time positioning in that it carries out iteration
only between adjacent points, and SR-UKFS-window could
be used for post processing purpose.
Future work includes research on fixed-lag smoother.
Theoretically, it requires less computation work when time
interval is long. But a drawback is that linear measurement
matrix is needed, which hinder it from applying to nonlinear
model, linearizing the measurement equation is not accepta-
ble, because it would undermine our original intention of
employing UKF as forward filter.
REFERENCES
[1] Simon Cooper and Hugh Durrant-Whyte, “A Kalman Filter Model for
GPS Navigation of Land Vehicles”, IEEE Conference on Intelligent
Robots and Systems (IROS’94) Munich, Germany, 1994 9:157- 163
[2] Mao X, Wada M. Hashimoto H. “Nonlinear GPS Models for Position
Estimate Using Low-cost Receiver”, The IEEE 6th Intelligent Trans-
portation System Conference, Shanghai, China, 2003: 637-642
[3] Mao X, Wada M. Hashimoto H. “Nonlinear Iterative Algorithm for
GPS Positioning with Bias Model”, 2004 IEEE Intelligent Transpor-
tation Systems Conference Washington, D.C., USA, October 3-6,
2004
[4] Haykin S. Kalman Filtering and Neural Networks. USA: Wiley
Publishing, 2001:221-240
[5] Van der Merwe R, Wan E A. “The Square- root Unscented Kalman
Filter for state and parameter estimation”, IEEE International Confe-
rence on Acoustics (Speech and signal Processing Proceedings), Salt
Lake City, Uta h , 2001(3):3461-3464
[6] Psiaki, M.L. Derivation and Simulation Testing of a Sigma-Points
Smoother. Journal of guidance, control, and dynamics, Keystone,
Colorado, August 21 - 24 2006
[7] Psiaki, M.L. “Backward-Smoothing Extended Kalman Filter”, Journal
of Guidance, Control, and Dynamics. 2005, Vol.28, No.5: 885-899
861