Tracking Algorithms for Multipath-Aided Indoor Localization

Tracking Algorithms for Multipath-Aided Indoor Localization Paul Meissner and Klaus Witrisal Graz University of Technology, Austria th UWB Forum on Sensing and Communication, May 5, Meissner, Witrisal UWB Forum Slide /9

Presentation overview Virtual-anchor based localization Statistical modeling of influences on multipath-aided localization Tracking algorithms to enhance robustness Performance results and discussion Meissner, Witrisal UWB Forum Slide /9

Virtual Anchor based Localization Scenario Scenario Room, floor plan given One single anchor node (known location) Range-based localization 35 3 5 5 5 Room Agent pos. Anchor Doors 5 5 3 5 Meissner, Witrisal UWB Forum Slide 3/9

Virtual Anchor based Localization Scenario Scenario Room, floor plan given One single anchor node (known location) Range-based localization Multipath propagation Signal reaches agent also via reflections Reflections mapped to anchors outside room Location computable virtual anchors (VAs) 35 3 5 5 5 5 5 Room Agent pos. Anchor Doors 3 5 Meissner, Witrisal UWB Forum Slide 3/9

Virtual Anchor based Localization Overview How to localize? Usage of UWB signals Multipath components resolvable Perform ranging to each VA Multipath extraction CIR [db] 9 3 UWB CIR Extract N pseudoranges z,...,z N, possible errors:. Mapping z i to VAs. False positive detections 3. Obstructed, invisible VAs 5 5 5 3 τ [ns] Figure: UWB-CIR, BW 7.5 GHz Meissner, Witrisal UWB Forum Slide /9

Virtual Anchor based Localization Overview How to localize? Usage of UWB signals Multipath components resolvable Perform ranging to each VA CIR [db] 9 UWB CIR z i (extracted) Multipath extraction 3 z z z 3 z z 5 z z 7 Extract N pseudoranges z,...,z N, possible errors:. Mapping z i to VAs. False positive detections 3. Obstructed, invisible VAs 5 5 5 3 τ [ns] Figure: UWB-CIR, BW 7.5 GHz Meissner, Witrisal UWB Forum Slide /9

Virtual Anchor based Localization Overview How to localize? Usage of UWB signals Multipath components resolvable Perform ranging to each VA CIR [db] 9 UWB CIR z i (extracted) VA delays Multipath extraction 3 z z z 3 z z 5 z z 7 Extract N pseudoranges z,...,z N, possible errors:. Mapping z i to VAs. False positive detections 3. Obstructed, invisible VAs VA VA 3 VA VA VA VA 5 5 5 5 3 τ [ns] Figure: UWB-CIR, BW 7.5 GHz Meissner, Witrisal UWB Forum Slide /9

Statistical model for ranges and ML estimation () A model accounting for uncertainties in z i is p zi p(z i p) = P VA v k N(z i p p k,σk )+( P VA)p zi,va (z i) k p... Position variable PVA...Prob. that z i corresponds to VA vk... Prob. that k-th VA is visible pk... Position of k-th VA.5..35.3 p(z i p) p(z i p).5..5..5 z [m] Meissner, Witrisal UWB Forum Slide 5/9

Statistical model for ranges and ML estimation () A model accounting for uncertainties in z i is p zi p(z i p) = P VA v k N(z i p p k,σk )+( P VA)p zi,va (z i) k p... Position variable PVA...Prob. that z i corresponds to VA vk... Prob. that k-th VA is visible pk... Position of k-th VA VA-caused z i noisy observations of distance to VA p(z i p).5..35.3.5..5..5 p(z i p) z [m] Meissner, Witrisal UWB Forum Slide 5/9

Statistical model for ranges and ML estimation () A model accounting for uncertainties in z i is p zi p(z i p) = P VA v k N(z i p p k,σk )+( P VA)p zi,va (z i) k p... Position variable PVA...Prob. that z i corresponds to VA vk... Prob. that k-th VA is visible pk... Position of k-th VA.5..35.3 p(z i p) VA-caused z i noisy observations of distance to VA Non-VA-matched z i (false detections) p(z i p).5..5..5 z [m] Meissner, Witrisal UWB Forum Slide 5/9

