IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 1

Mobile Localization in Non-Line-of-Sight UsingConstrained Square-Root Unscented Kalman FilterSiamak Yousefi, Student Member, IEEE, Xiao-Wen Chang, and Benoit Champagne, Senior Member, IEEE

Abstract—Localization and tracking of a mobile node (MN)in non-line-of-sight (NLOS) scenarios, based on time of arrival(TOA) measurements, is considered in this work. We developa constrained form of square root unscented Kalman filter(SRUKF), where the sigma points of the unscented transforma-tion are projected onto the feasible region by solving constrainedoptimization problems. The feasible region is the intersectionof several discs formed by the NLOS measurements. We showhow we can reduce the size of the optimization problem andformulate it as a convex quadratically constrained quadraticprogram (QCQP), which depends on the Cholesky factor of thea posteriori error covariance matrix of the SRUKF. As a result ofthese modifications, the proposed constrained SRUKF (CSRUKF)is more efficient and has better numerical stability compared tothe constrained UKF. Through simulations, we also show that theCSRUKF achieves a smaller localization error compared to othertechniques and that its performance is robust under differentNLOS conditions.

Index Terms—Constrained Kalman filter, convex optimization,localization, non-line of sight.

I. INTRODUCTION

NETWORK-based radio localization has received great at-tention in recent years due to limitations of the global po-

sitioning system (GPS) in indoor places and dense urban areas,and now finds numerous applications in surveillance, security,etc. [1]. In this technology, radio signals exchanged between amobile node (MN) and fixed reference nodes (RN) with knownpositions1, are exploited to determine the unknown location ofthe MN. Several different types of measurement can be usedfor localization, e.g., time of arrival (TOA), time differenceof arrival (TDOA), received signal strength (RSS), angle ofarrival (AOA), and a hybrid of these. Amongst the differentlocalization techniques, TOA-based methods in which the MNand RNs are synchronized are usually preferred, especially inthe context of IEEE 802.15.4a which exploits ultra wideband(UWB) technology [2]. Indeed, measurement of TOA can bedone accurately with UWB signalling due to its fine timingresolution and robustness against multipath and fading.One of the main challenges in radio localization is the

non-line of sight (NLOS) problem, which occurs due to theblockage of the direct sight between the MN and RNs. In anNLOS situation, either due to reflection of the radio wavesby scatterers or penetration through blocking objects, the

S. Yousefi and B. Champagne are with the Department of Electrical andComputer Engineering, McGill University, Montreal, QC, Canada, H3A 0E9.E-mail: siamak.yousefi@mail.mcgill.ca, benoit.champagne@mcgill.ca.X.W. Chang is with the School of Computer Science, McGill University,

Montreal, QC, Canada, H3A 0E9. E-mail: chang@cs.mcgill.ca1In wireless cellular networks, the RN are identified with the base stations,

while in wireless sensor networks, they are called anchors.

travel time of the received signals increases [3], [4], [5].Consequently, the NLOS error of each measured TOA needsto be modelled as a random variable with a positive bias,which can be quite large [6]. The first step in dealing with theNLOS problem is to detect the NLOS measurements and, ifnecessary, discard them. To this end, some techniques estimatethe variance of each measurement, and if it is above a giventhreshold, the corresponding link is identified as NLOS [7],[8], [9]. NLOS identification techniques using signal featureshave also been proposed for UWB applications [4], [10], [5].If, however, the NLOS measurements cannot be discardeddue to an insufficient number of LOS measurements forunambiguous localization, the next step is to mitigate theireffect through further processing.There are numerous works focusing on NLOS mitigation

for the localization of stationary nodes, which are mostlybased on (memoryless) constrained optimization techniques,e.g. [11], [12]. In these approaches, the position of the MNis constrained to be within the convex hull formed by theintersection of multiple discs, each disc being centered at oneof the NLOS RNs and with a radius equal to the correspondingmeasured range. By restricting the MN position in this wayand by employing the LOS measurements in the cost functionto be minimized, the unknown location can be found throughsolving a constrained optimization problem. For a survey onTOA-based memoryless localization in NLOS scenarios, see[6] and the references therein.For an MN with available dynamic model, filtering tech-

niques are preferred compared to memoryless methods. Thisis especially the case when data from inertial measurementsunits (IMU) are used in parallel with range information fortracking purposes [13], [14]. Some methods apply Kalmanfilter preprocessing on measured TOAs to smooth out theeffect of the variances of the NLOS biases, while scalingthe covariance matrix in an extended Kalman filter (EKF) tofurther mitigate the effect of their means [7], [8], [9]. However,these approaches can only achieve a moderate performancefor large NLOS biases. In [15], [16], it is assumed thatthe mean and variance of the NLOS biases are known; inpractice, however, this information is not available accuratelybeforehand unless prior field measurements are obtained.Some other approaches regard the NLOS bias as a nuisance

parameter and try to estimate its distribution using Kerneldensity estimation (KDE) techniques. In [17], a robust semi-parametric EKF is proposed for NLOS mitigation of anMN. The performance of this technique is improved by theinteracting multiple model (IMM) algorithm in [18]. Althoughconsidered for TOA measurements, these techniques are also

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 2

suitable when AOA, RSS or a hybrid of these are employed.However, in addition to high computational cost, the perfor-mance of KDE still depends on how well it can model thedistribution of the NLOS biases. It is claimed that for cellularapplications, the performance is only satisfactory when theratio of NLOS to LOS measurements is less than a half anda higher ratio might result in divergence of KDE algorithms[18].In some other techniques, the random NLOS biases are

considered as parameters in the state vector, to be jointlyestimated with other state parameters [19], [20], [21], [22],while the NLOS bias variation over time is modelled as arandom walk. The technique in [19] uses EKF, while [20] and[21] use particle filters (PFs) that generally have a high compu-tational cost. In [22], an improved EKF is used where boundconstraints on the NLOS biases are enforced for improvingthe localization accuracy. Although the above techniques canmitigate the effect of NLOS biases to some extent, theirperformance might not be good due to the mismatch betweenthe random walk model and the physical reality, which isunavoidable considering the unpredictable nature of the biases.Furthermore, by including the biases in the state vector, thecomputational cost of the filter grows noticeably [17].In this work, we propose an efficient square root unscented

Kalman filter (SRUKF) with convex inequality constraints forlocalization of an MN in NLOS situations. The proposedconstrained SRUKF (CSRUKF) is based on a combinationof the SRUKF in [23] for unconstrained problems and theconstrained UKF in [24]. In our proposed algorithm, similarto some memoryless approaches, the NLOS measurements areremoved from the observation vector and are employed insteadto form a closed convex constraint region [6]. At each timestep, we use a SRUKF to estimate the state vector and computethe Cholesky factor of the error covariance matrix. To imposethe constraints onto the estimated quantities, as proposed in[24], the sigma points of the unscented transformation mayneed to be projected onto the feasible region by solving aconvex quadratically constrained quadratic program (QCQP).However, we show that the projection can be done in a moreefficient and numerically stable way by solving a QCQPwith reduced size, in which the cost function depends on theCholesky factor of the a posteriori error covariance matrix,readily obtained from the SRUKF. Through simulations, ourproposed algorithm is shown to achieve a good localizationperformance under different NLOS scenarios. In particular, insevere NLOS conditions and with small measurement noises,our method achieves a superior performance compared toother benchmark approaches. Another salient advantage is itsrobustness to false alarm (FA) errors2 in NLOS identification,which makes it suitable for practical applications where sucherrors may be inevitable.The organization of the paper is as follows: In Section II,

the system model is described and the problem formulationis presented. The proposed constrained SRUKF algorithm isdeveloped in Section III, along with a discussion of compu-2In this work, a false alarm refers to the erroneous identification of an LOS

link as being NLOS, while a missed detection (MD) refers to the oppositesituation.

tational complexity. The simulation results and comparisonswith different algorithms are given in Section IV. Finally,Section V concludes the paper.Notation: Small and capital bold letters represent vectors

and matrices, respectively. The vector 2-norm operation isdenoted by ‖ · ‖, and (·)T and (·)−1 stand for matrix trans-pose and inverse operations, respectively. A diagonal matrixwith entries x1, . . . , xM on the main diagonal is denoted bydiag(x1, . . . , xM ). For i ≤ j, q(i : j) denotes a vector ofsize j − i + 1 obtained by extracting the i-th to j-th entriesof vector q, inclusively. The symbol I denotes an identitymatrix of appropriate dimension. For a positive semi-definiteHermitian matrix R, R1/2 denotes its unique positive semi-definite square root matrix, i.e. such thatR1/2R1/2 = R [25].

II. SYSTEM DESCRIPTION AND PROBLEM STATEMENTA. System ModelConsider a network of M fixed RNs and one MN, dis-

tributed on a 2-dimensional (2D) plane and exchanging timingsignals via wireless links. With reference to a Cartesiancoordinate system in this plane, let ai ∈ R

2 denote theknown position vector of the i-th RN, where i ∈ {1, . . . ,M},while xk ∈ R

2 and vk ∈ R2 denote the unknown position

and velocity vectors of the MN at discrete time instant k,respectively. Let the state vector be sk = [xT

k ,vTk ]

T ∈ R4,

which includes the position and velocity components of theMN. The motion model is assumed to be a nearly-constantvelocity model as

sk = Fsk−1 +Gwk−1, (1)

where the matrices F and G are

F =

⎡⎢⎢⎣1 0 δt 00 1 0 δt

0 0 1 00 0 0 1

⎤⎥⎥⎦ , G =

⎡⎢⎢⎣

δt2

2 0

0 δt2

2δt 00 δt

⎤⎥⎥⎦ , (2)

and δt is the time step duration. The vector wk−1 ∈ R2 in

(1) is a zero-mean white Gaussian noise process (acceleration)with diagonal covariance matrix Q = σ2

wI.In this work, we consider TOA-based localization, in which

the range between the MN and each RN is obtained bymultiplying the time of flight of the radio wave by the speedof light. If the MN and RNs are accurately synchronized, thena one-way ranging scheme can be used; otherwise, a two-wayranging protocol may be employed where the relative clockoffsets are removed from the TOA measurements [26]. Let Lk

and Nk denote the index sets of the RNs that are identifiedas LOS and NLOS nodes at time instant k, respectively.The range measurements can thus be represented by vectorrk ∈ R

M with components

rik =

{hi(sk) + ni

k, i ∈ Lk,

hi(sk) + bik + nik, i ∈ Nk,

(3)

where hi(sk) = ‖xk − ai‖, nik is the measurement noise

and bik is a positive random NLOS bias, which is usuallyconsidered independent from ni

k. The noise terms nik, for

i ∈ {1, . . . ,M}, are modelled as independent white Gaussian

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 3

processes, with zero-mean and known variance σ2n > 0. The

probability distributions of the biases bik are time-varying dueto the movement of the MN and other objects in the area. In theliterature, different distributions have been considered for thebiases, for instance: exponential [27], [28], shifted Gaussian[16], and uniform [4] are widely employed. However, having apriori knowledge about the distributions of the NLOS biasesrequires preliminary field measurements, which may not bepossible in practical applications. Therefore, in this work, wedo not make any specific assumption about the distributions ofthe NLOS biases, although we suppose that the NLOS linksare identified accurately at every time instant3.The processing of range measurements for NLOS identifi-

cation and mitigation can either be done at the MN or at afusion center connected to the RNs. The former is used in theMN self-localization applications, while the latter is of interestto target tracking applications.

B. Problem FormulationThe state vector sk and the NLOS biases bik for i ∈ Nk

are the unknown parameters in the above model. Representingthe NLOS biases by a simple dynamic model such as arandom walk might be justified for certain environments asconsidered in [20], [21], but in general environments this mayonly be considered an approximation. The optimal choice forthe variance of the random walk increment is also intractableas discussed in [29]. Including the biases bik in the state vectoralso increases the computational complexity of the Kalmanfilter, therefore, it may not be computationally efficient as well.Since the random walk model may not be an accurate

approximation for the evolution of bik over time, we avoidusing this model and estimating the biases. To simplify theproblem and reduce the number of unknowns, we eliminatethe NLOS measurements from the observation vector rk,and instead use the information carried out by the biases torestrict the position of the MN within a certain range. Forinstance, in many applications, it can be assumed that the TOAmeasurement noise ni

k is small compared to bik (especially inUWB ranging), which implies that bik + ni

k ≥ 0 [6]. In lightof (3), this assumption is equivalent to

‖xk − ai‖ ≤ rik, i ∈ Nk, (4)

which is obviously a convex constraint as in [30]. If the smallnoise assumption cannot be made, e.g., in narrowband systemswhere TOA-based ranging measurement errors are relativelylarge, the constraints in (4) may not be satisfied. To avoid thislimitation, we can generalize the latter as

‖xk − ai‖ ≤ rik + εσn, i ∈ Nk, (5)

where ε ≥ 0 is a small number to ensure that the MN is locatedinside a disc with radius rik + εσn. Note that even if the biasis zero for a given link (i.e., LOS situation), it is more likely

3We assume that for every time instant, an NLOS identification techniquehas been applied on the measured ranges before employing our proposedfilter. There are numerous techniques which identify the NLOS link using thevariance test [7], [8], [9]. For UWB applications, the features of the receivedTOA signal can also be employed for NLOS identifications as proposed in[4], [5], [10].

that the MN satisfies the constraint in (5) as compared to (4).Therefore, we propose to use the constraint in (5) throughoutthis work due to its robustness against measurement noise andFA error in NLOS identification. In the sequel, the feasibleregion, denoted by Dk refers to the convex set formed by theintersection of the discs in (5); hence

Dk ={x : ‖x− ai‖ ≤ rik + εσn, ∀i ∈ Nk

}. (6)

At every time instant k, let us remove the NLOS mea-surements from the observations in (3) and only keep theLOS measurements, i.e., rik for all i ∈ Lk. The remainingLOS range measurements can be represented by the vectorzk ∈ R

|Lk|. Note that in the worst case, where all themeasurements are identified as NLOS, the vector zk is empty.The state space model and constraints can thus be expressedas

zk = h(sk) + nk, (7a)sk = Fsk−1 +Gwk−1, (7b)‖xk − ai‖ ≤ rik + εσn, i ∈ Nk, (7c)

where h(sk) and nk are vectors whose entries are hi(sk) andnik for i ∈ Lk, respectively. Under our previous assumptionson the measurement noise ni

k in (3), the covariance matrixof nk is positive-definite diagonal, i.e. R = E[nkn

Tk ] =

σ2nI ∈ R

|Lk|×|Lk|. The constraints in (7c) are only on thefirst two elements of the state vector, i.e., xk, as we have a2D positioning scenario herein. Note that if the constraintsin (7c) are removed from the state model, then an ordinarynonlinear filtering technique such as EKF can be used. Thisapproach is also known as EKF with outlier rejection [20]since the NLOS measurements are regarded as outliers andtherefore discarded.In minimum mean square error (MMSE) estimation, e.g.,

Kalman-type filters, one tries to find the conditional mean andcovariance matrix of the state vector sk given the measure-ments up to current time instant k, as characterized by the con-ditional probability density function (PDF) f(sk|z1, . . . , zk).However, when extra information about the state vector isavailable in the form of inequality constraints, the probabilitythat the MN is outside the feasible region should be zero.Hence a truncated or constrained conditional PDF, fc(.|.), canbe defined as

fc(sk|z1, . . . , zk) =

{1β f(sk|z1, . . . , zk), if xk ∈ Dk,

0, otherwise,(8)

where β �∫xk∈Dk

f(sk|z1, . . . , zk)dsk is a normalizationconstant. Therefore, one can estimate the state vector byfinding the conditional mean of sk with truncated PDF as

sk =

∫xk∈Dk

skfc(sk|z1, . . . , zk)dsk, (9)

and the covariance matrix of the constrained state estimate canbe found through

Σk =

∫xk∈Dk

(sk − sk)(sk − sk)T fc(sk|z1, . . . , zk)dsk.

(10)

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 4

This idea is known as PDF truncation, where the distributionof the state vector given the measurements is forced to be zerooutside the feasible region [31]. For a linear dynamic modelwith zero-mean Gaussian measurement and process noises,where the state vector is subject to linear inequality constraints,closed form expressions for sk and Σk in (9)-(10) havebeen obtained using PDF truncation along with the Gaussianassumption [32]. For nonlinear inequality constraints, it isproposed in [32] to do a Taylor series linearization of theconstraints around the current state estimate and then applythe aforementioned method; however, this approach may notbe accurate [33]. In general cases with nonlinear inequalityconstraints, PDF truncation requires multidimensional MonteCarlo integration which becomes computationally expensiveas the size of the state vector grows. Therefore, these compu-tationally demanding techniques may not be suitable to solveour problem.In the following section, we show how we can efficiently

approximate sk and Σk using an alternative approach thatcombines the SRUKF [21] for unconstrained problems withthe projection-based constrained UKF in [22].

III. CONSTRAINED NONLINEAR FILTERING WITH SIGMAPOINT PROJECTION

Another family of methods for imposing inequality con-straints on the state vector are the projection-based techniques,in which the unconstrained state estimate, obtained througha Kalman-type filter, is projected onto the feasible regionby solving an optimization problem [31]. However, by thisapproach, one cannot estimate the constrained error covariancematrix of the state, i.e., Σk, accurately. Therefore, in additionto the unconstrained state estimate, some representative samplepoints of the conditional PDF f(sk|z1, . . . , zk) need to beprojected onto the feasible region. For instance, the sigmapoints of the unscented transformation (UT) can give goodstatistical information about the mean and the error covariancematrix of the state estimate [34]. Based on this idea, in [24],a constrained UKF technique has been proposed in which thesigma points of the UKF violating the constraints are projectedonto the feasible region. However, due to the dependenceof the projection function on the inverse of the a posteriorierror covariance matrix, the method in [24] may becomenumerically unstable [33]. In the following subsections, wefirst describe a variation of the SRUKF that is better suited toour specific problem; then, to overcome the above mentionednumerical issue, we design a more efficient and numericallyreliable method for projecting the sigma points generated fromthe a posteriori estimates, onto the feasible region; finally,we summarize our algorithm and comment on its numericalcomplexity.

A. Unconstrained SRUKF Algorithm

The proposed algorithm in this part is based on the SRUKFpresented in [23] with slight modification such that the algo-rithm is more efficient and numerically reliable. Let sk−1|k−1

be the estimated state and Σk−1|k−1 be the estimated error

covariance matrix of the state, based on the available mea-surements up to current time instant k − 1. Let Uk−1|k−1

be the upper triangular Cholesky factor of Σk−1|k−1, i.e.,Σk−1|k−1 = UT

k−1|k−1Uk−1|k−1. Then, for the next timeinstant, the a priori estimate of the state vector and thecorresponding error covariance matrix, denoted as sk|k−1 andΣk|k−1, respectively, can be obtained through prediction as

sk|k−1 = Fsk−1|k−1, (11)Σk|k−1 = FΣk−1|k−1F

T +GQGT . (12)

Alternatively, the computation of (12) can be avoided as onlythe Cholesky factor of the a priori covariance matrix, denotedby Uk|k−1 is required [23]. To this aim, let us rewrite (12) as

Σk|k−1 =[FUT

k−1|k−1 GQ1

2

] [Uk−1|k−1FT

Q1

2GT

], (13)

If we compute the QR factorization of the second matrix onthe right hand side of (13), we obtain Uk|k−1:

Uk|k−1 = qr

{[Uk−1|k−1F

T

Q1

2GT

]}, (14)

where by definition, the function qr{.} returns the uppertriangular factor of the QR factorization of its matrix argument.With the help of Uk|k−1, the sigma points of the SRUKF

are generated as proposed in [23], i.e.:

s(j)k|k−1 =

⎧⎪⎨⎪⎩sk|k−1, j = 0,

sk|k−1+√ηα(U

Tk|k−1)j , j = 1, . . . , N,

sk|k−1−√ηα(U

Tk|k−1)j−N , j = N + 1, . . . , 2N,

(15)where N is the dimension of the state vector (in this work,N = 4), (UT

k|k−1)j denotes the j-th column of matrixUTk|k−1,

and ηα is a tuning parameter which controls the spread ofthe sigma points. To better understand the geometric meaningof parameter ηα, we can assume that sk|k−1 and Σk|k−1

obtained through the proposed filter are approximately equalto the mean and covariance matrix of the conditional PDFf(sk|z1, . . . , zk−1). Define random variable ηk = (sk −sk|k−1)

TΣ

−1k|k−1(sk−sk|k−1), which is the weighted squared

distance between sk and sk|k−1. Suppose that the parameterηα in (15) is chosen such that Pr(ηk ≤ ηα) = α, where0 < α < 1 represents a desired confidence level. Then, theregion of RN defined by ηk ≤ ηα represents a confidenceellipsoid, on the boundary of which the sigma points in(15) (except s

(0)k|k−1) fall. For example, if α = 0.9, the

probability for sk to lie inside the ellipsoid delimited bythe sigma points with the corresponding ηα is 90%. If weassume that f(sk|z1, . . . , zk−1) is approximately Gaussian,then the random variable η has a Chi-square distribution withN degrees of freedom and it becomes easy to find a value forηα corresponding to a certain ellipsoid with confidence levelα 4.The generated sigma points are transformed through the

nonlinear measurement function as

z(j)k|k−1 = h(s

(j)k|k−1), j = 0, . . . , 2N. (16)

4The Matlab function chi2inv(α,N) can be used for this purpose.

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 5

Then, the mean, cross-covariance matrix, and error covariancematrix of the transformed sigma points can be estimated bymeans of weighted sums as in [35]:

zk|k−1 =2N∑j=0

w(j)z(j)k|k−1, (17)

Σs,zk|k−1 =

2N∑j=0

w(j)(s(j)k|k−1 − sk|k−1)(z

(j)k|k−1 − zk|k−1)

T ,

(18)

P zk|k−1 =

2N∑j=0

w(j)(z(j)k|k−1 − zk|k−1)(z

(j)k|k−1 − zk|k−1)

T +R,

(19)

where R is the covariance matrix of the measurement noisenk in (7a) and the weights w(j) appearing in these expressionsare defined in a similar way as in [36]:

w(j) =

{1− N

ηα, j = 0,

12ηα

, j = 1, . . . , 2N,(20)

and therefore satisfy∑2N

j=0 w(j) = 1.

If the weight w(0) in (20) is negative, it is possible that thecovariance matrix obtained through (19) becomes indefinite(i.e., with negative eigenvalues). However, by choosing asufficiently large value of α, we can guarantee that ηα ≥ N ; inturn, this implies that w(0) ≥ 0 and the covariance matrix (19)then becomes positive definite. In this work, we are interestedin projecting the sigma points that are far away from the meanand it is therefore legitimate to consider ellipsoids with largerconfidence levels, so that the above issue can be naturallyavoided 5. In our dynamic model, withN = 4 and based on theChi-square assumption for ηk, it follows that if α > 0.6, thenηα > N and the positive definiteness of (19) is guaranteed.For numerical stability, instead of forming P z

k|k−1 explic-itly, its Cholesky factor is calculated. Specifically, if we let

e(j)z =

√w(j)(z

(j)k|k−1 − zk|k−1), j = 0, . . . , 2N, (21)

then the upper triangular Cholesky factor of P zk|k−1, denoted

by Uzkis obtained through

Uzk= qr

{[e(0)z , e(1)z , . . . , e(2N)

z ,R1

2

]T}. (22)

It is proposed in [23] to first compute the Kalman gain

Kk = Σs,zk|k−1(P

zk|k−1)

−1 = Σs,zk|k−1U

−1zk

U−Tzk

, (23)

and then, the a posteriori state estimate and the Choleskyfactor of the error covariance matrix can be updated through

sk|k = sk|k−1 +Kk(zk − zk|k−1), (24)Uk|k = cholupdate{Uk|k−1,KkU

Tzk,−1}, (25)

5In [34], a scaled version of the unscented transformation has been proposedto capture higher moments of the nonlinear measurement function, where thegenerated sigma points are located in the vicinity of each other. This methodalso guarantees positive definiteness of the covariance matrix. However, ourproblem is not highly nonlinear and we are interested to generate sigma pointsthat might be far away from one another, therefore, our parameter selectionis different from [34] and [23].

where cholupdate{Uk|k−1,KkUTzk,−1} is the consecutive

downdates6 of the Cholesky factor of UTk|k−1Uk|k−1 using

the columns of KkUTzk. Note that (25) follows from the

covariance matrix update

Σk|k � UTk|kUk|k = UT

k|k−1Uk|k−1 −KkUTzkU zkK

Tk .

(26)Herein, however, we propose a more efficient and numeri-

cally reliable way to compute sk|k and Uk|k. Instead of theKalman gain Kk, we compute

T k = Σs,zk|k−1U

−1zk , (27)

which can be obtained by solving multiple triangular linearsystems T kU z,k = Σ

s,zk|k−1. Then, it follows from (23) that

Kk = T kU−Tzk. Substituting this expression into (24) we

obtain

sk|k = sk|k−1 + T kU−Tzk

(zk − zk|k−1), (28)

where the vector yk � U−Tzk

(zk − zk|k−1) can be obtainedby solving the triangular linear system

UTzkyk = zk − zk|k−1. (29)

From (25) and (27), it follows that the covariance matrix canbe updated as

Σk|k = UTk|k−1Uk|k−1 − T kT

Tk , (30)

hence the Cholesky factor of Σk|k can be computed as

Uk|k = cholupdate{Uk|k−1,T k,−1}. (31)

Compared to the algorithm in [23], this modified algorithmfor the estimation of sk|k and Uk|k saves about 2N |Nk|2flops at each time step k. It is also more numerically reliableas it avoids solving some linear systems, which could be ill-conditioned, and computing some matrix-matrix multiplica-tions.Note that if all the measurements at time instant k are in

NLOS, then the measurement vector zk is empty. Hence wewill use the predicted state in (11) and the Cholesky factorof the predicted covariance matrix in (14) to replace the aposteriori state vector in (28) and Cholesky factor of the errorcovariance matrix in (31), respectively.

B. Imposing the Constraints on the EstimatesUp to this point, the a posteriori state estimate and the

Cholesky factor of the a posteriori error covariance matrixhave been obtained using a SRUKF without taking the con-straints (7c) into account. To impose the constraints on theestimated state and error covariance matrix, similar to [24], anew set of sigma points are generated according to

s(j)k|k =

⎧⎪⎨⎪⎩sk|k, j = 0,

sk|k +√ηα(U

Tk|k)j , j = 1, . . . , N,

sk|k −√ηα(U

Tk|k)j−N , j = N + 1, . . . , 2N.

(32)

6In Matlab, the built-in function Cholupdate can be employed to dorank-1 Cholesky update or downdate, indicated by the third argument of thefunction.

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 6

(a) (b)

Fig. 1: Illustration of the proposed projection technique: (a) Unconstrained state estimate and the uncertainty ellipsoid of sigmapoints, amongst which some are outside the feasible region. (b) The projected sigma points fall inside the feasible region andthe uncertainty ellipsoid is shrunk.

The generated sigma points (except s(0)k|k) form an uncertaintyellipsoid with sk|k at its centre as illustrated in Fig. 1 forN = 2. After the generation of sigma points s(j)k|k with desiredconfidence ellipsoid, those which violate the constraints areprojected onto the convex feasible region through

P(s(j)k|k) = argmin

q

{(q − s

(j)k|k)

TW k(q − s(j)k|k)

},

s.t.∥∥q(1 :2)− ai

∥∥ ≤ rik + εσn, i ∈ Nk,(33)

where W k is a symmetric positive definite (SPD) weightingmatrix [33], [35]. One reasonable choice is W k = Σ

−1k|k,

which gives the smallest estimation error covariance matrixwhen a linear KF is applied to a system with linear dynamicequations and with zero-mean Gaussian observation and exci-tation noises [37].The optimization problem in (33) is a quadratically con-

strained quadratic program (QCQP), which is convex sinceW k is SPD and the constraints are convex [38, p.153]. Asthe constraints are only on the first two elements of the statevector, it is possible to reduce the size of the QCQP problem.A conventional way to do so is as follows. Suppose that q(1 :2)is fixed, then we can find the optimal q(3 : N), which is afunction of q(1 : 2). By substituting the optimal q(3 :N) intothe cost function, we obtain a QCQP, which only involves theunknown q(1 :2).However, in the above approach, we first need to find

the matrix W k through an inverse operation which is bothunnecessarily costly and numerically unstable if the covariancematrix Σk|k is ill-conditioned. To avoid these shortcomings,we propose to use an idea from [39] in order to reformulate andreduce the size of the convex QCQP problem in (33) such thatit can be solved in a more numerically reliable way. Recallingthat Σk|k = UT

k|kUk|k, the objective function in (33) can beexpressed as (q − s

(j)k|k)

TU−1k|kU

−Tk|k (q − s

(j)k|k). To get around

the inverse operation, we define

u = U−Tk|k (s

(j)k|k − q), (34)

from which it follows that

q = s(j)k|k −UT

k|ku. (35)

It is convenient to partition the lower triangular matrix UTk|k

as follows:UT

k|k =

[L11 0

L21 L22

], (36)

where L11 ∈ R2×2 and L22 ∈ R

(N−2)×(N−2) are lowertriangular. Then it follows from (35) that

q(1 :2) = s(j)k|k(1 :2)−L11u(1 :2). (37)

Using (34) and (37), we can reformulate the QCQP problem(33) as

minu

{uT (1 :2)u(1 :2) + uT (3 :N)u(3 :N)

}, (38)

s.t.∥∥L11u(1 :2)− (s

(j)k|k(1 :2)− ai)

∥∥ ≤ rik + εσn, i ∈ Nk.

Since the constraints do not include u(3 : N), the optimalchoice is obviously u(3 :N) = 0 and the optimization problem(38) becomes

minu(1:2)

{uT (1 :2)u(1 :2)

}, (39)

s.t.∥∥L11u(1 :2)− (s

(j)k|k(1 :2)− ai)

∥∥ ≤ rik + εσn, i ∈ Nk.

This 2D convex QCQP problem can now be solved efficientlyusing iterative techniques [38].After finding the optimal u(1 : 2), we can compute the

optimal q using (35) and the fact that the optimal u(3 :N) = 0

as follows:

P(s(j)k|k) � q = s

(j)k|k −

[L11

L21

]u(1 :2). (40)

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 7

The above approach for reducing the size of the QCQPproblem (33) not only avoids a matrix inverse computation,which may cause numerical instability (see [39]), but it isalso computationally efficient. This approach is even moresuitable when a SRUKF is employed since the Choleskyfactor Uk|k of Σk|k is readily provided in (31). We note thatin some particular scenarios, especially under FA in NLOSidentification, it is possible that the feasible region Dk in (6)becomes empty and consequently, (39) has no solution. In thiscase, we simply propose to increase ε until Dk becomes non-empty.After finding the projected sigma points through (40),

the mean and covariance matrix may be estimated throughweighted averaging

sPk|k =2N∑j=0

w(j)P(s(j)k|k), (41)

ΣPk|k =

2N∑j=0

w(j)(P(s(j)k|k)− sPk|k)(P(s

(j)k|k)− sPk|k)

T . (42)

As before, instead of (42) we compute the Cholesky factorUP

k|k of ΣPk|k:

e(j)P =

√w(j)(P(s

(j)k|k)− sPk|k)), j = 0, . . . , 2N,

UPk|k = qr

{[e

(0)P , e

(1)P , . . . , e

(2N)P ]T

}. (43)

As illustrated in Fig. 1, we note that the projected sigma pointsgenerally have a different mean and covariance matrix. Theweighted average of the sigma points achieved through thistechnique lies inside the feasible region since the average ofselected points in a convex feasible region must lie in it [40].Furthermore, the covariance matrix of the error is generallyreduced as the sigma points have moved closer to each other.Finally, in the next iteration of the unconstrained SRUKF,

the constrained a posteriori state estimate sPk|k and theCholesky factor of the corresponding error covariance matrixUP

k|k replace sk|k and Uk|k , respectively as

sk|k = sPk|k, (44)

Uk|k = UPk|k. (45)

C. Algorithm Summary and Computational AnalysisThe proposed CSRUKF algorithm, which is summarized in

Algorithm 1, consists of two main stages: modified version ofSRUKF and projection of sigma points, which are discussedin more details below.The SRUKF is more efficient and numerically stable than

UKF and the computational complexity analysis has also beenpresented in [23], in which it is shown that this algorithmrequires O(N3), where N is the size of the state vector. How-ever, the cost of the first stage of our algorithm is generallysmall compared to the cost of the projection operations in thesecond stage.The QCQP in (39) is a convex optimization problem, which

is not NP hard [38, p.153], and can be solved in polynomialtime using an extended optimization package in Matlab such asSedumi [41]. Since u(1 : 2) ∈ R

2, the optimization problem

Algorithm 1 CSRUKF1: Initialize s0|0 and set Σ0|0 to a large SPD diagonal matrix.2: Set ηα and ε

3: for k = 1, . . . ,K do4: Prediction of sk|k−1 using (11), and Uk|k−1 using (14).5: if |Lk| = 0 then6: Set sk|k = sk|k−1 and Uk|k = Uk|k−1.7: else8: Find the predicted measurement through (16).9: Calculate the predicted mean (17) and implement

qr{.} in (22).10: Estimate the cross-covariance in (18).11: Solve (27) to find T k.12: Estimate the a posteriori mean sk|k using (28) and

Cholesky factor of a posteriori covariance matrixUk|k using (31).

13: end if14: Generate the sigma points using (32).15: For every sigma point whose first two elements fall

outside Dk solve (39) and find the projected point (40).16: Estimate sPk|k using (41) and UP

k|k using (43).17: Replace sPk|k and UP

k|k as the a posteriori estimates,i.e., (44) and (45).

18: end for

can be solved with moderate cost for 2N + 1 sigma pointsat most. However, these calculations can be performed inparallel and independently of each other; hence our techniqueis suitable for parallel processing. The computational cost ofthe algorithm depends on the number of sigma points in (32)that fall inside the feasible region, as the projection operationneeds not to be applied on them. By tuning the parameter α wecan achieve a trade-off between accuracy and computationalcost. On the one hand, if α is small, then it is more likely thatmany sigma points will fall inside the feasible region, resultingin a lower computational cost. However, selecting a small αmay degrade the localization performance as the estimatedquantities remain unchanged after applying the constraints. Onthe other hand, selecting a large α increases the computationalcost but at the same time may result in sampling many of thenon-local points, and thus the linearisation of h(sk) might beinaccurate [34]. In our simulations, it is observed that selecting0.65 ≤ α ≤ 0.85 can offer a reasonable trade-off in terms ofaccuracy and computational cost.

IV. SIMULATION RESULTS

The simulations are implemented in Matlab 2010b on a 64-bit computer with Intel i7-2600 3.4GHz processor and 12GBof RAM. We consider a 2-D area with M = 5 fixed RNslocated at known positions a1 = [0, 0], a2 = [2000, 0]T ,a3 = [0, 2000]T , a4 = [−2000, 0]T , a5 = [0,−2000]T , wherethe units are in meters. A mobile agent moves on this 2-D plane according to the motion model considered earlierin (1) with noise covariance matrix Q = 0.04I2 and thetime step duration is set to δt = 0.2s for K = 1000 timeinstants. The initial MN state vector, including the position

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 8

and velocity components, is normally distributed with zeromean and covariance matrix diag([104, 104, 102, 102]).To model the range measurement, the true distance between

each RN and MN is perturbed with an additive zero-meanGaussian noise. We consider two different measurement noisescenarios: large noise with standard deviation σn = 150mand small noise with σn = 15m, where in our algorithm,we assume that these values are known7. The large noiseassumption can model general applications like narrowbandcellular mobile positioning as considered in [17], [42], whilethe small noise assumption is suitable for localization applica-tions with accurate ranging, e.g., IEEE 802.15.4.a. Note thatthe accuracy of UWB ranging can be improved by increasingthe bandwidth of the system [43]. We also perturb some ofthe measurements by NLOS biases which are modelled asexponential random variables with parameter γ = 500m [18]8.To consider the possible transition of an RN from LOS toNLOS and vice versa, we assume that the status of each RNcan change with a certain probability after every 250 timeinstants. This assumption is reasonable and in line with [44],[9] as the channel conditions might not change drasticallyfor an MN moving with moderate speed. We consider threedifferent scenarios as follows where the transition from LOSto NLOS and vice versa is done with probability of 0.5:

• Scenario I: There are 4 NLOS RNs all the time, while theother RN (the one in the center of the plane) can changebetween LOS and NLOS.

• Scenario II: There are 3 NLOS RNs all the time, whilethe other 2 RNs can transit between LOS and NLOS.

• Scenario III: There are 2 NLOS RNs all the time, whilethe other 3 RNs can change between LOS and NLOS.

For the proposed CSRUKF we consider ε = 3 and α =70%, which corresponds to ηα = 4.8784 under the Gaussianposterior PDF assumption. Note that for the CSRUKF, all thesigma points violating the constraints are projected onto thefeasible region. For solving the QCQP problem, we use theoptimization toolbox Yalmip [45] and Sedumi solver [41].In order to see if projecting all the sigma points is neces-

sary to achieve a good result in NLOS scenarios, we firstconsider the common projection technique where only thea posteriori state estimate of a KF is projected onto thefeasible region [37]. Therefore, sk|k obtained through theSRUKF is projected onto the feasible region, thus the new aposteriori state estimate satisfies the constraints, however, thea posteriori covariance matrix is not changed and remains thesame as the unconstrained case. This approach has in general alower computational cost compared to the proposed CSRUKFalgorithm since at most one projection operation needs to bedone at each iteration. We denote this approach by projectionKalman filter (PKF) and for solving the optimization problemwe follow the similar procedure as done for CSRUKF.For comparison purposes, we consider the conventional

7In practice, knowledge about σn can be obtained by means of preliminarycalibration experiments in a given environment, or through on-line calculationbased on path-loss model for radio propagation.8Our algorithm can still work well with a range dependent NLOS bias

model as considered in [28], however, we avoid considering this case due tothe lack of space.

techniques proposed in [8], [44], [46], in which the rangemeasurements are processed using a KF and then the smoothedrange measurements are used in an EKF where the diagonalelements of the covariance matrix corresponding to the NLOSmeasurements are scaled for further mitigation of NLOSbias. While these techniques differ slightly in terms of pre-processing and variance calculation, we consider the simpleone in [44] denoted by the smooth EKF (SEKF) with scal-ing factor 1.5 and assume that the NLOS identification andvariance calculation are done without error.The Cramer-Rao lower bound (CRLB) analysis in NLOS

shows that if no prior statistics about the distribution of theNLOS bias is available then the optimal strategy is to discardthe NLOS measurements and only use LOS ones [47]. If priorstatistics are available then the NLOS measurements shouldalso be used to achieve a lower MSE. However, this bound canonly be practical if there are enough LOS measurements forunambiguous localization; hence, for a small number of LOSlinks it may not be useful. Even though the posterior Cramer-Rao bound (PCRB) on positioning RMSE has been derivedapproximately in [48], [49], these derivations are based on theassumption that the NLOS bias has a Gaussian distributionwith known mean and variance. Evaluating the PCRB forother NLOS distributions such as exponential is even morechallenging. Since in this paper, there is no information aboutthe distribution of the NLOS biases, except that they arepositive, the mentioned lower bound is still loose and cannotaccurately show the lowest possible error in estimating thestate vector.Due to these limitations in finding a lower bound on the

positioning RMSE, we consider a semi-ideal situation wherethe mean and variance of the NLOS bias of each link is known.To apply a KF to this case, the mean of the bias is subtractedfrom each NLOS measurement, and the error covariancematrix Rr of the measurement vector rk = [r1k, r

2k, . . . , r

Mk ]T

is scaled according to the variance of the corresponding NLOSbias. Then we apply an unconstrained SRUKF to a dynamicsystem with the same state motion model as in (1) and withunbiased set of measurements. For instance if i ∈ Nk, thenRr(i, i) = σ2

n+σ2b , where σ2

b is the variance of the NLOS bias.Note that after subtracting the mean of the NLOS bias fromeach NLOS range measurement, the remaining error is a com-bination of a shifted exponentially distributed variable withzero mean and a zero-mean Gaussian noise. Therefore, if theerror is dominated by the measurement noise, i.e., σ2

n � σ2b ,

then nonlinear Kalman filters give nearly MMSE estimationperformance for moderately nonlinear systems. However, ifthe error is dominated by the NLOS bias, i.e., σ2

b � σ2n, these

filters are unlikely to give nearly optimal performance in theMMSE sense. Although this approach, which is denoted bybias-aware SRUKF (BSRUKF), is not optimal in the MMSEsense and may not be even a performance lower bound forour technique when the mean and variance of the NLOS biasare known, it can be regarded as a useful benchmark forcomparison with our method.To evaluate the performance of the algorithms in different

scenarios, we perform T = 100 Monte Carlo (MC) trials foreach scenario and consider different trajectories at each trial.

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 9

Let xtk and xt

k|k denote the true MN position and its estimatedvectors at the k-th time step of the trajectory over the t-thMonte Carlo trial, respectively. The performance metrics arethe cumulative distribution function (CDF) of the positioningerror ek, expressed as

CDF(ek) = P

[‖xt

k − xtk|k‖ ≤ ek

], (46)

and the root mean square error (RMSE) of the positionestimates at time step k, defined as

ek =

√E

[(xt

k − xtk|k)

T (xtk − xt

k|k)], (47)

where P and E, which are the probability function andexpectation operator, respectively, are evaluated approximatelyusing MC trials.In the following, we compare the effect of measurement

noise and NLOS bias on the performance of different tech-niques in each considered scenario. We assume that the initialestimate s0|0 is normally distributed with mean equal to thetrue state s0 and covariance matrix Σ0|0 = diag([9× 104, 9×104, 103, 103]).

A. Large Measurement NoiseIn the first scenario, we consider the case of a narrowband

ranging application where the noise variance is relativelyhigh, i.e., σn = 150m is considered. The RMSE versus timestep is illustrated in Fig. 2 for scenarios I, II, and III. Thecorresponding CDF of the positioning error is also plotted foreach scenario.We can observe that for scenario I and II, the CSRUKF

performs almost similar to SEKF, while the RMSE of the PKFis relatively high. This shows that in order to obtain a decentlocalization performance, the projection of all the sigma pointsin CSRUKF is necessary as compared to projecting only themean as done in PKF. The RMSE of all the techniques arelower bounded by the RMSE of BSRUKF, which uses moreprior information about the NLOS biases. The RMSE and CDFof scenario III indicate that the performance of the CSRUKFis better than those of PKF and SEKF noticeably.

B. Small Measurement NoiseFor further verification of our algorithm, we consider a case

where the noise variance is relatively small, i.e., σn = 15m,which can model errors in UWB ranging applications. TheRMSE and CDF of the estimation error are illustrated in Fig.3 for the scenarios I, II, and III.As observed in Fig. 3, in all the scenarios, the proposed

CSRUKF performs better than all the other methods, espe-cially the BSRUKF. There are several reasons why CSRUKFcan outperform the BSRUKF to this extent for small measure-ment noise: First, the BSRUKF cannot necessarily provide aperformance lower bound, since after removing the mean ofthe bias from the NLOS range measurements, the remainingerror term does not follow a Gaussian distribution; hence,applying a Kalman filter to this problem is not the optimumMMSE estimation technique. In small noise scenarios, the

NLOS bias dominates over the measurement noise, and there-fore the distribution of the error in the BSRUKF is far from theGaussian distribution. For large measurement noise scenariosin Fig. 2, where the NLOS bias is not significantly larger thanthe measurement noise, the error distribution was closer to aGaussian one, which was one of the reasons that BSRUKFwas performing better than CSRUKF. Second, when σn islarge, the feasible region Dk becomes larger compared to thecase that σn is small. Therefore, it is more likely that mostof the sigma points lie inside Dk and no projection is done,so the second stage of our algorithm does not improve the aposteriori estimate. Note that by restricting the sigma points tobe within a smaller feasible region, a better location estimatemay be obtained.

C. Robustness to Errors in NLOS IdentificationIn this part, we analyse the performance of our proposed

technique in the presence of NLOS identification errors, i.e.,FA and MD, which are inevitable in some applications.To see the effect of FA on the proposed CSRUKF, we

assume that we have one LOS and four NLOS RNs. However,due to the FA, the LOS link is also wrongly detected as beingNLOS. Therefore, CSRUKF, and PKF wrongly remove theLOS measurements from the measurement vector and employthe wrongly detected measurement to impose a constraint onthe state vector. Since the use of a larger ε can increase thechance that a LOS measurement also satisfies the constraintin (5), it is expected that FA does not severely degrade theperformance of our proposed technique. The simulation resultsare shown in Fig. 4, where it is observed that CSRUKF isrobust against FA error in NLOS identification and outper-forms the SEKF. Note that the BSRUKF algorithm is evaluatedunder perfect NLOS identification, while its performance isstill worse than our proposed technique.If the NLOS links are regarded as LOS ones, i.e., in the

presence of NLOS MD error, all the Kalman-type filters haveto use a biased measurement in their observation vector, andthus it is not surprising that their performances are degraded.Therefore, for our algorithm to perform well in most ofthe times, the threshold used for NLOS identification shouldchange such that the probability of MD becomes very small.

D. Computation TimeThe average computation time of each algorithm is calcu-

lated for each scenario and is shown in Table I. The SEKFhas a very small computation time, because it is essentially anordinary Kalman filter. The computation time of PKF, whereonly the state vector needs to be projected onto the feasibleregion, is obviously lower than CSRUKF because only oneQCQP problem might need to be solved at every time step.The most computationally demanding part of CSRUKF is theprojection of the sigma points, which might be implementedin parallel form. However, in the simulation we have donethese projections sequentially, and therefore the total elapsedtime is larger for CSRUKF. Still, the highest computation timeof CSRUKF is lower than the total elapsed time for the entiretrajectory (200 seconds), meaning that with the computer used

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 10

0 200 400 600 800 1000101

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]

PKFBSRUKFCSRUKFSEKF

(a)

0 200 400 600 800 1000101

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]

