An IMM-based Decentralized Cooperative Localization with LoS and NLoS UWB Inter-agent Ranging
11 An IMM-based Decentralized Cooperative Localization withLoS and NLoS UWB Inter-agent Ranging
Jianan Zhu Solmaz S. Kia, senior member, IEEE
University of California Irvine
Abstract —This paper studies the global localization of a groupof communicating mobile agents via an ultra-wideband (UWB)inter-agent ranging aided dead-reckoning system. We propose aloosely coupled cooperative localization algorithm that acts asan augmentation atop the local dead-reckoning system of eachmobile agent. This augmentation becomes active only when anagent wants to process a relative measurement it has taken. Themain contribution of this paper is to address the challengesin the proper processing of the UWB range measurements inthe framework of the proposed cooperative localization. Eventhough UWB offers a decimeter level accuracy in line-of-sight(LoS) ranging, its accuracy degrades significantly in non-line-of-sight (NLoS) due to the significant unknown positive biasin the measurements. Thus, the measurement models for theUWB LoS and NLoS ranging conditions are different, and properprocessing of NLoS measurements requires a bias compensationmeasure. On the other hand, in practice, the measurementmodal discriminators determine the type of UWB range mea-surements with only some level of certainty. To take into accountthe probabilistic nature of the NLoS identifiers, our proposedcooperative localization employs an interacting multiple model(IMM) estimator. The effectiveness of our proposed method isdemonstrated via an experiment for a group of pedestrians whouse UWB relative range measurements among themselves toimprove their geolocation using a shoe-mounted INS system.
I. I
NTRODUCTION
In the absence of the Global Positioning System (GPS) incomplex environments, the localization of mobile agents canbecome a challenging task. Localization based on inertialnavigation system (INS) [1] or odometry [2] provides a self-contained solution but suffers from unbounded error accumu-lation of inherent measurement noises overtime. Aiding by de-tecting and processing measurements from external landmarkshelps to bound the localization error [3], [4] but has limitedusage when external landmarks are not widely available. For agroup of communicating mobile agents, aiding via cooperativelocalization (CL) by processing inter-agent measurements asfeedback to update the location estimate makes the localizationsystem more reliable under the circumstances when externallandmarks are sparse [5]. However, the effectiveness of CLdepends on how frequently the inter-agent measurements areavailable, and also on the accurate modeling and processingof the inter-agent measurements. Moreover, in CL, the inter-agent measurement updates create strong correlations betweenagents. Ignoring the correlations will lead to over-confidentestimations and even filter divergence. On the other hand,
The authors are with the Mechanical and Aerospace Eng. Dept. of Univ.of California Irvine, CA 92697, USA, jiananz1,[email protected] .This work is supported by the U.S. Dept. of Commerce, National Institute ofStandards and Technology award 70NANB17H192. keeping track of the correlation explicitly requires persis-tent inter-agent communication, therefore comes with highcommunication overhead and stringent connectivity require-ment [5]. Therefore, the effectiveness of CL also depends ondevising consistent decentralized implantation that accountsfor inter-agents correlations with reasonable computation andcommunication cost per agent. From the communication costperspective, loosely coupled CL algorithms [6]–[11], whichaccount for unknown inter-agent correlations by implicit ap-proaches that are closely related to the covariance inter-section method in sensor fusion literature [12], [13], offerthe most efficient solution. These algorithms do not requireany network-wide connectivity; only the two agents involvedin a relative measurement should exchange information toprocess that relative measurement. In this paper, we adopt the
Discorrelated minimum variance (DMV) approach of [11] asour CL framework.A variety of sensing technologies including computer vision-based techniques [14] and wireless radio signal based tech-niques [15] are used for inter-agent relative measurements inCL implementations. The computer vision-based techniques’requirement of LoS condition between the agents makes themless effective in complex and cluttered environments. For suchenvironments, wireless signal based inter-agent rangings offera more efficient solution. Among the wireless signal basedtechniques, UWB technology due to its high time resolution,wide bandwidth, and capability to work under NLoS condi-tion [16] has attracted significant attention for applicationsin dense multi-path environments. UWB uses a time-of-flight(ToF) based approach for ranging and offers a decimeterlevel ranging in LoS conditions [17]. However, NLoS UWBranging measurements are positively biased and thus havelower accuracy [18], which can have a significant impact onlocalization performance. Thus, the measurement models forthe UWB LoS and NLoS ranging conditions are different,and proper processing of NLoS measurements requires a biascompensation measure.To mitigate the adverse effect of the NLoS ranging bias onthe localization accuracy, one idea is to identify the NLoSmeasurements and drop them [19]–[21]. But, this approachlimits the effectiveness of the UWB measurement feedbacksin dense and complex environments. To avoid discarding theNLoS measurements, empirical analysis and machine learningmethods that aim to identify and remove the bias are proposed[22]–[26]. However, these approaches require collecting alarge amount of training data. The machine learning techniquesalso come with high computational complexity to analyzethe signal channel statistics. As such, these methods are a r X i v : . [ ee ss . SP ] S e p not a practical solution for real-time online applications inunknown environments. When UWB is used as an aiding fora dead-reckoning system, [27]–[29] use the algorithmic biascompensation methods from estimation filter literature [30]to deal with UWB NLoS bias. [27] uses the covarianceinflation method followed by a constrained Kalman filteringto compensate for bias in UWB range measurements in acooperative localization algorithm. However, the covarianceinflation method is known to be conservative and can lead tofilter inconsistency [30]. On the other hand, [28], [29] use theSchmidt Kalman filtering (SKF) [30], [31], which is knownto yield a more efficient bias compensation, followed by anovel constrained sigma point based filtering to process NLoSmeasurements with respect to beacons with known locations toaid an INS localization. In this paper, we adopt the SKF biascompensation approach for NLoS UWB bias compensationand incorporate it into the framework of the DMV CL.Localization filters that process both LoS and NLoS UWBranging such as those in [27], [28] assume that the LoS andNLoS measurements can be identified and distinguished fromeach other with exact certainty. These localization filters use apower-based NLoS identification method [32] to identify theLoS and NLoS conditions. The power-based NLoS identifi-cation methods work based on the fact that in LoS conditionpower of the received direct-path signal takes a big proportionof the total received signal power, while in NLoS conditionthe direct-path is significantly attenuated or even completelyblocked. When the difference between total received powerand the direct-path power is larger than a threshold value, therange measurement is identified to be NLoS [32]. The perfor-mance of this approach however depends highly on the choiceof the discrimination threshold value. Moreover, as we demon-strate via an experimental study in our preliminary work [29],in practice deterministic identification of the UWB rangingmode is not accurate, and identification that determines thetype of UWB range measurements deliver their results withonly some level of certainty, see Fig. 1. Given the probabilisticnature of the power-based LoS/NLoS identification method,processing inter-agent UWB range measurements should bemodeled as a dynamic multiple model problem . Optimal esti-mation of a dynamic multiple model problem requires a setof parallel filters whose number increases exponentially withtime [33], [34]. To design a practical localization algorithmwith a reasonable computation cost, we adopt the suboptimalIMM estimator framework [35], [36].To summarize, the main contribution of this paper is to expandthe loosely coupled DMV CL algorithm of [11] so that itprocesses inter-agent UWB ranging measurements based onthe interacting multiple model (IMM) estimator given themulti-modal nature of such measurements. To do so, we alsodevelop an SKF based NLoS UWB bias compensation that canwork in the framework of the DMV CL. The effectivenessof our proposed method is demonstrated via an experimentfor a group of pedestrians who use UWB relative rangemeasurements among themselves to improve their geolocationusing a shoe-mounted INS system. Fig. 1 – An experimental result that demonstrates the probabilisticnature of the power-based LoS/NLoS identification [29].
II. P
ROBLEM DEFINITION
Consider a team of N mobile agents with computation andcommunication capabilities in which each agent is equippedwith a set of proprioceptive sensors, e.g., INS or wheelencoders, to localize itself in a global frame using dead-reckoning. At each time step t ∈ Z ≥ let this local stateestimate and the corresponding error covariance constitute thelocal belief, bel i – ( t ) = ( ˆx i – ( t ) , P i – ( t )) , of agent i about itspose in the global frame. To bound the localization error of thedead-reckoning system, suppose each agent is also equippedwith an UWB transceiver to take and process relative rangemeasurements from other team members as well as possibly afew UWB beacons with known locations in the environment.We assume that agents are able to uniquely identify the otherUWB nodes (hereafter, UWB nodes refers to both a mobileagent or a beacon) via the unique MAC address of their UWBtransceivers. The measurement model for the UWB rangingbetween any agent i and another node j is z ij ( t ) = (cid:107) p i ( t ) − p j ( t ) (cid:107) (cid:124) (cid:123)(cid:122) (cid:125) h ( x i ( t ) , x j ( t )) + b i ( t ) + ν i ( t ) + (cid:40) , LoS b i ( t ) , NLoS , (1)where b i ( t ) is the additive bias modeled as Gaussian noise withmean ¯ φ i and variance E [ b i ( t ) b i ( t )] = Φ i > , while ν i ( t ) is the additive zero-mean white Gaussian measurement noisewith variance E [ ν i ( t ) ν i ( t )] = R i > . NLoS signal propa-gations can be distinguished from the LoS signal propagationbased on a real-time signal power-based approach without anyprior information about the environment [32]. The Previouswork in [28] assumed that using a power-based identificationmethod we can distinguish LoS and NLoS ranging conditionsfrom each other with exact certainty by implementing aseparation threshold. Then, the belief updates using relativeinter-agent measurement processing can be carried out usingthe respective measurement model. However, in practice, theidentification methods do not exactly identify the measurementcondition with absolute certainty. As we have shown in ourpreliminary work [29], if we record the power metric of ourpower-based modal identifier under a controlled environmentwhere we know the true measurement type, we arrive ata probabilistic identification outcome as shown in Fig. 1.Therefore, what the power-based UWB mode discriminatoris delivering is a likeliness level about the measurementmode. Let M ( t ) ∈ { M , M } be the modal state of the UWB ranging measurement at time t . The power-based UWBmode discriminator assigns a normalized probability that themeasurement is in LoS (denoted by M ) or NLoS (denotedby M ), p ( M ( t ) = M i ) , i ∈ { , } , where p ( M )+ p ( M ) = 1 . (2)As our experiment in Section V shows, using a threshold in thepower-based UWB mode discriminator to assign a determin-istic measurement type, and then a consequent measurementprocessing leads to an inferior localization result.On the other hand, a relative measurement update (in LoS orNLoS) using a feedback gain K i in the form of ˆx i + = ˆx i – + K i ( z ij − ˆ z ij ) , (3)creates a correlation among the state estimates of agents i and j , i.e., P + ij (cid:54) = after implementing (3). To maintain the exactaccount of the updated and the propagated cross-covarainceterms for filter consistency, agents need to communicate witheach other at all times. However, under limited connectivitycondition, it is ideal that agent i and agent j communicateif and only if a relative measurement is taken between them.Our objective in this paper is to design a relative measurementprocessing method that respects this minimal communicationconnectivity requirement while also takes into account thestochastic nature of the UWB ranging measurement modalvariable M ( t ) .III. A N IMM
BASED ESTIMATOR WITH
UWB
RANGINGFEEDBACK
In this section, we derive the constituting equations of theIMM filtering for estimate correction via UWB ranging feed-back. To simplify the notation, we derive our equations forwhen an agent i takes only one measurement with respectto other nodes at each time. The case of multiple concurrentmeasurements is discussed in the next section, when weimplement our IMM based estimator in the context of theDMV based loosely coupled CL.Note that by implementing a power-based UWB modal dis-criminator [32], a confidence level about the measurementmode can be derived with probability density function f M ( m ) = (cid:88) n =1 p n ( t ) δ ( M m − M n ) , m ∈ { , } , (4)where p n ( t ) ∈ [0 , and (cid:80) n =1 p n ( t ) = 1 . Here the subscript represents the unbiased LoS ranging mode and the subscript represents the biased NLoS ranging mode. Note that thedensity function (4) is independent of the modal history, thusfor any UWB ranging mode n ∈ { , } , we can always write P ( M ( t ) = M n | M ( t −
1) = M m )= P ( M ( t ) = M n ) = p n ( t ) , m ∈ { , } . (5) Next, let the exteroceptive measurements history taken byagent i from initial time to time step t be Z i t . Moreover,let the l th model hypotheses sequence, through time t be M it,l = { M m ,l , M m ,l , ..., M m t,l } , (6)where m t,l ∈ { , } is the measurement model index attime t . Because for each time step, there are two possiblemeasurement models, then t different measurement modelhypotheses sequences exist at time t , i.e., l ∈ { , ..., t } . Theconditional probability density function of the state x i ( t ) attime step t is obtained using the total probability theoremwith respect to the mutually exclusive and exhaustive set ofevents (6) with l ∈ { , ..., t } , as a Gaussian mixture with anexponentially increasing number of terms p ( x i ( t ) | Z i t ) = t (cid:88) l =1 p ( x i ( t ) | M it,l , Z i t ) P ( M it,l | Z i t ) , (7)where p ( x i ( t ) | M it,l , Z i t ) is the model-conditioned updateddistribution and P ( M it,l | Z i t ) is the probability of the l thmodel hypotheses sequence conditioned on the observations.From (7), t filters are needed to run in parallel to derive theexact distribution. The computational and memory complexitymakes the optimal method impractical. IMM estimator [30,chapter 1] is a feasible sub-optimal solution that only requiresthe number of filters linear to the number of models operatingin parallel in each step. Following [35], a cycle of the IMMestimator from right after the previous measurement update upto and including the current measurement update includes thefollowing steps: • Mixing: P ( M ( t ) | Z i t − ) ← P ( M ( t − | Z i t − ) , (8a) p ( x i ( t − | M ( t ) , Z i t − ) ← p ( x i ( t − | M ( t − , Z i t − ) , (8b) • Model-based propagation: p ( x i ( t ) | M ( t ) , Z i t − ) ← p ( x i ( t − | M ( t ) , Z i t − ) , (9) • Probability evolution: P ( M ( t ) | Z i t ) ← P ( M ( t ) | Z i t − ) , (10) • Model-based updating: p ( x i ( t ) | M ( t ) , Z i t ) ← p ( x i ( t ) | M ( t ) , Z i t − ) , (11) • Combination: p ( x i ( t ) | Z i t ) ← p ( x i ( t ) | M ( t ) , Z i t ) . (12)In IMM estimator, the cycle is initialized from the model-conditioned updated distributions p ( x i ( t − | M ( t − , Z i t − ) and the model probability based on the observation history P ( M ( t − | Z i t − ) from the previous cycle. To simplifythe notation, we use M n ( t − to represent M ( t −
1) = M n , n ∈ { , } . (8a) is expanded according to the Chapman-Kolmogorov equation [37] as P ( M n ( t ) | Z i t − ) = (cid:88) m =1 P ( M n ( t ) | M m ( t − P ( M m ( t − | Z i t − ) . (13)Given (5) and (cid:80) m =1 P ( M m ( t − | Z i t − ) = 1 , however, (13)results in P ( M n ( t ) | Z i t − ) = p n ( t ) , n ∈ { , } . (14)Next, based on the law of total probability, (8b) reads as p ( x i ( t − | M n ( t ) , Z i t − ) = (cid:88) m =1 p ( x i ( t − | M m ( t − , Z i t − ) × P ( M m ( t − | M n ( t ) , Z i t − ) , n ∈ { , } . (15)However, as Lemma 3.1 shows by invoking (5), (15) can besimplified to (16). Lemma 3.1:
Given the probability density function (4) modelfor UWB ranging mode type and (5), then we have p ( x i ( t − | M n ( t ) , Z i t − ) = p ( x i ( t − | Z i t − ) , (16)for n ∈ { , } . Proof:
Note that by virtue of Bayes rule, we obtain P ( M m ( t − | M n ( t ) , Z i t − ) (17) = P ( M n ( t ) | M m ( t − , Z i t − ) P ( M m ( t − | Z i t − ) (cid:80) m =1 P ( M n ( t ) | M m ( t − P ( M m ( t − | Z i t − ) . By virtue of (5), (17) can be written as P ( M m ( t − | M n ( t ) , Z i t − )= p n ( t ) P ( M m ( t − | Z i t − ) (cid:80) m =1 p n ( t ) P ( M m ( t − | Z i t − )= P ( M m ( t − | Z i t − ) , for n ∈ { , } , and m ∈ { , } . Here, we also used (cid:80) m =1 P ( M m ( t − | Z i t − ) = 1 . Then (15), for n ∈ { , } ,is equivalent to p ( x i ( t − | M n ( t ) , Z i t − )= (cid:88) m =1 p ( x i ( t − | M m ( t − , Z i t − ) P ( M m ( t − | Z i t − ) . From (20) we can write p ( x i ( t − | M n ( t ) , Z i t − ) = p ( x i ( t − | Z i t − ) , for n ∈ { , } , which concludes the proof. (cid:3) Lemma 3.1 states that (8b) of mixing step is not neededand the model-conditioned posterior distribution p ( x i ( t − | M n ( t ) , Z i t − ) can be determined directly from the pos-terior distribution p ( x i ( t − | Z i t − ) of the previous cycle.Then, since the propagation model of agent i does not havedependency on modal state, the model-based propagation (9)then is simply obtained from propagating the posterior distri-bution from previous step through the system model, i.e., p ( x i ( t ) | Z i t − ) ← p ( x i ( t − | Z i t − ) , i.e., in summary, the flow shown in Fig. 2 without mixingstep (8) and model-based propagation step (9) will be equiv-alent to traditional IMM estimator given the UWB ranging Fig. 2 – One cycle of IMM estimator of agent i with UWB rangingcorrection feedback. mode type probability density function (4). This propertysimplifies the IMM estimator and makes implementation of theIMM CL easier as an augmentation service atop the local fil-ters. Next, we note that the propagated distribution is updatedin two parallel process conditioned on different measurementmodels as in Fig. 2 to derive the model-conditioned updateddistribution p ( x i ( t ) | M n ( t ) , Z i t ) . For n ∈ { , } , the modelprobability is evolved according to P ( M n ( t ) | Z i t ) = P ( z ij ( t ) | M n ( t ) , Z i t − ) P ( M n ( t ) | Z i t − ) (cid:80) m =1 P ( z ij ( t ) | M m ( t ) , Z i t − ) P ( M m ( t ) | Z i t − ) (18) P ( z ij ( t ) | M n ( t ) , Z i t − ) is the model-conditioned likelihood,which can be derived from the likelihood function of the model M ( t ) if the distribution is Gaussian as follows [38, chapter 2] p ( z ij ( t ) | M n ( t ) , Z i t ) = e ( − ˜ z ij n / S ijn ) (cid:113) π | S ijn | , (19)where ˜ z ijn = z ij − ˆ z ijn and S ijn are the model-matchedinnovation and corresponding covariance. In IMM approach,the optimal estimate (7) is approximated finally by p ( x i ( t ) | Z i t ) = (cid:88) n =1 P ( M n ( t ) | Z i t ) p ( x i ( t ) | M n ( t ) , Z i t ) . (20)IV. A N IMM
BASED COOPERATIVE LOCALIZATION VIA
UWB
INTER - AGENT RANGING
Given the IMM estimator in Fig. 2 for the UWB rangingcorrection feedback, we propose the adaptive UWB-basedcooperative localization (AUCL) algorithm shown in Fig. 3 toprocess the UWB-based relative range measurements taken byagent i from another node j in the form of a loosely coupled Fig. 3 –
The propose AUCL, an augmentation atop of the local filterof agent i becomes active when there is an inter-agent UWB rangemeasurement. It contains two parallel updating filter, one is usedto process unbiased LoS measurement and the other one is used toprocess NLoS measurement with bias compensation. augmentation. To develop our loosely coupled CL we employthe DMV approach of [11]. For notational simplicity, ouralgorithm is depicted for when there is a single relative mea-surement taken by agent i at each time. To process multipleconcurrent relative measurements, we use sequential updating(see [36, page 103]). That is, agent i first collects the localbelief of the agents that it has taken relative measurementsfrom at time t . Then, it processes them via our proposedmethods one after the other by using its previously updatedbelief as its local belief. In what follows, we explain thecomponents of the AUCL algorithm. predictBelief function ( p ( x i ( t ) | Z i t − )) ← p ( x i ( t − | Z i t − ) ): At each time step t ∈ Z + , the dead-reckoningsystem (e.g., INS or odometery) of each agent i propagates anestimate of the ego state ˆx i – ( t ) = f i ( ˆx i + ( t − , u i ( t )) ∈ R n x and the corresponding positive definite error covariance matrix P i – ( t ) ∈ S ++ n x , in a global frame (e.g., the global earth-fixedcoordinate frame with axes pointing north, east and downfor an INS system). This dead-reckoning process is executedthrough the predictBelief function in the AUCL algorithm.When there is no exteroceptive measurement to update thelocal belief, we set bel i + ( t ) = bel i – ( t ) = ( ˆx i – ( t ) , P i – ( t )) ,otherwise we proceed to correct the belief as outlined below. loscorrectBelief function ( p ( x i ( t ) | M ( t ) , Z i t ) ← p ( x i ( t ) | Z i t − ) ): Let the relative range measurement z ij ( t ) by agent i from any mobile agent j be in LoS. Sincethere is no bias in the measurement, to correct the local beliefof agent i using this measurement ( loscorrectBelief functionin Fig. 3), we employ the DMV update. The idea in DMVapproach is that instead of maintaining the cross-covarianceterm P – ij in the joint covariance matrix of any two agents i and j , we use the conservative upper bound below (cid:20) P i – ( t ) P – ij ( t ) P – ij ( t ) (cid:62) P j – ( t ) (cid:21) ≤ (cid:20) ω P i – ( t ) − ω P j – ( t ) (cid:21) , ω ∈ [0 , , (21) to obtain ¯ P i ( ω ) that satisfies E f [( x i − ˆx i + )( x i − ˆx i + ) (cid:62) ] ≤ ¯ P i ( ω ) , and has no dependency on P – ij ( t ) . Then, a ‘minimumvariance’ like update gain K i in (3) is obtained from mini-mizing the trace of ¯ P i ( ω ) . Following [11], the updated beliefby processing LoS measurements bel i + ( t ) = ( ˆx i + ( t ) , P i + ( t )) (subscript 1 is used to represent LoS condition for simplicity)for agent i is (( loscorrectBelief ) function in Fig. 3) ˆx i + = ˆx i – + ¯ K i ( ω i(cid:63) ) ( z ij − ˆ z ij ) , P i + = ¯ P i ( ω i(cid:63) ) , (22)where ¯ K i ( ω ) = P i – ω H i (cid:62) i (cid:0) H ii P i – ω H ii (cid:62) + H ij P j – − ω H ij (cid:62) + R i (cid:1) − . (23)Using this gain that minimizes the trace of ¯ P i ( ω ) , we obtain ¯ P i ( ω ) = (cid:0) ω ( P i – ) − + (1 − ω ) H i (cid:62) i ( H ij P j – H i (cid:62) j + (1 − ω ) R i ) − H ii (cid:1) − . (24)where the optimal ω (cid:63) ∈ [0 , is obtained from ω i(cid:63) = argmin ≤ ω ≤ log det ¯ P i ( ω ) , (25)where H ii = ∂h ( ˆx i – , ˆx j – ) /∂ x i and H ij = ∂h ( ˆx i – , ˆx j – ) /∂ x j areelements of the linearized model of h ( ˆx i – , ˆx j – ) . nloscorrectBelief function ( p ( x i ( t ) | M ( t ) , Z i t ) ← p ( x i ( t ) | Z i t − ) ): Let the relative range measurement z ij ( t ) by agent i from mobile agent j be in NLoS. Toaccount for the measurement bias, recall (1), we use aSKF framework [30]. In SKF, the bias is appended to thestates as a random variable, to account for its uncertaintyand correlation with the state estimate but its value is notupdated based on the measurement feedback. We let thejoint extended state and the prior belief of agent i and j attime t be, respectively, x J ( t ) = ( x i ( t ) (cid:62) , x j ( t ) (cid:62) , b i – ( t )) (cid:62) , andbel – J ( t ) = ( ˆx – J ( t ) , P – J ( t )) where ˆx – J ( t ) = ˆx i – ( t ) ˆx j – ( t )ˆ b i – ( t ) , P – J ( t ) = P i – ( t ) P – ij ( t ) C ii – ( t ) P – ij (cid:62) ( t ) P j – ( t ) C ji – ( t ) C ii – (cid:62) ( t ) C ji – (cid:62) ( t ) B i – ( t ) , (26)where the state-bias cross-covariance terms are C ii – =E f [( x i − ˆx i – )( b i − ˆ b i – )] and C ji – = E f [( x j − ˆx j – )( b i − ˆ b i – )] . Wenote that C ji – (cid:54) = C ij – (cid:62) , because C ij – = E f [( x i − ˆx i – )( b j − ˆ b j – )] , where b j is the bias in the measurements taken by agent j . Finally, note that B i – = E[( b i − ˆ b i – )( b i − ˆ b i – )] = ( ¯ φ i ) +Φ i .Here, E f [ . ] indicates that the expectation is taken over first-order approximate relative measurement or system models.Every agent i maintains and propagates the set of state-bias cross-covariances { C ij – } Ni =1 between its local state andthe bias in the measurements of all the agents { , · · · , N } ,according to C il – = E f [( x i − ˆx i – )( b l − ˆ b l – )] . Thus, C il – ( t + 1) = F i ( t ) C il + ( t ) , l ∈ { , · · · , N } , (27)where F i ( t ) = ∂ f i ( ˆx i + ( t − , u i ( t )) /∂ x i . Initially C il + (0) = , the state-bias cross-covariance terms become non-zero as agents update their states using inter-agent relative measure-ments. As seen in (27), the propagated state-bias cross covari-ance terms of agent i are computed locally. We show belowalso that these terms can be updated using local variables ofagent i and the state-bias cross-covaraince terms of agent j .Thus, using the DMV type approach, we only need to accountfor lack of knowledge of P – ij , when we want to update statesof agent i . We note that since (21) holds, we can also write P – J ( t ) ≤ ω P i – ( t ) ii – ( t ) − ω P j – ( t ) C ji – ( t ) C ii – (cid:62) ( t ) C ji – (cid:62) ( t ) B i – ( t ) , ω ∈ [0 , . (28)Then, by taking into account that in the SKF framework, agent i updates its extended prior states according to (3) and ˆ b i + ( t ) = ˆ b i – ( t ) , B i + ( t ) = B i – ( t ) . (29)Note that E f [( x i − ˆx i + )( x i − ˆx i + ) (cid:62) ] ≤ ¯ P i ( ω, K i ) = (cid:2) ( I − K i H ii ) − K i H ij − K i (cid:3) ω P i – ii – − ω P j – C ji – C ii – (cid:62) C ji – (cid:62) B i × (cid:2) ( I − K i H ii ) − K i H ij − K i (cid:3) (cid:62) + K i R i K i (cid:62) (30)for any ω ∈ [0 , , where we used the first-order expansion of h ( x i , x j ) about ˆx – J described by h ( x i , x j ) ≈ h ( ˆx i – , ˆx j – ) + H ii ( x i − ˆx i – ) + H ij ( x j − ˆx j – ) + ( b i − ˆ b i ) . The gain is foundby minimizing the mean square error of the upper bound (30) ¯ K i ( ω ) = argmin K i Tr ( ¯ P i ( ω, K i )) , which gives us ¯ K i ( ω ) =( 1 ω P i – H ii (cid:62) + C ii – ) S ik − , (31)where S lk = H ii P i – ω H ii (cid:62) + H ij P j – − ω H ij (cid:62) + H ii C ii – + H ij C ji – + C ii – (cid:62) H ii (cid:62) + C ji – (cid:62) H ij (cid:62) + B i + R i . Using this gain, ¯ P i ( ω, ¯ K i ( ω )) in (30) reads as ¯ P i ( ω ) = ¯ P i ( ω, ¯ K i ( ω )) = P i – ω − ( P i – ω H i (cid:62) i + C ii – ) × S ij − ( P i – ω H i (cid:62) i + C ii – ) (cid:62) . (32)We obtain the optimal ω ∈ [0 , from ω i(cid:63) = argmin ≤ ω ≤ log det ¯ P i ( ω ) . (33)Subsequently, the SKF based nloscorrectBelief updated beliefbel i + ( t ) = ( ˆx i + ( t ) , P i + ( t )) for agent i is ˆx i + = ˆx i – + K i ( z ij − ˆ z ij ) , (34a) P i + = ¯ P i ( ω i(cid:63) ) , (34b)while the bias is updated according to (29). The nloscorrentBelief corresponds to the model 2 basedupdate ins Fig 2. Moreover, the state-bias cross-covariancesare updated according to C il + = E f [( x i − ˆx i + )( b l − ˆ b l + )] =E[(( I − K i H ii ) ˜x i – − K i H ij ˜x j – − K i ˜ b i – )˜ b l – ] , where ˜x k – = ( x k − ˆx k + ) and ˜ b k – = ( b k − ˆ b k – ) , k ∈ { , · · · , N } .Then, we can write C il + = (cid:40) ( I − K i H ii ) C ii – − K i H ij C ji – − K i B i + , l = i ( I − K i H ii ) C il – − K i H ij C jl – , l (cid:54) = i for any l ∈ { , · · · , N } . Here, K i is given by (31) evaluatedat ω (cid:63) in (33). PredictBias function:
This function given by (27) propagatesthe set { C il – } Nl =1 of state-bias cross-covariances between thelocal state of agent i and the bias in the measurements of allthe agents { , · · · , N } locally. losProbability and nlosProbability functions: These functionscalculate the model n ∈ { , } probability evolution in Fig. 2,and their function is given by (18). combination function: This function realizes the last step inthe IMM-based estimator of Fig. 2 given by (20). Consideringa Gaussian process, the combined belief bel i + ( t ) accordingto (20) is given by ˆx i + ( t ) = (cid:88) n =1 P ( M n ( t ) | Z t ) ˆx i + n ( t ) , (35a) P i + ( t ) = (cid:88) n =1 P ( M n ( t ) | Z t )( P i + n ( t ) + ¯P in ( t )) , (35b)where ¯P n ( t ) = ( ˆx i + n ( t ) − ˆx i + ( t ))( ˆx i + n ( t ) − ˆx i + ( t )) (cid:62) . Thestate-bias cross-covariance is affected by the combinationof state and becomes C il + ( t ) = P ( M | Z t ) C il + ( t ) , l ∈{ , · · · , N } . Inter-agent communication : To perform loscorrectBelief function the local belief bel j – of agent j should be com-municated to agent i . To preform nloscorrectBelief function,besides the local belief bel j – , agent j should transmit its state-bias correlation set col i – = { C jl – } Nl =1 to agent i , as well. Remark 4.1 (Reducing the communication message size ofthe AUCL algorithm):
To reduce the communication messagesize/cost, we can allow agents to drop exact tracking ofthe inter-agent state-bias cross-covariance terms, and insteadaccount for them implicitly. To do so, we write the joint extendstate of agent i and j as x J ( t ) = ( x i ( t ) (cid:62) , b i – ( t ) , x j ( t ) (cid:62) ) (cid:62) , withthe corresponding joint belief bel – J ( t ) = ( ˆx – J ( t ) , P – J ( t )) , where P – J ( t ) = (cid:34) P i – ( t ) C ii – ( t ) P – ij ( t ) C ii ( t ) (cid:62) B i – ( t ) C ji – ( t ) (cid:62) P – (cid:62) ij C ji – ( t ) P j – ( t ) (cid:35) . Then to account for lackof knowledge about C ji – , we use the upper bound on P – J ( t ) in P – J ( t ) ≤ ω (cid:20) P i – ( t ) C ii – C ii ( t ) (cid:62) B i – ( t ) (cid:21) − ω P j – ( t ) , ω ∈ [0 , . (36)to obtain a ¯ P i ( ω, K i ) that satisfies E f [( x i − ˆx i + )( x i − ˆx i + ) (cid:62) ] ≤ ¯ P i ( ω, K i ) and does not depend on P – ij and C ji – . Then, wecan obtain the update gain and the subsequent updates estimate and the covariance from a process similar to the one thatfollows (30). Next, we note that E f [( x J − ˆx + J )( x J − ˆx + J ) (cid:62) ] ≤ ( I − K J H J ) ω (cid:63) (cid:20) P i – ( t ) C ii – C ii ( t ) (cid:62) B i – ( t ) (cid:21) − ω (cid:63) P j – ( t ) × ( I − K J H J ) (cid:62) + K J R i K J (cid:62) , (37)where K J = (cid:104) K i (cid:62) (cid:105) (cid:62) and H J = (cid:2) H ii H ij (cid:3) .Here, recall that x j + ( t ) = x j – ( t ) , and ˆ b i + ( t ) = ˆ b i – ( t ) .Then to update the state-bias covariance for agent i , weuse the corresponding component of the conservative upperbound in (37), which reads as C ii + ( t ) C ii + ( t ) = ω (cid:63) ( I − K i H ii ) C ii – − ω (cid:63) K i B i + . (cid:3) Update with respect to beacons : Similar as the relative rangemeasurement with respect to a mobile agent, we follow theIMM estimator in Fig. 2 to process the range measurementswith respect to beacons. Since the position of beacons are ex-actly known without the involvement of uncertainty, we simplyemploy the update step of EKF for LoS correction and employthe NLoS correction from our previous work [28]. The onlycorrelation needs to update is C ii – = E f [( x i − ˆx i – )( b i − ˆ b i – )] .We close this section with the following lemma that showsthat if the measurement model is known deterministically, ourproposed IMM-based CL gives the same updated estimate thatthe processing based on the known mode gives. Therefore,we can conclude that the IMM-based CL is the more generalmethod to treat the UWB ranging correction feedback. Lemma 4.1:
If the measurement model at any time t canbe identified with absolute certainty, i.e., p n ( t ) = 0 or , n ∈ { , } , the IMM-based CL update is equivalent to simplyswitch between loscorrectBelief and nloscorrectBelief . Proof:
Consider bel l – ( t ) for l ∈ { i, j } , and letthe inter-agent range measurement z ij ( t ) detected at time t be identified with absolute certainty as NLoS, i.e., p ( t ) = 0 , p ( t ) = 1 . Substituting (14) into (18), wehave that P ( M ( t ) | Z t ) = P ( z ij ( t ) | M n ( t ) , Z i t ) p n ( t ) (cid:80) m =1 P ( z ij ( t ) | M m ( t ) , Z i t ) p m ( t ) , or P ( M ( t ) | Z i t ) = 0 , P ( M ( t ) | Z i t ) = 1 . Thus, from (35a), weobtain bel i + ( t ) = bel i + ( t ) . The same argument applies if themeasurement is known to be in LoS. (cid:3)
V. E
XPERIMENTAL EVALUATIONS
We demonstrate the performance of our proposed AUCLalgorithm via two experiments for a group of pedestrianswho use UWB relative range measurements among themselvesto improve their shoe-mounted INS system geolocation. Theportable localization unit, shown in Fig. 4, that is used inthese experiments consists of a foot-mounted IMU (VectorNavVN-100) and an UWB transceiver (DecaWave DWM1000)connected to a computing unit with a portable battery.
First Experiment : Our first experiment was conducted on thesecond floor of the Engineering Gateway Building at the UCI Fig. 4 –
The portable localization unit used in the experiment. TheIMU mounts on the shoe.
Fig. 5 –
The localization result (trajectories, and the loop-closureerrors in terms of percentage of distance traveled) of a field testingwith pedestrian agents and two beacons with known location. Thethree plots in the bottom show the NLoS probability of, respectively,inter-agent, agents to B1, and agents to B2 measurements. campus (an indoor environment) with the floor plan that isshown in Fig. 6. In this experiment, two pedestrians walkedalong a pre-defined reference trajectory shown by the blacksolid plot in Fig. 6. They started from the black cross and gocounter-clockwise. In this experiment, only agent has accessto the beacon with a known position outside of the building.Beacon used to let agent have a better localization accuracythan agent . Then, agent improves its own localizationestimate by processing inter-agent range measurements withrespect to agent . Since in this particular setup, we knowa priori that the beacon is outside of the building, and thus Fig. 6 – The localization result (trajectory and loop-closureerror) of the first experiment in an indoor environment. Theplot in the bottom shows the NLoS probability of inter-agentmeasurements and the gap in the plot is because the two agentswere out of the sensing range of each other.all the measurements between agent and the beacon shouldbe in NLoS, agent processes measurements with respectto the beacon with p ( t ) = 1 at any time t it has accessto such measurements. In this experiment, agent uses onlythe measurements from the beacon to improve its localizationaccuracy (red trajectory in Fig. 6, with legend ‘Deterministic’).In case of inter-agent ranging between agent and , we donot have any a priori knowledge about the UWB ranging modeat each time. Using a power-based UWB modal discriminator,the probability of the measurements between agents and being in NLoS is shown in the bottom plot of Fig. 6.As we can see by employing the AUCL algorithm, agent obtains a better localization (the blue trajectory with the legend‘AUCL’, in comparison to using a threshold to identify exactlythe model of the inter-agent measurements and deterministicprocessing of the identified mode (the red plot with the legend‘Deterministic’). The green trajectory with the legend ‘NaiveUWB’ shows the localization performance of a filter thatignores the bias in the NLoS measurements. As we can see,in case of agent the performance of this filter is even worsethan the performance of INS only localization, because all themeasurements between agent and the beacon are in NLoSand ignoring the bias in the measurements has a significantdegrading effect. A video presentation of this experiment isavailable at [39]. Second experiment : In our second experiment, a team of threepedestrian agents walked along a reference trajectory, whichwas in the outer path around the pool in Fig. 5 with a length ofabout meters. Each agent was equipped with the portablelocalization unit shown in Fig. 4. Two beacons (B1 and B2)were placed along the path at known locations as shown inFig. 4. The inter-agent and the agents to the beacons rangemeasurement mode was not known a priori and dependingon where the agents were with respect to each other themeasurement could be LoS or NLoS. The power-based modaldiscriminator was used to identify the probability of eachmeasurement mode. The bottom three plots in Fig. 5 show the probability that the measurements are in NLoS during thetest based on the power-based UWB modal discriminator. Inthis experiment, agent and agent started walking from thesame point in the opposite direction. Agent waited alongthe path of agent and started later at the time when agent got closer. The experiment stopped when agent and agent returned to the starting point so we use the loop closure errorof these two agents as our performance indicator. We run fourparallel localization filters on each agent. For all three agents,the INS only localization using the foot-mounted IMUs dueto the error accumulation results in the trajectories that drift,as shown in the blue solid plot, with legend ‘INS only’ inFig. 5. To bound the error, relative range measurements whenagents were in the measurement range of each other wereprocessed to update the local estimates obtained from INS.Due to the existence of obstruction in between agents suchas bushes, trees, swimming pool equipment, and people, themeasured relative range measurements were under a mix ofLoS and NLoS conditions. Ignoring the bias in the measure-ments resulted in poor localization accuracy and even filterdivergence as the black dotted plot with the legend ‘NaiveUWB’ in Fig. 5. On the other hand, as seen in Fig. 5, AUCLalgorithm, the red plot with legend ‘AUCL’, by employing biascompensation and also taking into account the probabilisticnature of the power-based UWB modal discriminator deliversthe best localization and smallest loop closure error, which isexpressed in terms of the percentage of the distance traveled.The trajectories in magenta with legend ‘Deterministic’ showsthe performance of the CL AUCL algorithm when we usedeterministic identification using a threshold to identify theUWB measurement mode with absolute certainty ( p = 0 or p = 1 ). As we can see ignoring the probabilistic nature ofmodal discriminator results in poorer localization performance.A video presentation of this experiment is available at [40].VI. C ONCLUSIONS
We proposed an adaptive UWB based cooperative localiza-tion solution for applications where maintaining network-wideconnectivity is challenging. Our design included a proper biascompensation for NLoS inter-agent UWB range processing,and also took into account the probabilistic multi-modal natureof UWB inter-agent range measurements. We used the IMMmethod to seamlessly handle the measurement model switch-ing between LoS and NLoS in the UWB range measurementsand used the Schmidt Kalman filtering for bias compensa-tion. We incorporated IMM filtering and bias compensationelements in the framework of a loosely coupled cooperativelocalization algorithm, that serves as an augmentation atopof a dead-reckoning system such as INS in a loose couplingmanner. For each agent, this augmentation becomes active onlywhen the agent takes a relative UWB range measurement withrespect to another mobile agent or a beacon. To process themeasurement, the agent needs only to communicate with theagent it has taken the measurements from. Our cooperative lo-calization solution also is a practical sub-optimal solution witha low computational complexity, which can be implementedin real-time on a single computing board. We demonstrated the effectiveness of our method via a real-time localization ofa pedestrian using an experimental setup.R
EFERENCES[1] D. Titterton and J. L. Weston,
Strapdown inertial navigation technology ,vol. 17. IET, 2004.[2] S. Thrun, W. Burgard, and D. Fox,
Probabilistic robotics . Cambridge,Mass.: MIT Press, 2005.[3] J. Leonard and H. F. Durrant-Whyte, “Mobile robot localization by track-ing geometric beacons,”
IEEE Transactions on Robotics and Automation ,vol. 7, pp. 376–382, June 1991.[4] G. Dissanayake, P. Newman, H. F. Durrant-Whyte, S. Clark, andM. Csorba, “A solution to the simultaneous localization and map build-ing (SLAM) problem,”
IEEE Transactions on Robotics and Automation ,vol. 17, no. 3, pp. 229–241, 2001.[5] S. S. Kia, S. Rounds, and S. Mart´ınez, “Cooperative localization formobile agents: a recursive decentralized algorithm based on Kalmanfilter decoupling,”
IEEE Control Systems Magazine , vol. 36, no. 2,pp. 86–101, 2016.[6] A. Bahr, M. R. Walter, and J. J. Leonard, “Consistent cooperativelocalization,” in
IEEE Int. Conf. on Robotics and Automation , (Kobe,Japan), pp. 8908–8913, May 2009.[7] P. O. Arambel, C. Rago, and R. K. Mehra, “Covariance intersection al-gorithm for distributed spacecraft state estimation,” in
American ControlConference , (Arlington, VA), pp. 4398–4403, 2001.[8] L. C. Carrillo-Arce, E. D. Nerurkar, J. L. Gordillo, and S. I. Roumeliotis,“Decentralized multi-robot cooperative localization using covarianceintersection,” in
IEEE/RSJ Int. Conf. on Intelligent Robots & Systems ,(Tokyo, Japan), pp. 1412–1417, 2013.[9] H. Li and F. Nashashibi, “Cooperative multi-vehicle localization usingsplit covariance intersection filter,” in
IEEE Intelligent Vehicle Sympo-sium , (Spain), pp. 211–216, 2012.[10] D. Marinescu, N. O’Hara, and V. Cahill, “Data incest in cooperativelocalisation with the common past-invariant ensemble Kalman filter,”(Istanbul, Turkey), pp. 68–76, 2013.[11] J. Zhu and S. S. Kia, “Cooperative localization under limited connec-tivity,”
IEEE Transactions on Robotics , vol. 35, no. 6, pp. 1523–1530,2019.[12] S. J. Julier and J. K. Uhlmann, “A non-divergent estimation algorithm inthe presence of unknown correlations,” in
American Control Conference ,vol. 4, pp. 2369–2373, Jun 1997.[13] S. J. Julier and J. K. Uhlmann,
Handbook of Data Fusion . CRC Press,2001.[14] J. Kim and H. Jun, “Vision-based location positioning using augmentedreality for indoor navigation,”
IEEE Transactions on Consumer Elec-tronics , vol. 54, no. 3, pp. 954–962, 2008.[15] R. Xiong, S. V. Waasen, C. Rheinl¨ander, and N. Wehn, “Developmentof a novel indoor positioning system with mm-range precision based onRF sensors network,”
IEEE Sensors Letters , vol. 1, no. 5, pp. 1–4, 2017.[16] L. Yang and G. B. Giannakis, “Ultra-wideband communications: an ideawhose time has come,”
IEEE signal processing magazine , vol. 21, no. 6,pp. 26–54, 2004.[17] F. Zafari, A. Gkelias, and K. K. Leung, “A survey of indoor localizationsystems and technologies,”
IEEE Communications Surveys & Tutorials ,2019.[18] B. Denis, J. Keignart, and N. Daniele, “Impact of NLOS propagationupon ranging precision in UWB systems,” in
IEEE conference on UltraWideband Systems and Technologies, 2003 , pp. 379–383, IEEE, 2003.[19] Z. Zeng, S. Liu, and L. Wang, “UWB/IMU integration approach withNLOS identification and mitigation,” in , pp. 1–6, IEEE, 2018.[20] N. Rajagopal, P. Lazik, N. Pereira, S. Chayapathy, B. Sinopoli, andA. Rowe, “Enhancing indoor smartphone location acquisition using floorplans,” in , pp. 278–289, IEEE, 2018.[21] M. Xia, C. Xiu, D. Yang, and L. Wang, “A novel PDR aided UWB indoorpositioning method,” in , pp. 1–7, IEEE, 2018.[22] B. Alavi and K. Pahlavan, “Modeling of the TOA-based distancemeasurement error using UWB indoor radio measurements,”
IEEEcommunications letters , vol. 10, no. 4, pp. 275–277, 2006. [23] S. Wu, Y. Ma, Q. Zhang, and N. Zhang, “NLOS error mitigation forUWB ranging in dense multipath environments,” in
Wireless Com-munications and Networking Conference, 2007. WCNC 2007. IEEE ,pp. 1565–1570, IEEE, 2007.[24] S. Marano, W. M. Gifford, H. Wymeersch, and M. Z. Win, “NLOSidentification and mitigation for localization based on UWB experimen-tal data,”
IEEE Journal on Selected Areas in Communications , vol. 28,no. 7, 2010.[25] Q. Zhang, D. Zhao, S. Zuo, T. Zhang, and D. Ma, “A low complexityNLOS error mitigation method in UWB localization,” in , pp. 1–5,IEEE, 2015.[26] K. Wen, K. Yu, and Y. Li, “NLOS identification and compensation forUWB ranging based on obstruction classification,” in
Signal ProcessingConference (EUSIPCO), 2017 25th European , pp. 2704–2708, IEEE,2017.[27] J. Zhu and S. Kia, “UWB-based infrastructure-free cooperative nav-igation with NLoS ranging bias compensation for indoor pedestriangeolocation,” in
International Technical Meeting of The Institute ofNavigation , pp. 334–344, 2019.[28] J. Zhu and S. S. Kia, “Bias compensation for UWB ranging forpedestrian geolocation applications,”
IEEE Sensors Letters , vol. 3, no. 9,pp. 1–4, 2019.[29] J. Zhu and S. S. Kia, “UWB ranging aided pedestrian geolocation withGPB-based filtering for LoS and NLoS measurement processing,” in ,pp. 781–787, IEEE, 2020.[30] Y. Bar-Shalom, P. K. Willett, and X. Tian,
Tracking and Data Fusion:A Handbook of Algorithms . Storrs, CT: Yaakov Bar-Shalom, 2011.[31] R. Y. Novoselov, S. M. Herman, S. M. Gadaleta, and A. B. Poore,“Mitigating the effects of residual biases with Schmidt-Kalman filtering,”in , vol. 1,pp. 8–pp, IEEE, 2005.[32] K. Gururaj, A. K. Rajendra, Y. Song, C. L. Law, and G. Cai, “Real-time identification of NLOS range measurements for enhanced UWBlocalization,” in
International Conference on Indoor Positioning andIndoor Navigation (IPIN), , pp. 1–7, IEEE, 2017.[33] Y. Bar-Shalom and X. R. Li,
Multitarget-Multisensor Tracking: Princi-ples and Techniques . Storrs,CT: Yaakov Bar-Shalom, 1995.[34] X.-R. Li and Y. Bar-Shalom, “Multiple-model estimation with variablestructure,”
IEEE Transactions on Automatic control , vol. 41, no. 4,pp. 478–493, 1996.[35] H. A. Blom and Y. Bar-Shalom, “The interacting multiple modelalgorithm for systems with markovian switching coefficients,”
IEEEtransactions on Automatic Control , vol. 33, no. 8, pp. 780–783, 1988.[36] Y. Bar-Shalom, P. K. Willett, and X. Tian,
Tracking and data fusion ,vol. 11. YBS publishing Storrs, CT, USA:, 2011.[37] A. Papoulis and S. U. Pillai,
Probability, random variables, and stochas-tic processes . Tata McGraw-Hill Education, 2002.[38] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan,