PaperlayerView exact revision on arXiv

Source-linked AI summary

A Belief Propagation Algorithm for Multipath-Based SLAM

Erik Leitinger, Florian Meyer, Franz Hlawatsch, Klaus Witrisal, Fredrik Tufvesson, Moe Z. Win

arXiv:1801.04463v4cs.ITcs.RO

TL;DR

Robust indoor radio-based SLAM is challenged by harsh multipath, uncertain MPC-feature associations, and limited feature visibility. The paper models specular reflections as virtual anchors and applies belief propagation to a Bayesian factor graph for joint trajectory, anchor, and association estimation. Synthetic and real ultra-wideband experiments report agent position errors below 0.2m for 100% and below 0.083m for 90% of all measurements.

  • Problem

    Robust indoor radio-based SLAM must handle harsh multipath conditions, unknown MPC-feature associations, and limited visibility of features.

  • Method

    A Bayesian factor-graph model uses belief propagation for probabilistic data association and joint sequential estimation of the agent and potential physical- and virtual-anchor states.

  • Results

    Agent position errors were below 0.2m for 100% and below 0.083m for 90% of all real ultra-wideband measurements.

  • Takeaways & Limitations

    The BP-SLAM algorithm estimates agent position and feature maps with high accuracy and robustness in synthetic cluttered conditions and real indoor radio measurements.

Abstract

from arXiv · show

We present a simultaneous localization and mapping (SLAM) algorithm that is based on radio signals and the association of specular multipath components (MPCs) with geometric features. Especially in indoor scenarios, robust localization from radio signals is challenging due to diffuse multipath propagation, unknown MPC-feature association, and limited visibility of features. In our approach, specular reflections at flat surfaces are described in terms of virtual anchors (VAs) that are mirror images of the physical anchors (PAs). The positions of these VAs and possibly also of the PAs are unknown. We develop a Bayesian model of the SLAM problem and represent it by a factor graph, which enables the use of belief propagation (BP) for efficient marginalization of the joint posterior distribution. The resulting BP-based SLAM algorithm detects the VAs associated with the PAs and estimates jointly the time-varying position of the mobile agent and the positions of the VAs and possibly also of the PAs, thereby leveraging the MPCs in the radio signal for improved accuracy and robustness of agent localization. The algorithm has a low computational complexity and scales well in all relevant system parameters. Experimental results using both synthetic measurements and real ultra-wideband radio signals demonstrate the excellent performance of the algorithm in challenging indoor environments.

I. INTRODUCTION

The paper addresses robust indoor radio-based SLAM by modeling specular multipath components as virtual-anchor features and resolving their uncertain associations probabilistically. It develops a Bayesian factor-graph approach with belief propagation for jointly estimating the agent trajectory and map features, and evaluates the method on synthetic and real data.

  • Motivation: Indoor radio-based SLAM remains difficult under harsh multipath conditions, motivating methods that exploit rather than suppress multipath information.The paper identifies diffuse multipath, uncertain MPC-feature association, and limited feature visibility as central challenges.
  • Radio-based SLAM model: Specular multipath components are represented by virtual anchors, mirror images of physical anchors whose positions may be unknown.The algorithm estimates VA positions and possibly PA positions together with the mobile agent trajectory.
  • Method: The proposed Bayesian model represents probabilistic data association and feature existence uncertainty in a factor graph processed by belief propagation.The factor-graph formulation supports joint inference over agent and feature states while exploiting conditional independencies.
  • Method: The algorithm jointly estimates the mobile agent state and the numbers and positions of physical and virtual anchors with low computational complexity and high scalability.It extends the feature-based SLAM framework to probabilistic MPC association and potential features.
  • Scope and novelty: The method is designed for challenging range-only SLAM, where annular feature distributions and ambiguous ranges complicate Gaussian modeling and data association.The paper states that established SLAM techniques do not straightforwardly address this setting.
  • Evaluation: Synthetic and real ultra-wideband experiments demonstrate high accuracy and robustness in challenging indoor environments.The evaluation includes both synthetic measurements and real radio signals.