SEKFPKFCSRUKFBSRUKF

(b)

0 200 400 600 800 1000101

102

103

Time Samples

RM

SE P

ositi

onin

g

SEKFPKFCSRUKFBSRUKF

(c)

0 100 200 300 400 5000

0.2

0.4

0.6

0.8

1

RMSE Positioning

Empi

rical

CD

F

SEKFPKFCSRUKFBSRUKF

(d)

0 200 400 600 800 10000

0.2

0.4

0.6

0.8

1

Positioning Error

Empi

rical

CD

F

SEKFPKFCSRUKFBSRUKF

(e)

0 100 200 300 400 5000

0.2

0.4

0.6

0.8

1

RMSE Positioning [m]

Empi

rical

CD

F

SEKFPKFCSRUKFBSRUKF

(f)

Fig. 2: Comparison of different techniques for large measurement noise with σn = 150m and exponentially distributed NLOSbias with parameter γ = 500m: (a) RMSE for scenario I, (b) RMSE for scenario II, (c) RMSE for scenario III, (d) CDF forscenario I, (b) CDF for scenario II, (c) CDF for scenario III.

0 200 400 600 800 1000101

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]

PKFBSRUKFCSRUKFSEKF

(a)

0 200 400 600 800 1000101

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]

SEKFPKFCSRUKFBSRUKF