Statistical model for ranges and ML estimation () A model accounting for uncertainties in z i is p zi p(z i p) = P VA v k N(z i p p k,σk )+( P VA)p zi,va (z i) k p... Position variable PVA...Prob. that z i corresponds to VA vk... Prob. that k-th VA is visible pk... Position of k-th VA.5..35.3 p(z i p) VA-caused z i noisy observations of distance to VA Non-VA-matched z i (false detections) No mapping of z i to VA assumed, VAs might be invisible p(z i p).5..5..5 z [m] Meissner, Witrisal UWB Forum Slide 5/9

Statistical model for ranges and ML estimation ()...multimodality, straightforward ML leads to large outliers ML estimator maximizes the objective N ˆp ML = arg max p z p(z p) = arg max p zi p(z i p) p p i= 5 TX RX CDF.... ML estimate 5 5 5 5 5 Normalized Histogram.....5.5.5 3 3.5.5 5 Err p [m] Err p,x,ml Cauchy fit 5 5 5 5 5 5 Err p,x,ml [m] Meissner, Witrisal UWB Forum Slide /9

How to obtain an accurate and robust estimator? Introduce position prior PDF and perform MAP estimation ˆp MAP = arg max p p(p)p z p (z p) p Mitigation of the multimodality Prior information by propagation of position estimate from one time step to the next for a moving agent Use a standard state-space model x k+ = f(x k,w k ) y k = h(x k,v k ) Include statistical models in these equations Meissner, Witrisal UWB Forum Slide 7/9