III. SYSTEM MODEL AND STATISTICAL FORMULATION

The system model represents the mobile agent and a dynamically changing set of potential physical or virtual-anchor features, each with position and existence uncertainty. Legacy features evolve over time, while measurements can initialize new features that are subsequently maintained or pruned.

  • A. Agent State and PF States: The mobile agent state contains its time-varying position and velocity, while potential features are indexed by physical-anchor identity and feature index.The feature set includes physical anchors and virtual anchors associated with them.
  • A. Agent State and PF States: Each potential feature has a position state and a binary existence variable indicating whether the feature is physically present at the current time.Nonexisting features are assigned augmented states for probabilistic inference.
  • A. Agent State and PF States: Potential-feature states are augmented with existence probabilities, allowing the model to represent features that may appear or disappear over time.The probability density for a nonexisting feature uses a dummy state density, while the existence variable carries the relevant probability.
  • A. Agent State and PF States: At each time step, features are classified as legacy or new, with the number of new potential features determined by the current measurements.Newly detected features become new potential features after measurements are observed.
  • A. Agent State and PF States: The potential-feature set is pruned so that features with small existence probabilities are discarded, preventing unchecked growth in the representation.Although new features are introduced from measurements, pruning means the feature count does not simply grow by the number of measurements.
  • A. Agent State and PF States: Legacy potential features and the agent evolve independently according to Markovian state dynamics, including feature survival and state transitions.A feature that does not exist at the previous time cannot persist as a legacy feature, whereas surviving features transition to new states.

B. Association Vectors

The association model treats measurement origins as unknown, explicitly allowing feature-generated detections, false alarms, and missed detections. Two complementary association vectors encode these relationships for scalable joint inference.

  • B. Association Vectors: A feature-oriented association vector records which measurement was generated by each legacy feature, assigning zero when the feature generated no measurement.The two association vectors are equivalent but retain redundant information for BP scalability and factor-graph construction.
  • B. Association Vectors: Each MPC-parameter measurement may originate from a legacy feature, be a false alarm, or correspond to an unobserved feature through a missed detection.The model assumes that each feature generates at most one measurement and each measurement has at most one generating feature.
  • B. Association Vectors: The probability that a potential feature generates a measurement is governed by its detection probability, while false alarms follow a specified probability density.These quantities enter the measurement-origin model as known functions or parameters.
  • B. Association Vectors: A measurement-oriented association vector records which legacy feature generated each measurement, assigning zero when no legacy feature generated it.Its entries represent measurement origins across the measurements associated with a physical anchor.

C. Prior Distributions

The prior model specifies distributions for legacy and newly detected PF states, their existence indicators, measurement counts, and data-association variables conditioned on the agent state.

  • For each PA j, the model tracks legacy PFs, new PFs, existence indicators, and the number of measurements or new PFs.
  • At initialization, prior information about PAs and VAs can be incorporated through legacy PFs, or legacy PFs can be absent.
  • Newly detected feature states are assumed iid, while false alarms and newly detected features follow Poisson distributions with mean µ^(j)_n.
  • The prior for new PF states is conditioned on the agent state, the legacy PF state, and the number of new PFs.
  • The association variables are conditionally independent given the legacy PF state vector, with consistency constraints preventing conflicting measurement assignments.

D. Likelihood Function

The likelihood function characterizes the statistical relation between measurements, the agent state, and PF states, with measurements modeled conditionally independently across PAs.

  • The likelihood function characterizes the statistical relation between measurements and the agent and PF states.
  • The specific dependence on the agent and PF states is determined by the concrete measurement model.
  • Measurements from different PAs are assumed conditionally independent given the agent state, PF states, association variables, and measurement counts.
  • For observed measurements, the likelihood is treated as a function of the agent state, legacy PF states, new PF states, and measurement counts.

IV. JOINT POSTERIOR PDF AND FACTOR GRAPH

The Bayesian model combines likelihoods and priors for new PF states and data association into a factorized representation suitable for factor-graph inference.

  • The BP-SLAM formulation uses a redundant probabilistic data-association representation based on an n-fold Cartesian product.
  • The factor construction combines the measurement likelihood, the prior for new PF states, and a modified prior pmf for association variables.
  • Multiplying the model components yields the factorization used to construct the BP-SLAM factor graph.
  • The modified prior pmf enforces association validity by excluding conflicting assignments and incorporating the exclusion constraint factor.
  • The expected number of newly detected features may be specified in advance or inferred online using a PHD filter.

B. Joint Posterior pdf

The joint posterior is derived from Bayes’ rule and independence assumptions, then factorized for belief-propagation-based detection and estimation of agent and PF states.

  • B. Joint Posterior pdf: Bayes’ rule and independence assumptions produce the joint posterior over agent, feature, association, and measurement-related states across all time steps.
  • B. Joint Posterior pdf: The posterior factorization is represented by the factor graph in Fig. 2.
  • B. Joint Posterior pdf: Belief propagation computes the required posterior distributions efficiently in a time-recursive manner for Bayesian detection and estimation.
  • B. Joint Posterior pdf: The algorithm estimates the agent state and detects and estimates PF states using posterior distributions and posterior existence probabilities.
  • B. Joint Posterior pdf: PFs are detected when their posterior existence probabilities exceed P_det and pruned when they do not exceed P_prun.

B. Message Passing Algorithm

The algorithm uses loopy belief propagation on a factor graph to approximate marginal posteriors that cannot be directly marginalized. Iterative message updates support agent, feature, and data-association estimation.

  • Loopy BP approximates marginal posteriors because direct marginalization of the joint posterior is infeasible.Because the factor graph contains loops, the resulting beliefs are approximations rather than exact posterior distributions.
  • Belief calculation: After message updates, beliefs approximate the agent, legacy-feature, and new-feature marginal posterior pdfs.The agent belief q(x_n) replaces the exact marginal posterior in subsequent calculations.
  • Measurement evaluation and data association: Measurement evaluation and iterative data association calculate messages for feature-oriented and measurement-oriented association variables.The algorithm performs these calculations for legacy and new particle filters associated with physical anchors.
  • Algorithm structure: Algorithm 1 performs prediction, measurement evaluation, iterative data association, measurement updates, belief calculation, and feature detection and estimation.A particle-based sequential Monte Carlo implementation uses a stacked state containing the agent and particle-filter states.
  • The recursive algorithm initializes beliefs at n=1 and propagates agent and feature-state beliefs from the preceding time step.Prediction messages are computed from q(x_n−1) and the preceding particle-filter beliefs.

VI. EXPERIMENTAL RESULTS

The proposed BP-SLAM algorithm is evaluated with synthetic and real measurement data in two-dimensional scenarios. The experiments use parameters listed in Table I.

  • Synthetic and real measurement data are used to evaluate BP-SLAM in two-dimensional scenarios.The algorithm and synthetic-measurement parameters are listed in Table I.

1) State-Evolution Model:

The model uses a near-constant-velocity agent, static particle-filter features, and MPC range measurements with Gaussian noise. Simulations specify feature births, detection, false alarms, initialization, and iterative-message settings.

  • State-Evolution Model: The agent follows a linear near-constant-velocity model with sampling period ΔT = 1s and iid zero-mean Gaussian driving noise.The state transition is x_n = Ax_n−1 + Bw_n, with w_n covariance σ_w^2I_2.
  • State-Evolution Model: Particle-filter features are modeled as static, although implementation adds a small driving process for numerical stability.The added process is iid across particles, time steps, and physical anchors, with zero-mean Gaussian statistics.
  • Measurement Model: The measurement model uses only scalar MPC ranges, with iid zero-mean Gaussian measurement noise and feature- and anchor-dependent variance.The model can be extended to include bearing or inertial-measurement-unit observations.
  • Feature birth and detection: New feature states are inferred online with a PHD filter using a birth density uniform over a circular 30m-radius region of interest.The detection probability is constant across particle filters and physical anchors.
  • Initialization: Initial physical-anchor positions are effectively known with σ_a,1 = 10^-3m, while virtual-anchor states have no prior information.Initial agent particles are drawn from a 4-D uniform distribution centered at the true starting position, with component support [−λ, λ].
  • Initialization: Using only range measurements leaves the estimated map and trajectory ambiguous up to arbitrary translation and rotation relative to the true positions.This ambiguity remains despite reported performance without prior information about initial agent and physical-anchor states.

B. Results for Synthetic Measurements

Synthetic-measurement experiments show that BP-SLAM jointly localizes the mobile agent and estimates associated features with robust accuracy across parameter settings. Measurement gating substantially reduces runtime without noticeable loss in localization or mapping performance.

  • Agent position RMSE remains mostly below 0.12m across settings and below 0.08m for SLAM 1 and SLAM 2.
  • The average detected PF counts are effectively equal to the true feature count L(1)_n = 5 for SLAM 1 and SLAM 2.
  • For SLAM 1, MOSPA errors for both PAs and their associated VAs decrease to below 0.11m.
  • Agent position RMSE converges in all simulation runs and settings, while individual PF position RMSEs are at most 0.25m and often around 0.05m.
  • The known-feature benchmark has significantly lower agent position RMSE, demonstrating the impact of map uncertainty on SLAM performance.
  • Measurement gating reduces average runtime by 20.1%, from 0.0929s to 0.0735s per time step, without noticeable RMSE or MOSPA loss.

2) Comparison with Rao-Blackwellized SLAM:

The paper compares BP-SLAM with Rao-Blackwellized SLAM using synthetic and real radio measurements. BP-SLAM achieves accurate, robust localization while requiring substantially lower computational complexity than RB-SLAM under comparable particle settings.

  • Comparison with Rao-Blackwellized SLAM: RB-SLAM tracks the joint agent-and-feature state with particle filtering, while Rao-Blackwellization exploits conditional feature-state independencies to avoid the curse of dimensionality.The range-only measurement model makes direct joint-state particle filtering typically infeasible.
  • Comparison with Rao-Blackwellized SLAM: BP-SLAM and RB-SLAM are compared on agent position RMSE and PA/VA MOSPA errors across multiple particle settings using synthetic measurements.BP-SLAM uses 50,000, 10,000, or 5,000 particles; RB-SLAM is evaluated with five parameter settings.
  • Comparison with Rao-Blackwellized SLAM: More than 10× lower runtime is achieved by BP-SLAM B than RB-SLAM 1 when both use the same number of particles, with only slightly poorer performance reported for BP-SLAM B.RB-SLAM computes distances between every agent-state particle and every feature-state particle, whereas BP-SLAM has more favorable scaling.
  • Comparison with Rao-Blackwellized SLAM: Increasing RB-SLAM’s agent-particle count improves estimation accuracy only slightly but increases computational complexity significantly.This conclusion is drawn from the runtime results and comparisons in Fig. 5(d)–(f).
  • Comparison with Rao-Blackwellized SLAM: Reducing RB-SLAM’s VA-particle count significantly decreases agent-state and VA-estimation accuracy and can cause divergence when VA particles converge to incorrect positions.For RB-SLAM 4 and RB-SLAM 5, 73% and 87% of simulation runs converged, respectively; plotted errors used only converged runs.
  • Comparison with Rao-Blackwellized SLAM: With real ultra-wideband measurements, BP-SLAM kept the maximum individual-trajectory RMSE below 0.2m in all cases and below 0.083m in 90% of cases.The measurements covered five closely spaced trajectories, each containing 900 agent positions.
Loading 1801.04463v4…