(b)

0 200 400 600 800 1000100

101

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]

SEKFPKFCSRUKFBSRUKF

(c)

0 100 200 300 400 5000

0.2

0.4

0.6

0.8

1

Positioning Error [m]

Empi

rical

CD

F

PKFBSRUKFCSRUKFSEKF

(d)

0 100 200 300 400 5000

0.2

0.4

0.6

0.8

1

Positioning Error [m]

Empi

rical

CD

F

PKFBSRUKFCSRUKFSEKF

(e)

0 100 200 300 400 5000

0.2

0.4

0.6

0.8

1

Positioning Error [m]

Empi

rical

CD

F

PKFBSRUKFCSRUKFSEKF

(f)

Fig. 3: Comparison of different techniques for small measurement noise σn = 15m and with exponentially distributed NLOSbias with parameter γ = 500m: (a) RMSE for scenario I, (b) RMSE for scenario II, (c) RMSE for scenario III, (d) CDF forscenario I, (b) CDF for scenario II, (c) CDF for scenario III.

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 11

0 200 400 600 800 100010

1

102

103

Time Samples

RM

SE P

ositi

onin

g [m

]PKFBSRUKFCSRUKFSEKF

(a)

0 50 100 150 200 250 3000

0.2

0.4

0.6

0.8