Moving agent Agent moves along trajectory Characterized by motion model T x k+ = T x k +w k } {{ } F x k = [p x, p y, v x, v y ] T k Models e.g. pedestrian motion Prior knowledge by correlation of successive positions and finite agent velocity v x,k, v y,k, [m/s] 3 Room Trajectory Doors 5 5 5 5 3 v x,k v y,k v k Velocities 3 5 7 9 k Figure: Example trajectory and velocities Meissner, Witrisal UWB Forum Slide /9

State-space model and estimation Noise models Probabilistic description of observation equation y k = h(x k,v k ) via PDFs of noise v k : Two possibilities for measurements y k ML-estimates: Heavy-tail distribution Model: Bivariate symmetric Cauchy distribution Pseudoranges: No deterministic linear h(xk, v k ) Model: Likelihood function p z p (z p) Both issues render a Kalman filter useless Meissner, Witrisal UWB Forum Slide 9/9

Modifying the Kalman filter to alleviate outliers ML-estimates as y k KF is heavily influenced Assume ˆx + k is good Next position probably in vicinity prior knowledge 9 7 5 Tx Room Trajectory p ML 3 Figure: Trajectory and ML estimates Meissner, Witrisal UWB Forum Slide /9

Modifying the Kalman filter to alleviate outliers ML-estimates as y k KF is heavily influenced 9 Tx Room Trajectory p ML Assume ˆx + k is good Next position probably in vicinity prior knowledge Place Gaussian position prior p k (p) over predicted position 7 5 3 ˆx k ˆx + k Figure: k k + (Prediction) Meissner, Witrisal UWB Forum Slide /9

Modifying the Kalman filter to alleviate outliers ML-estimates as y k KF is heavily influenced Assume ˆx + k is good Next position probably in vicinity prior knowledge Place Gaussian position prior p k (p) over predicted position Refine the ML-measurement: y k = arg max p p z p(z p) 9 7 5 3 Figure: log-likelihood of z k Meissner, Witrisal UWB Forum Slide /9

Modifying the Kalman filter to alleviate outliers ML-estimates as y k KF is heavily influenced Assume ˆx + k is good Next position probably in vicinity prior knowledge Place Gaussian position prior p k (p) over predicted position Refine the ML-measurement: y k = arg max p p k(p)p z p (z p) Finally, Kalman update 9 7 5 3 Figure: log-likelihood prior Meissner, Witrisal UWB Forum Slide /9

KF with measurement refinement Performance (example) Room Traj. TX Doors 5 5 5 5 3 v x,k, v y,k, [m/s] 3 v x,k v y,k v k Velocities 3 5 7 9 k Figure: Performance of KF, perfect initialization, refinement makes KF a robust estimator, parameter tuning effort reduced Meissner, Witrisal UWB Forum Slide /9

KF with measurement refinement Performance (example) 5 Tx Room Trajectory p ML KF ref 5 5 5 5 5 Figure: Performance of KF, perfect initialization, refinement makes KF a robust estimator, parameter tuning effort reduced Meissner, Witrisal UWB Forum Slide /9

Initialization Gaussian sum filter (GSF) Previous KF variant assumes good estimate at k Initialization? E.g. at doors GSF: M parallel (refined) KFs Posterior PDF of x k mixture of M Gaussians KFs can compute responsibility for current measurement Weights of all but one KF decay quickly 9 7 5 3 Figure: GSF initialization Meissner, Witrisal UWB Forum Slide /9

Initialization Gaussian sum filter (GSF) Previous KF variant assumes good estimate at k 9 Tx Room Trajectory p ML Initialization? E.g. at doors 7 GSF: M parallel (refined) KFs Posterior PDF of x k mixture of M Gaussians KFs can compute responsibility for current measurement Weights of all but one KF decay quickly 5 3 Figure: First ML-estimates Meissner, Witrisal UWB Forum Slide /9

Initialization Gaussian sum filter (GSF) Previous KF variant assumes good estimate at k Initialization? E.g. at doors GSF: M parallel (refined) KFs Posterior PDF of x k mixture of M Gaussians KFs can compute responsibility for current measurement Weights of all but one KF decay quickly KF weights KF.9 KF KF 3..7..5..3.. 3 5 7 9 k Figure: Weights of KFs Meissner, Witrisal UWB Forum Slide /9

Gaussian sum filter Performance (example) 5 Tx Room Trajectory p ML GSF 5 5 5 5 5 Figure: Performance of GSF, random init with M = Meissner, Witrisal UWB Forum Slide 3/9

Gaussian sum filter Performance (example) 7 Tx Room Trajectory p ML GSF 5 3 3 5 7 9 Figure: Performance of GSF, random init with M =, zoom Meissner, Witrisal UWB Forum Slide 3/9

Bayesian State estimation Particle filters KF-based schemes can not fully account for statistical model yk... ML-estimate: Cauchy distribution yk... Pseudorange-vector: Likelihood function p z p (z p) Particle filter for Bayesian state estimation Initialization: Select set of initial particles, equal weight Prediction: Propagate each particle via state equation Update: Likelihood p(y k particle) for measurement. ML-estimates: Bivariate Cauchy distr. centered at ML-est.. Pseudoranges: Value of p z p (z p) at particle Resampling: Posterior particles drawn from predicted ones according to likelihoods Compute estimate from particles (e.g. x and y median) Meissner, Witrisal UWB Forum Slide /9

Example Particle filter with pseudorages........ p x p y Meissner, Witrisal UWB Forum Slide 5/9

Particle filters Performance (example) 5 Tx Room Trajectory p ML PF range 5 5 5 5 5 Figure: Performance of PF with pseudorange measurements Meissner, Witrisal UWB Forum Slide /9

Particle filters Performance (example) 5 Tx Room Trajectory p ML PF ML 5 5 5 5 5 Figure: Performance of PF with ML measurements, Cauchy model Meissner, Witrisal UWB Forum Slide /9

All estimators Performance comparison.9..7. CDF.5..3.. ML KF ref GSF PF range PF ML.5.5.5 3 Err p [m] Figure: Average performance for 5 runs over trajectory Meissner, Witrisal UWB Forum Slide 7/9

All estimators Performance Correlated Visibilities We expect VA-visibilities to have correlated behavior KFs drawn away from true trajectory (prior is wrong) PFs show less sensitivity.9..7. CDF.5..3. ML KF ref, iid PFrange, iid PFML, iid KF ref, HMM. PFrange, HMM PFML, HMM.5.5.5 3 Err p [m] Meissner, Witrisal UWB Forum Slide /9

Conclusions and ongoing work Concept for multipath-aided localization and tracking Dealing with multimodal/heavy-tail models in our concept Variants of state-space estimators to gain robustness Effective use of floor plans and signal reflections Ongoing work Multipath extraction from UWB impulse response Validation/extension of models with measurements Tracking of VAs Thanks for your attention! Meissner, Witrisal UWB Forum Slide 9/9