1

Positioning Error [m]

Empi

rical

CD

F

PKFBSRUKFCSRUKFSEKF

(b)

Fig. 4: Comparison of different techniques for σn = 15mand exponentially distributed NLOS bias with parameter γ =500m, and FA in identification of an LOS RN: (a) RMSE, (b)CDF.

here, the algorithm can be applied for online tracking. Notethat the computational cost of many other popular methodssuch as KDE or particle filters is also much higher thanordinary EKF or SRUKF and therefore, our algorithm remainscompetitive in terms of computation time.

TABLE I: Average running time of each algorithm in eachscenario evaluated for the entire trajectory (200s) in seconds

Scenario I Scenario II Scenario IIISEKF 0.38 0.41 0.45PKF 25.72 10.5935 1.9141CSRUKF 64.03 31.4485 8.3804

V. CONCLUSION AND FUTURE WORK

A constrained square-root unscented Kalman filter(CSRUKF) with projection technique was proposed in thispaper for the aim of TOA-based localization of an MN inNLOS scenarios. The NLOS measurements were removedfrom the measurement vector and instead, they were employedto impose quadratic constraints onto the position coordinatesof the MN. The sigma points of the UKF which violatedthe constraints were projected on the feasible region bysolving a convex quadratically constrained quadratic program

(QCQP). As compared to other constrained UKF techniques,we considered a square root filter and avoided computing theinverse of the state covariance matrix both in the Kalman filterand in the optimization steps; consequently our approach hasbetter numerical stability and lower computational cost. In thesimulation experiments, the proposed CSRUKF performedbetter than other approaches in different NLOS scenarios.In particular, CSRUKF performance was excellent when asmall measurement noise variance was considered, suggestingthat it is particularly suitable for high resolution TOA-basedUWB localization. Another advantage of our technique isits robustness to FA errors in NLOS identification. Theproposed CSRUKF can be extended to the situations wherethe information of an IMU is fused with range measurementsfor more accurate mobile localization. The proposed CSRUKFcan also be extended to cooperative tracking of multiple MNs.However, several challenges remain in this respect such as thecommunication overload and the efficient implementation ofa distributed version of CSRUKF. We leave the investigationof such cooperative localization scenarios to the future work.

ACKNOWLEDGMENT

The authors would like to thank the anonymous reviewersfor their helpful comments and suggestions in improving thequality of this work.

REFERENCES[1] A. Sayed, A. Tarighat, and N. Khajehnouri, “Network-based wireless

location: challenges faced in developing techniques for accurate wirelesslocation information,” IEEE Signal Process. Magazine, vol. 22, no. 4,pp. 24–40, Jul. 2005.

[2] “IEEE standard for local and metropolitan area networks–Part 15.4:Low-rate wireless personal area networks (LR-WPANs) amendment 1:MAC sublayer,” IEEE Std 802.15.4e-2012, pp. 1–225, 2012.

[3] K. Pahlavan, F. O. Akgul, M. Heidari, A. Hatami, J. M. Elwell, andR. D. Tingley, “Indoor geolocation in the absence of direct path,” IEEEWireless Communications, vol. 13, no. 6, pp. 50–58, Dec. 2006.

[4] S. Venkatesh and R. Buehrer, “Non-line-of-sight identification in ultra-wideband systems based on received signal statistics,” IET Microwaves,Antennas Propagation, vol. 1, no. 6, pp. 1120–1130, Dec. 2007.

[5] S. Marano, W. M. Gifford, H. Wymeersch, and M. Z. Win, “NLOSidentification and mitigation for localization based on UWB experimen-tal data,” IEEE J. Sel. Area Commun., vol. 28, no. 7, pp. 1026–1035,Sep. 2010.

[6] I. Guvenc and C.-C. Chong, “A survey on TOA based wireless localiza-tion and NLOS mitigation techniques,” IEEE Communications SurveysTutorials, vol. 11, no. 3, pp. 107–124, quarter 2009.

[7] N. Thomas, D. Cruickshank, and D. Laurenson, “Performance of aTDOA-AOA hybrid mobile location system,” in Proc. 2nd Int. Conf.on 3G Mobile Communication Technologies, 2001, pp. 216–220.

[8] C.-D. Wann, Y.-J. Yeh, and C.-S. Hsueh, “Hybrid TDOA/AOA indoorpositioning and tracking using extended Kalman filters,” in Proc. IEEEVehicular Tech. Conf.-Spring, vol. 3, May 2006, pp. 1058–1062.

[9] K. Yu and E. Dutkiewicz, “NLOS identification and mitigation formobile tracking,” IEEE Trans. on Aerospace and Electronic Systems,vol. 49, no. 3, pp. 1438–1452, Jul. 2013.

[10] I. Guvenc, C.-C. Chong, and F. Watanabe, “NLOS identification andmitigation for UWB localization systems,” in Proc. IEEE WirelessCommunications and Networking Conf., Mar. 2007, pp. 1571–1576.

[11] S. Venkatesh and R. Buehrer, “NLOS mitigation using linear pro-gramming in ultrawideband location-aware networks,” IEEE Trans. onVehicular Tech., vol. 56, no. 5, pp. 3182–3198, Sep. 2007.

[12] C.-L. Chen and K.-T. Feng, “An efficient geometry-constrained locationestimation algorithm for NLOS environments,” in Proc. Int. Conf. onWireless Networks, Communications and Mobile Computing, vol. 1, Jun.2005, pp. 244–249.

IEEE TRANSACTION ON VEHICULAR TECHNOLOGY (DRAFT) 12

[13] J. Hol, F. Dijkstra, H. Luinge, and T. Schon, “Tightly coupledUWB/IMU pose estimation,” in Proc. IEEE Int. Conf. on Ultra-Wideband, Sep. 2009, pp. 688–692.

[14] J. Youssef, B. Denis, C. Godin, and S. Lesecq, “Loosely-coupled IR-UWB handset and ankle-mounted inertial unit for indoor navigation,” inProc. IEEE Int. Conf. on Ultra-Wideband, Sep. 2011, pp. 160–164.

[15] B.-S. Chen, Y. C.-Y., L. F.-K., and L. J.-F., “Mobile location estimatorin a rough wireless environment using extended Kalman-based IMMand data fusion,” IEEE Trans. on Vehicular Tech., vol. 58, no. 3, pp.1157–1169, Mar. 2009.

[16] C. Fritsche, U. Hammes, A. Klein, and A. Zoubir, “Robust mobileterminal tracking in NLOS environments using interacting multiplemodel algorithm,” in Proc. IEEE Int. Conf. on Acoustics, Speech andSignal Process., Apr. 2009, pp. 3049–3052.

[17] U. Hammes, E. Wolsztynski, and A. Zoubir, “Robust tracking andgeolocation for wireless networks in NLOS environments,” IEEE J. ofSelected Topics in Signal Process., vol. 3, no. 5, pp. 889–901, Oct. 2009.

[18] U. Hammes and A. Zoubir, “Robust MT tracking based on m-estimationand interacting multiple model algorithm,” IEEE Trans. on SignalProcess., vol. 59, no. 7, pp. 3398–3409, 2011.

[19] M. Najar, J. Huerta, J. Vidal, and J. Castro, “Mobile location with biastracking in non-line-of-sight,” in Proc. IEEE Int. Conf. on Acoustics,Speech, and Signal Processing, vol. 3, May 2004, pp. 956–959.

[20] D. Jourdan, J. Deyst, J.J., M. Win, and N. Roy, “Monte Carlo localizationin dense multipath environments using UWB ranging,” in Proc. IEEEInt. Conf. on Ultra-Wideband, Sep. 2005, pp. 314–319.

[21] J. Gonzalez, J. L. Blanco, C. Galindo, A. Ortiz-de Galisteo, J. A.Fernandez-Madrigal, F. A. Moreno, and J. L. Martınez, “Mobile robot lo-calization based on ultra-wide-band ranging: A particle filter approach,”Robot. Auton. Syst., vol. 57, no. 5, pp. 496–507, May 2009.

[22] S. Yousefi, X. Chang, and B. Champagne, “Improved extended Kalmanfilter for mobile localization with NLOS anchors,” in Proc. Int. Conf.on Wireless and Mobile Communications, Jul. 2013, pp. 25–30.

[23] R. Van der Merwe and E. Wan, “The square-root unscented Kalmanfilter for state and parameter-estimation,” in Proc. IEEE Int. Conf. onAcoustics, Speech, and Signal Processing, vol. 6, May 2001, pp. 3461–3464.

[24] R. Kandepu, B. Foss, and L. Imsland, “Applying the unscented Kalmanfilter for nonlinear state estimation,” J. of Process Control, vol. 18,no. 78, pp. 753–768, 2008.

[25] R. A. Horn and C. R. Johnson, Matrix Analysis. Cambridge UniversityPress, 1990.

[26] D. Dardari, R. D’Errico, C. Roblin, A. Sibille, and M. Win, “Ultrawidebandwidth RFID: The next generation?” Proceedings of the IEEE,vol. 98, no. 9, pp. 1570–1582, Sep. 2010.

[27] A. Molisch, “Ultrawideband propagation channels-theory, measurement,and modeling,” IEEE Trans. on Vehicular Tech., vol. 54, no. 5, pp. 1528–1545, Sep. 2005.

[28] K. Yu and Y. Guo, “Improved positioning algorithms for nonline-of-sight environments,” IEEE Trans. on Vehicular Tech., vol. 57, no. 4, pp.2342–2353, Jul. 2008.

[29] R. M. Vaghefi and R. M. Buehrer, “Target tracking in NLOS environ-ments using semidefinite programming,” in IEEE Military Communica-tions Conf., Nov. 2013, pp. 169–174.

[30] L. Doherty, K. pister, and L. El Ghaoui, “Convex position estimationin wireless sensor networks,” in Proc. IEEE 20th Annual Joint Conf.of the Computer and Communications Societies, vol. 3, Apr. 2001, pp.1655–1663.

[31] D. Simon, “Kalman filtering with state constraints: a survey of linearand nonlinear algorithms,” IET Control Theory and Applications, vol. 4,no. 8, pp. 1303–1318, Aug. 2010.

[32] D. Simon and D. L. Simon, “Constrained Kalman filtering via densityfunction truncation for turbofan engine health estimation,” Int. J. ofSystems Science, vol. 41, no. 2, pp. 159–171, Feb. 2010.

[33] J. Lan and X. Li, “State estimation with nonlinear inequality constraintsbased on unscented transformation,” in Proc. 14th Int. Conf. on Infor-mation Fusion, Jul. 2011, pp. 1–8.

[34] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinearestimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, Mar.2004.

[35] D. Simon, Optimal State Estimation: Kalman, H-Infinity, and NonlinearApproaches. Wiley-Interscience, Aug. 2006.

[36] D. Zachariah, I. Skog, M. Jansson, and P. Handel, “Bayesian estimationwith distance bounds,” IEEE Signal Process. Letters, vol. 19, no. 12,pp. 880–883, Dec. 2012.

[37] D. Simon and D. L. Simon, “Kalman filtering with inequality constraintsfor turbofan engine health estimation,” IEE Proc. on Control Theory andApplications, vol. 153, no. 3, pp. 371–378, May 2006.

[38] S. Boyd and L. Vandenberghe, Convex Optimization. New York, NY,USA: Cambridge University Press, 2004.

[39] C. Paige, “Computer solution and perturbation analysis of generalizedlinear least squares problems,” J. Math. of Comput., vol. 33, no. 145,pp. 171–183, 1979.

[40] D. Luenberger and Ye, Linear and Nonlinear Programming, 3rd ed.Springer, 2008.

[41] J. F. Strum, “Using sedumi 1.02, a matlab toolbox for optimization oversymmetric cones,” 1998.

[42] U. Hammes and A. Zoubir, “Robust mobile terminal tracking in NLOSenvironments based on data association,” IEEE Trans. on Signal Pro-cess., vol. 58, no. 11, pp. 5872–5882, Nov. 2010.

[43] S. Gezici, Z. Tian, G. Giannakis, H. Kobayashi, A. Molisch, H. Poor,and Z. Sahinoglu, “Localization via ultra-wideband radios: a look atpositioning aspects for future sensor networks,” IEEE Signal Process.Mag., vol. 22, pp. 70–84, Jul. 2005.

[44] B. L. Le, K. Ahmed, and H. Tsuji, “Mobile location estimator withNLOS mitigation using Kalman filtering,” in Proc. IEEE WirelessCommunications and Networking Conf., vol. 3, Mar. 2003, pp. 1969–1973.

[45] J. Lofberg, “Yalmip : a toolbox for modeling and optimization inMatlab,” in Proc. IEEE Int. Symp. on Computer Aided Control SystemsDesign, Sep. 2004, pp. 284–289.

[46] K. Yu and E. Dutkiewicz, “Improved Kalman filtering algorithms formobile tracking in NLOS scenarios,” in Proc. IEEE Wireless Commu-nications and Networking Conf., Apr. 2012, pp. 2390–2394.

[47] Y. Qi, H. Kobayashi, and H. Suda, “Analysis of wireless geolocationin a non-line-of-sight environment,” IEEE Trans. on Wireless Commun.,vol. 5, no. 3, pp. 672–681, Mar. 2006.

[48] L. Chen, S. Ali-Lytty, R. Pich, and L. Wu, “Mobile tracking inmixed line-of-sight/non-line-of-sight conditions: Algorithm and theoret-ical lower bound,” Wireless Personal Communications, vol. 65, no. 4,pp. 753–771, Aug. 2012.

[49] C. Fritsche, A. Klein, and F. Gustafsson, “Bayesian Cramer-Rao boundfor mobile terminal tracking in mixed LOS/NLOS environments,” IEEEWireless Commun. Letters, vol. 2, no. 99, pp. 335–338, Jun. 2013.