betaFeedback

WorksComputer Science

Radio-to-Pose Information for Gauge-Drift Correction in Cooperative Localization

Abstract

Cooperative localization uses odometry and inter-robot measurements to estimate the motion of a robot team. These measurements describe only relative geometry, but the team's estimated position and orientation can drift together over time, i.e., gauge drift. In this work, we study correction of this gauge drift using a radio attenuation map registered in the same world coordinate frame used for robot poses. Specifically, we propose Radio-to-Pose Information (RPI), an interface that converts power residuals and their pose derivatives into a Gaussian information matrix and vector pair. RPI builds on the insight that received power is sensitive to shared displacement in the world map. The pose estimator uses the interface together with odometry and inter-robot measurements to update poses and minimize uncertainty. We characterize the shared motions detectable from the map and observed links. For an uncertain shared map, RPI keeps correlations across robots and time and reproduces joint Gaussian pose estimates and covariances. Across 10001000 simulated Gaussian trials, RPI matches the results of joint estimation. Across 32 paired trials with a registered map, RPI reduces common-position RMSE from 0.5420.542m to 0.1400.140m in a 50×5050\times50m arena compared with a baseline that uses radio only to weight relative measurements.

Introduction

To carry out tasks such as exploration, mapping, and environmental monitoring, robot teams rely on coordinated motion and shared observations. When global positioning is unavailable, they can estimate motion through odometry and inter-robot measurements. These observations constrain relative geometry, but the estimated team can move away from its true location as a group. For example, translating every robot estimate by one meter leaves all pairwise distances unchanged. This shared error matters when robots must reach mapped destinations or report observations in world coordinates.

It is also important to precisely estimate uncertainty because the team can know its relative geometry while remaining uncertain about its position in the world. This ambiguity is called gauge freedom. Gauge freedom describes indistinguishable coordinate frames, which in the presence of motion or noise leads to gauge drift. Gauge drift describes the temporal growth of error or uncertainty in these shared-motion directions. Teams construct a reference frame from a prior over their initial poses, but subsequent noisy observations determine how well the team remains localized. For trajectories without a fixed point of reference, common translations and rotations maintain relative observations, but they do not provide useful information about global state.

A radio attenuation map aligned to the world frame can provide a reference because received power depends on both link length and the attenuation field. For example, in Figure 1, a common translation preserves separation and distance loss but changes link attenuation. Using a global attenuation map which captures knowledge of obstacles in the environment can help distinguish the team’s positions by using differences in the received power they observe.

Figure 1. In the left map, when there is no registered attenuation map, equal-length links have identical distance-only predictions. In the right map, when a registered map is present, equal-length links have different power predictions. Colors indicate attenuation in dB/m.

We propose the Radio-to-Pose Information (RPI) interface which leverages this behavior. The RPI is an interface between two components: First, a radio module predicts received power from the current poses and attenuation map, then determines the power mismatch and pose sensitivity as an information matrix and vector. Second, a pose estimator uses this message to compute a correction and its uncertainty.

This paper describes three contributions:

  1. We show that fixed weights on relative measurements add zero direct information about common motion. We characterize which shared motions the attenuation map and observed links can detect.

  2. We propose the Radio-to-Pose Information (RPI) interface, which converts power residuals and pose sensitivity into an information matrix and vector for pose estimation.

  3. We derive centroid covariance growth and a lower bound on mean-squared common-position error under stated motion and measurement assumptions. We evaluate agreement with joint estimation, drift correction, and sensitivity to map error.

Cooperative Localization. Relative measurements help robots estimate their positions with respect to one another, but they do not determine the position and orientation of the entire team in the world frame [1], [2]. Gauge fixing chooses a coordinate frame for estimation [3]. Simultaneous localization and mapping (SLAM) adds persistent environmental features and loop closures, which can reduce trajectory drift [4]. However, a map built only from the robot’s own measurements defines position relative to the robot’s chosen coordinate frame rather than an externally registered world frame. Our comparison concerns a cooperative localizer that uses radio to set fixed weights on relative factors.

Radio and Field Maps. Radio tomography estimates spatial attenuation from power measurements and known link positions [5]. Radio SLAM jointly estimates motion and propagation structure using radio observations, including multipath information [6], [7]. Its reference frame depends on the available anchors and priors. Known physical anchors can provide a reference [7], but may not be available. Instead, this paper focuses on studying inter-robot links against a registered attenuation field. This follows the principle of localization against surveyed terrain or magnetic maps [8], [9].

Gaussian Fusion. Factor-graph smoothers combine measurement models and priors through local linearization [4]. Gaussian elimination provides a way to summarize part of this model for another estimator. Distributed smoothing must account for previously shared information when summaries are replaced [10]. Fusion with shared variables must also preserve the induced correlations or use an appropriate conservative approximation [11], [12]. RPI applies these requirements to a radio module whose uncertain map is shared across robots and time. Equivalent radio factors in a factor-graph smoother give the same local Gaussian result when the models and linearizations agree.

Motion and Radio Model Setup

Shared Motion

In the cooperative localization problem, a robot team seeks to estimate each robot’s trajectory in a fixed world frame over an estimation window. Consider NN robots over a finite set of time steps T={t0,…,tw}\mathcal{T} = \{t_0, \ldots, t_w\}, called the estimation window, where t0t_0 and twt_w are its first and last time steps. Let xi,t=(pi,t,θi,t)∈SE⁡(2)x_{i,t} = (p_{i,t},\theta_{i,t}) \in\operatorname{SE}(2) denote the true pose of robot ii at time t∈Tt \in\mathcal{T}, where pi,t∈R2p_{i,t} \in\mathbb{R}^2 is its position and θi,t\theta_{i,t} is its heading. Let XX collect the poses of all robots across the estimation window, representing their trajectories. Each robot estimates its pose x^i,t\hat{x}_{i,t} using odometry and inter-robot measurements. A centralized server collects these estimates into X^\hat{X} and solves for the team’s trajectories. Both XX and X^\hat{X} contain N(w+1)N(w+1) poses.

In order to represent a gauge drift, a shared rigid transformation g=(Rψ,tg)∈SE⁡(2)g=(R_\psi,t_g)\in\operatorname{SE}(2) rotates every pose by ψ\psi and translates every position by the translation vector tg∈R2t_g\in\mathbb{R}^2, where Rψ=[cos⁡ψ−sin⁡ψsin⁡ψcos⁡ψ]R_\psi=\begin{bmatrix}\cos\psi& -\sin\psi\\ \sin\psi& \cos\psi\end{bmatrix}. Thus, gg is applied to every pose in XX, giving pi,t↦Rψpi,t+tgp_{i,t}\mapsto R_\psi p_{i,t}+t_g and θi,t↦θi,t+ψ\theta_{i,t}\mapsto\theta_{i,t}+\psi. This shared transformation preserves inter-robot distances, relative poses, and motion expressed in each robot’s own frame.

and motion expressed in each robot’s own frame. Let K\mathcal{K} denote the set of available relative measurements over the estimation window, including relative-position measurements between robots and odometry between consecutive time steps, as shown in Figure 2. Each index k∈Kk\in\mathcal{K} identifies one measurement and the number of available measurements is bounded by ∣K∣≤(w+1)N(N−1)2+Nw|\mathcal{K}|\leq(w+1)\frac{N(N-1)}{2}+Nw. For each kk, let rk(X)r_k(X) denote the difference between the recorded sensor measurement and the corresponding value predicted using the robot poses in XX. Since the transformation preserves these predicted values, rk(gX)=rk(X)r_k(gX)=r_k(X). A shared translation changes the team’s position in the fixed world frame, while a shared rotation changes its orientation. To determine which shared motions the measurements can detect, we express small translations and rotations as pose perturbations. About an origin oo, with 90∘90^\circ rotation matrix S=[0−110]S=\begin{bmatrix}0&-1\\1&0\end{bmatrix}, a small common translation aa and rotation ω\omega produce

Figure 2. This graph represents the robot team at each time step. Solid links represent measurements between robots at the same time, while dashed arrows represent odometry of the same robot across consecutive time steps. Each described rk(X)r_k(X) compares a recorded measurement with the measurement predicted by the poses.

δpi,t=a+ωS(pi,t−o),δθi,t=ω,δX=Gξ.(1)\delta p_{i,t}=a+\omega S(p_{i,t}-o),\qquad\delta\theta_{i,t}=\omega,\qquad\delta X=G\xi. \tag*{(1)}

where ξ=(ax,ay,ω)T\xi=(a_x,a_y,\omega)^\mathsf{T} describes the shared translation and rotation applied to all robots and time steps. The columns of G∈R3N(w+1)×3G\in\mathbb{R}^{3N(w+1)\times3} correspond to the two translation directions and rotation.

Radio Power and Reference

For a radio link ℓ=(i,j,t)\ell=(i,j,t) between robots ii and jj at time tt, let di,j,t=∥pj,t−pi,t∥2>0d_{i,j,t}=\lVert p_{j,t}-p_{i,t}\rVert_2>0. The field F(v)F(v) provides attenuation in dB/m at world location v∈R2v\in\mathbb{R}^2. We predict received power by integrating the field over the spatial domain, weighted by each location’s contribution to the link.

μℓ(X,F)=Pref−10γlog⁡10(di,j,t/d0)−∫wℓ(v,X)F(v) dv.\mu_\ell(X,F)=P_{\mathrm{ref}}-10\gamma\log_{10}(d_{i,j,t}/d_0)-\int w_\ell(v,X)F(v)\,\mathrm{d}v.

PrefP_{\mathrm{ref}} is the reference received power in dBm at distance d0=1md_0=1\mathrm{m} without mapped attenuation, and γ\gamma is the path loss exponent governing the propagation decrease in received power with distance. The weighting function wℓ(v,X)w_\ell(v,X) describes each location vv’s contribution to link attenuation, given its position with respect to robot poses. We assume known propagation parameters and Gaussian power noise ϵ∼N(0,Rℓ)\epsilon\sim\mathcal{N}(0,R_\ell), where Rℓ>0R_\ell>0, the radio measurement noise covariance matrix, is independent of the robot poses. The measured power vector yy, pose Jacobian JrJ_r, and radio information matrix MrM_r satisfy

y=μ(X,F)+ϵ,Jr=∂μ∂X,Mr=JrTRr−1Jr.y=\mu(X,F)+\epsilon,\qquad J_r=\frac{\partial\mu}{\partial X},\qquad M_r=J_r^{\mathsf T}R_r^{-1}J_r.

where ϵ∼N(0,Rr)\epsilon\sim\mathcal{N}(0,R_r). For a known field, the radio information along a pose perturbation uu is uTMru=∥Rr−1/2Jru∥2u^{\mathsf T}M_ru=\lVert R_r^{-1/2}J_ru\rVert^2. By registering the field FF, we provide a reference in the world frame. If the weights satisfy wℓ(gv,gX)=wℓ(v,X)w_\ell(gv,gX)=w_\ell(v,X), transforming the poses to X′=gXX'=gX and the map to F′(v)=F(g−1v)F'(v)=F(g^{-1}v) leaves the predicted powers unchanged. Therefore, estimating the poses and map together cannot determine their absolute position and orientation unless the prior provides a world-frame reference. We provide this reference by setting the map’s position and orientation to the world frame.

Error and Map Information

If gauge drift occurs, the robot team’s collective position (i.e., the centroid) relative to the world map becomes uncertain (§IV-A). We show that while existing techniques based on relative measurements alone cannot solve the gauge drift problem for robot teams (§IV-B), it is possible to use radio field attenuation information from the world map to reduce this source of error (§IV-C).

Team Centroid Error Growth

Relative observations do not measure the motion noise with respect to the centroid; as a result, the team cannot correct common-position drift using this kind of observation. To see why, assume each robot’s heading is known and its position evolves in world coordinates. Then, the position of robot ii at time step tt evolves to t+1t+1 by pi,t+1=pi,t+di,t+ni,tp_{i,t+1}=p_{i,t}+d_{i,t}+n_{i,t}, where the process noise ni,t∼N(0,σp2I2)n_{i,t}\sim\mathcal{N}(0,\sigma_p^2I_2), di,td_{i,t} is the displacement given by odometry per time step, and σp2\sigma_p^2 is the motion-noise variance per position coordinate per time step. In this equation, the process noises are independent across robots and time and are independent of the initial state.

Define the team centroid as ct=N−1∑i=1Npi,tc_t=N^{-1}\sum_{i=1}^{N}p_{i,t}, where NN is the number of robots. Assume that c0c_0 is the initial centroid and has covariance Pc0P_{c_0}. The measurement noise is independent of initial robot positions and process noise.

Averaging the robot dynamics gives ct+1=ct+dˉt+nˉtc_{t+1}=c_t+\bar d_t+\bar n_t, nˉt∼N(0,σp2NI2)\bar n_t\sim\mathcal{N}\left(0,\frac{\sigma_p^2}{N}I_2\right), where dˉt=N−1∑idi,t\bar d_t=N^{-1}\sum_i d_{i,t} and nˉt=N−1∑ini,t\bar n_t=N^{-1}\sum_i n_{i,t}. The average motion noise is independent of the motion noise that changes the robots’ relative positions. The observation history does not reduce uncertainty in c0c_0 or the accumulated centroid noise. Thus,

Cov⁡(ct∣Yt)=Pc0+tσp2NI2,\operatorname{Cov}(c_t\mid\mathcal{Y}_t)=P_{c_0}+\frac{t\sigma_p^2}{N}I_2,

where Yt\mathcal{Y}_t denotes the observation history through time tt. For any estimate based on this history,

E∥c^t−ct∥2≥tr⁡Pc0+2tσp2N.\mathbb{E}\lVert\hat c_t-c_t\rVert^2\geq\operatorname{tr}P_{c_0}+\frac{2t\sigma_p^2}{N}.

Relative Weights

Relative observations improve estimates of the robots’ positions with respect to one another. However, using weighted links alone cannot solve this problem.

If rkr_k is invariant under the pose transformation X↦gXX\mapsto gX, differentiating along a gauge direction gives JkG=0J_kG=0, where Jk=∂rk/∂XJ_k=\partial r_k/\partial X. For the joint pose–map transformation above, the corresponding identity is

Jk,XGX+Jk,FGF=0.J_{k,X}G_X+J_{k,F}G_F=0.

The weighted relative information matrix is M(q)=∑kqkJkTRk−1JkM(q)=\sum_k q_kJ_k^{\mathsf T}R_k^{-1}J_k, where qkq_k describes nonnegative radio weights and RkR_k is the indexed measurement noise covariance. Also, GX=GG_X=G from Eq. (1) describes the shared pose motion, and GFG_F describes the corresponding map change caused by the same translation and rotation.

Proposition 1 (Fixed relative weights). For fixed nonnegative radio weights qkq_k on invariant relative measurements with covariances Rk≽0R_k\succcurlyeq0,

GTM(q)G=∑kqk(JkG)TRk−1(JkG)=0.G^{\mathsf T}M(q)G=\sum_kq_k(J_kG)^{\mathsf T}R_k^{-1}(J_kG)=0.

Keeping only a subset of the relative factors still provides no information along the gauge directions.

Proof. Each term in the sum is zero, so keeping only some of the factors does not change the result.

The proposition shows that fixed-weight relative measurements provide no direct information about shared translation or rotation. Additional pose information can come from modeling how received radio power depends on the robot poses.

A registered attenuation map can constrain this common motion when shared displacement changes the power predicted for the observed links. Radio sensitivity to shared motion depends on how that motion changes the attenuation sampled by the observed links. Because a shared rigid motion leaves link length unchanged, only the map attenuation term changes the predicted power.

Assume F∈C1F\in C^1 and wℓ(gv,gX)=wℓ(v,X)w_\ell(gv,gX)=w_\ell(v,X). Then, substituting v=gv′v=gv',

qℓ=∫wℓ(v,gX)F(gv′) dv=∫wℓ(v′,X)F(gv′) dv′,q_\ell=\int w_\ell(v,gX)F(gv')\,dv=\int w_\ell(v',X)F(gv')\,dv',

where v′=g−1vv'=g^{-1}v is the spatial variable before applying the rigid transformation gg. This expresses the attenuation using the original link weights and the field evaluated at the transformed locations. For Vξ(v′)=a+ω×(v′−o)V_\xi(v')=a+\omega\times(v'-o), differentiating along the shared motion ξ\xi at the identity gives

(JrGξ)ℓ=−∫wℓ(v′,X)∇F(v′)TVξ(v′) dv′.(2)(J_rG\xi)_\ell=-\int w_\ell(v',X)\nabla F(v')^{\mathsf T}V_\xi(v')\,dv'. \tag*{(2)}

(JrGξ)ℓ(J_rG\xi)_\ell measures the sensitivity of link ℓ\ell’s predicted power to the shared motion ξ\xi, with a fixed map. Let NF\mathcal{N}_F denote the shared motions satisfying ∇F(v′)TVξ(v′)=0\nabla F(v')^{\mathsf T}V_\xi(v')=0 for all v′v'. The link sensitivity condition requires every motion that the map reveals outside NF\mathcal{N}_F to produce a nonzero first-order change in at least one observed link’s attenuation integral, despite possible missed variation or cancellation in other links.

Proposition 2 (Detectable shared motion). Let Lobs\mathcal{L}_{\mathrm{obs}} be the set of observed links. With the map and the robots’ relative geometry fixed,

NF⊆{ξ:(JrGξ)ℓ=0  ∀ℓ∈Lobs}.\mathcal{N}_F\subseteq\{\xi:(J_rG\xi)_\ell=0\ \ \forall\ell\in\mathcal{L}_{\mathrm{obs}}\}.

The equality holds under the link sensitivity condition.

Proof. If ξ∈NF\xi\in\mathcal{N}_F, ∇F(v)TVξ(v)=0\nabla F(v)^{\mathsf T}V_\xi(v)=0 everywhere. By Eq. (2), (JrGξ)ℓ=0(J_rG\xi)_\ell=0 for all links, proving the inclusion. Under the link sensitivity condition, every ξ∉NF\xi\notin\mathcal{N}_F gives (JrGξ)ℓ≠0(J_rG\xi)_\ell\ne0 for at least one observed link, establishing equality. ∎

With the map fixed and link weights satisfying wℓ(gv,gX)=wℓ(v,X)w_\ell(gv,gX)=w_\ell(v,X), we use Eq. (2) to construct several simple examples of unresolved motion. A constant field has zero spatial gradient, so the radio model cannot detect any shared translation or rotation. If F(v)=f(vx)F(v)=f(v_x), vertical translation does not change sampled link attenuation. A radial field cannot distinguish rotation about its center. Uncertain field values reduce the information carried by these derivatives. Repeated map patterns can produce the same radio measurements at different team locations. However, the full rank of the shared-motion Jacobian ensures local distinguishability. This means that any small shared translation or rotation of the team changes at least one predicted radio measurement.

RPI Interface

We design RPI using the insight that a radio map aligned to world coordinates can provide a reference for the team’s position. We treat the map’s position and orientation in world coordinates as known. This method accounts for Gaussian uncertainty in its attenuation values as we describe in §V-B. We introduce RPI to pass information from the radio module to the pose estimator, as Figure 3 illustrates.

Figure 3. RPI converts power measurements and map information into (Λ,η)(\Lambda,\eta) for pose estimation. Updated poses feed back to the radio module for relinearization. Revised messages replace previous contributions.

Grid model

We first discretize the attenuation field and compute received power over it. To support nonlinear settings, where the predicted radio power depends nonlinearly on robot positions, we represent the attenuation field by grid values F\mathbf{F}. Within each cell, bilinear interpolation combines its four corner values with weights βm(v)\beta_m(v) that sum to one, giving Fh(v)=∑mβm(v)FmF_h(v)=\sum_m\beta_m(v)F_m, where hh denotes the grid spacing and FmF_m the attenuation value at grid vertex mm. We then sample this field along each radio link to obtain Ah(X)A_h(X), the matrix that converts map grid values into attenuation for each link ℓ=(i,j,t)\ell=(i,j,t). Attenuation and distance loss use d~ij,t=∥pj,t−pi,t∥2+δd2\tilde d_{ij,t}=\sqrt{\lVert p_{j,t}-p_{i,t}\rVert^2+\delta_d^2}, where δd≈10−6 m\delta_d\approx10^{-6}\,\mathrm{m}.

[Ah(X)]ℓm=∑q=1Qαℓq(X)βm(vℓq(X)),[A_h(X)]_{\ell m}=\sum_{q=1}^{Q}\alpha_{\ell q}(X)\beta_m(v_{\ell q}(X)),
[μfree(X)]ℓ=Pref−10γlog⁡10(d~ij,td0),[\mu_{\mathrm{free}}(X)]_\ell=P_{\mathrm{ref}}-10\gamma\log_{10}\left(\frac{\tilde d_{ij,t}}{d_0}\right),
μh(X,F)=μfree(X)−Ah(X)F.\mu_h(X,\mathbf{F})=\mu_{\mathrm{free}}(X)-A_h(X)\mathbf{F}.

The vector μfree(X)\mu_{\mathrm{free}}(X) contains the predicted received powers before mapped attenuation, with entry [μfree(X)]ℓ[\mu_{\mathrm{free}}(X)]_\ell for each link ℓ∈Lobs\ell\in\mathcal{L}_{\mathrm{obs}}. αℓq\alpha_{\ell q} is the integration weight of sample qq along link ℓ\ell. We use Q=64Q=64 samples including both endpoints,

vℓq=(1−sq)pi,t+sqpj,t,sq=q−1Q−1,v_{\ell q}=(1-s_q)p_{i,t}+s_qp_{j,t},\qquad s_q=\frac{q-1}{Q-1},

with weights

αℓq=d~ij,tbq∑sbs,bq=11+e−κ[1−(2sq−1)2].\alpha_{\ell q}=\tilde d_{ij,t}\frac{b_q}{\sum_{s}b_s},\qquad b_q=\frac{1}{1+e^{-\kappa[1-(2s_q-1)^2]}}.

κ\kappa is the spatial weight rise parameter that dictates how fast the weight gradient changes between the mid and endpoints. We set κ=8\kappa=8 to give nearly uniform weights across the link interior while reducing the endpoint weights. We differentiate the discretized power prediction with respect to the poses at the current estimates, holding the grid values fixed, to obtain the radio pose Jacobian JXJ_X. Then, we differentiate the prediction with respect to the grid values holding the poses fixed, which gives JF=−Ah(X)J_F=-A_h(X).

Power to Pose Information

We show how the radio module converts received power into an informative pose update. Let F\mathbf{F} denote the map values and F^\hat{\mathbf{F}} their current estimate. At the current pose and map estimates (X^,F^)(\hat X,\hat{\mathbf{F}}), define the power residual r=y−μh(X^,F^)r=y-\mu_h(\hat X,\hat{\mathbf{F}}). Assume a map prior F∼N(Fˉ,PF)\mathbf{F}\sim\mathcal{N}(\bar{\mathbf{F}},P_F), independent of the pose prior, where Fˉ\bar{\mathbf{F}} is the prior map mean and PF≻0P_F\succ0 its covariance. Let zz denote a local pose correction and f=F−F^f=\mathbf{F}-\hat{\mathbf{F}} a map correction. Linearization gives r≈JXz+JFf+ϵr\approx J_Xz+J_Ff+\epsilon. Then f∼N(mf,PF)f\sim\mathcal{N}(m_f,P_F), where mf=Fˉ−F^m_f=\bar{\mathbf{F}}-\hat{\mathbf{F}}. Let HtaskH_{\mathrm{task}} denote the pose information matrix and btaskb_{\mathrm{task}} the information vector driving the latest pose correction, both formed from the linearized odometry, relative factors, and pose priors. task refers to the pose-estimation objective before adding radio information. Joint Gaussian estimation solves, with W=Rr−1W=R_r^{-1} where WW is the precision matrix that describes how strongly radio prediction errors are penalized in the estimator,

min⁡z,f12zTHtaskz−btaskTz+12∥r−JXz−JFf∥W2+12∥f−mf∥PF−12.(3)\min_{z,f}\quad\frac{1}{2}z^\mathsf{T}H_{\mathrm{task}}z-b_{\mathrm{task}}^\mathsf{T}z+\frac{1}{2}\lVert r-J_Xz-J_Ff\rVert_W^2+\frac{1}{2}\lVert f-m_f\rVert_{P_F^{-1}}^2. \tag*{(3)}

The radio residual is weighted by WW, and the map-prior deviation by the precision matrix PF−1P_F^{-1}, the inverse of the map-prior covariance. To pass the radio information to a pose-only estimator, define

A=JXTWJX,a=JXTWr,A=J_X^\mathsf{T}WJ_X,\qquad a=J_X^\mathsf{T}Wr,
B=JFTWJF+PF−1,b=JFTWr+PF−1mf,B=J_F^\mathsf{T}WJ_F+P_F^{-1},\qquad b=J_F^\mathsf{T}Wr+P_F^{-1}m_f,
C=JXTWJF.C=J_X^\mathsf{T}WJ_F.

Eliminating the map correction ff through the Schur complement of the map information BB gives the RPI pair

Λ=A−CB−1CT,η=a−CB−1b.(4)\Lambda=A-CB^{-1}C^\mathsf{T},\qquad\eta=a-CB^{-1}b. \tag*{(4)}

With a known map, we set F^\hat{\mathbf{F}} to its known value, so f=0f=0 and (Λ,η)=(A,a)(\Lambda,\eta)=(A,a). Adding task information gives the Gaussian pose covariance and update

Pz=(Htask+Λ)−1,z^=Pz(btask+η).(5)P_z=(H_{\mathrm{task}}+\Lambda)^{-1},\qquad\hat z=P_z(b_{\mathrm{task}}+\eta). \tag*{(5)}

The summed information must be positive definite. For nonlinear iterations, the estimator solves for z^\hat z, updates the poses, and relinearizes.

Proposition 3 (Gaussian pose agreement). With matching factors, priors, weights, coordinates, and linearizations,

Eq. (4)–(5) reproduce the pose mean and covariance obtained by solving Eq. (3) jointly.

Proof. The joint objective is quadratic in (z,f)(z, f). Completing the square in ff and integrating it leaves 12zT(Htask+Λ)z−(btask+η)Tz\frac{1}{2}z^{\mathsf{T}}(H_{\mathrm{task}}+\Lambda)z-(b_{\mathrm{task}}+\eta)^{\mathsf{T}}z plus a constant. The minimizer and covariance are given by Eq. (5).

Shared History

Uncertainty in the shared map can affect pose estimates for multiple robots at different times. RPI therefore combines the radio measurements across robots and time steps before eliminating the map variables. This maintains the cross-robot and cross-time correlations the shared map induces. With respect to Proposition 3, the resulting message gives the exact pose marginal for the Gaussian system.

The pose estimator reconstructs the RPI interface after each relinearization. For NN robots and LL time steps, a pose information matrix requires O(N2L2)O(N^{2}L^{2}) storage. The RPI message contains (Λ,η)(\Lambda,\eta), the order of the pose variables, and the linearization point.

Evaluation

We evaluate how closely RPI matches joint Gaussian estimation (§VI-B), its ability to correct nonlinear drift (§VI-C), its sensitivity to map error (§VI-D), and the directions constrained by the map (§VI-E).

Methodology

All evaluations generate noisy measurements from known robot trajectories and an attenuation map, then estimate positions with known headings. Methods within each trial share observations and initialization.

For position error ei,t=p^i,t−pi,te_{i,t}=\hat{p}_{i,t}-p_{i,t}, we report common error eˉt=N−1∑iei,t\bar{e}_{t}=N^{-1}\sum_i e_{i,t} and relative error ei,t−eˉte_{i,t}-\bar{e}_{t}, measuring centroid displacement and errors in the team’s relative arrangement, respectively. We evaluate nonlinear RMSE over the final third of each trajectory to emphasize accumulated drift later in the run.

For stacked terminal-position covariance PP, common uncertainty is tr⁡Pcentroid\sqrt{\operatorname{tr}P_{\mathrm{centroid}}}, where Pcentroid=CpPCpTP_{\mathrm{centroid}}=C_pP C_p^{\mathsf{T}}, Cp=N−1(1NT⊗I2)C_p=N^{-1}(\mathbf{1}_N^{\mathsf{T}}\otimes I_2). Centroid NEES per coordinate is eˉtwTPcentroid−1eˉtw/2\bar{e}_{\mathrm{tw}}^{\mathsf{T}}P_{\mathrm{centroid}}^{-1}\bar{e}_{\mathrm{tw}}/2, with expected value one under a calibrated Gaussian model. Larger values indicate overconfidence. We also evaluate coverage of 95% coordinate intervals in the Gaussian test.

Linear Gaussian Interface Test

We test whether RPI preserves the joint Gaussian pose solution when shared-map correlations affect accuracy. Across 1,000 trials, we record three robots at 12 time steps, giving 72 position coordinates. The measurement Jacobians remain fixed. For this test, we use a simplified map with nine uncertain radio-bias coefficients governed by a spatially correlated Gaussian prior. We set radio, relative-position, and map-coefficient standard deviations to be 0.12 dB, 0.06 m, and 0.25 dB/m, respectively. We vary radio standard deviation from 0.03 to 1.92 dB in the noise-sensitivity experiment.

We consider three configurations: (i) Joint estimates robot positions and one shared map together. (ii) RPI eliminates the map variables and passes their contribution to the pose solver through (Λ,η)(\Lambda,\eta). (iii) Per-step map assigns an independent map to each time step, preserving within-step correlations but discarding correlations across time.

We expect RPI to outperform Per-step map because it preserves shared-map correlations across time. Similarly, if RPI is a useful abstraction, it would match Joint’s performance. Figure 4 confirms these expectations. RPI and Joint overlap across radio-noise standard deviations from 0.03 to 1.92 dB, with mean terminal centroid error increasing from 0.686 to 0.797 m. As the noise level increases, all three configurations converge to the same pose priors and relative-position measurements.

Figure 4. Gaussian shared-map test over 1,000 paired trials. (a) Mean terminal centroid error versus radio-noise standard deviation. (b) Trajectory and terminal-centroid NEES per coordinate, with nominal value one. (c) Covariance difference relative to Joint. Error bars are 95% intervals from trial means.

Per-step map also underestimates uncertainty. Terminal centroid NEES is 1.890, compared with 1.012 for RPI and Joint, and nominal 95% coordinate coverage falls from approximately 94.7% to 84.88%.

Nonlinear Drift Correction

We test whether RPI reduces common drift while preserving relative accuracy. We simulate four robots moving for 80 steps of 0.5 s and observe all six inter-robot links. Known headings and world-coordinate relative-position measurements leave two common translation directions unconstrained by the relative measurements. We update radio predictions and Jacobians as the estimates change. Methods share relative-factor weights within each map condition, and these weights remain unchanged during optimization.

We compare four methods using the same supplied map, observations, initialization, and fixed relative-factor weights: (i) Scalar uses radio quality only to reweight relative-position factors. (ii) RPI (fixed map) treats the supplied map as exact and adds pose information (Λ,η)=(A,a)(\Lambda,\eta)=(A,a). (iii) Joint (fixed map) directly adds the nonlinear radio term 12∥y−μH(X,F)∥R−12\frac{1}{2}\lVert y-\mu_H(X,F)\rVert^2_{R^{-1}} and optimizes the poses with FF fixed. (iv) RPI (uncertain map) uses the shared attenuation-value model in Section V.

The uncertain-map method estimates all 40×4040\times40 grid values with a Gaussian prior centered on the supplied map. We use a coefficient standard deviation of 0.25 dB/m and Gaussian spatial correlations with a 3 m length scale. The variance combines an 80% spatially correlated component and a 20% component independent across grid vertices. This covariance remains unchanged across map conditions. We use up to 25 Levenberg–Marquardt iterations, obtaining map increments after each pose update before relinearizing. Terminal uncertainty uses the terminal-position block of the undamped local marginal pose covariance PzP_z in Eq. (5).

Across 32 paired trials, initial-position, odometry, relative-coordinate, and radio noise standard deviations are 0.5 m, 0.08 m, 0.1 m, and 3 dB. To standardize the test, all methods share fixed radio quality weights on relative factors.

We expect that fixed-map RPI will again match Joint’s common error while preserving its relative accuracy, and will reduce common-position RMSE relative to Scalar. Figure 5 confirms these results: fixed-map RPI and Joint reduce common-position RMSE from Scalar’s 0.542 m to 0.140 m, a 74% reduction, with paired-change interval [−0.494,−0.311] m. Relative RMSE remains near 0.060 m. For fixed-map RPI and Joint, terminal common uncertainty decreases from Scalar’s 0.615 m to 0.189 m. Uncertain-map RPI gives common-position RMSE of 0.158 m and terminal common uncertainty of 0.232 m.

Figure 5. Results from 32 paired nonlinear trials with correct and perturbed maps: (a) common-position RMSE, (b) relative-position RMSE, and (c) terminal common uncertainty. RMSE uses the final third of each trajectory. Error bars show 95% intervals computed from trial means.

Map Error Ablation

We test sensitivity to map error by keeping the true map unchanged while supplying a map shifted by 1 m, attenuation values multiplied by 1.2, or both. Relative-factor weights are recomputed for each supplied map and shared across methods. Fixed-map RPI and Joint treat the supplied map as exact, while uncertain-map RPI uses the same attenuation-value model and prior covariance as in §VI-C.

Figure 5 shows that fixed-map RPI and Joint overlap, with final-third common-position RMSE of 0.981, 0.659, and 1.183 m under registration, attenuation, and combined errors, respectively. Uncertain-map RPI reduces these errors to 0.957, 0.172, and 0.967 m, while maintaining relative RMSE near 0.060 m, and outperforms Scalar’s 0.542 m under attenuation error, but not under registration or combined errors. This occurs because uncertain-map RPI models attenuation-value uncertainty but does not explicitly model registration error, which can bias the position estimates.

Fixed-map RPI and Joint report terminal centroid uncertainty between 0.171 and 0.189 m despite their increased position errors. Uncertain-map RPI reports between 0.217 and 0.233 m. Thus, shared attenuation uncertainty improves robustness to attenuation-scale error but does not resolve registration bias in these experiments.

Directional Map Ablation

Finally, we test which translation directions the map can constrain using constant, xx-varying, and two-dimensional maps. At the supplied map’s means, we verify that the observed links provide information about zero, one, and two independent directions of shared translation, respectively. Trajectories, non-radio measurements, initialization, and radio-noise realizations remain fixed. Each map generates its own radio observations and is also used for inference, avoiding map mismatch. We compare Scalar, fixed-map RPI, and uncertain-map RPI using the nonlinear procedure in §VI-C.

Figure 6 shows that Scalar has common x/yx/y RMSE of 0.324/0.403 m in all conditions. Fixed-map RPI gives the same errors with the constant map, 0.170/0.403 m with the xx-varying map, and 0.099/0.091 m with the two-dimensional map. Uncertain-map RPI gives 0.322/0.407, 0.219/0.405, and 0.118/0.094 m, respectively, preserving the directional pattern with slightly higher mean errors in the useful directions. Both RPI variants therefore primarily reduce error in directions informed by the map.

Figure 6. Directional map ablation over 32 paired trials. Final-third common x/yx/y RMSE for maps with translation-information ranks zero, one, and two. Error bars show 95% intervals across trials.

Conclusion

In this work, we showed how a registered attenuation map provides information about shared robot motion that fixed weights on relative measurements cannot directly supply. We introduced RPI, a mathematical interface that converts radio measurements and map information into pose information while preserving shared-map correlations. Our analysis characterizes which shared motions the map and observed links can detect.

Our simulations confirm that RPI matches the pose estimates and covariances from the benchmarked joint optimization method. Our nonlinear experiments show reduced common-position error while maintaining relative-position accuracy, with directional map ablations confirming that correction occurs in map-constrained directions. Map mismatch can increase position error without a corresponding increase in reported uncertainty. Future work will evaluate RPI on real measured radio data and study pose-update convergence. We will release the code upon acceptance.

References

  1. [1]S. Roumeliotis and G. Bekey, “Distributed multirobot localization,” IEEE Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781–795, 2002.DOI
  2. [2]G. P. Huang, N. Trawny, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based consistent EKF estimators for multi-robot cooperative localization,” Autonomous Robots, vol. 30, pp. 99–122, Jan. 2011.DOI
  3. [3]B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustment — a modern synthesis,” in Vision Algorithms: Theory and Practice (B. Triggs, A. Zisserman, and R. Szeliski, eds.), (Berlin, Heidelberg), pp. 298–372, Springer Berlin Heidelberg, 2000.DOI
  4. [4]M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” Intl. J. of Robotics Research, IJRR, vol. 31, pp. 216–235, Feb. 2012.DOI
  5. [5]J. Wilson and N. Patwari, “Radio tomographic imaging with wireless networks,” IEEE Transactions on Mobile Computing, vol. 9, p. 621–632, May 2010.DOI
  6. [6]E. Leitinger, F. Meyer, F. Hlawatsch, K. Witrisal, F. Tufvesson, and M. Z. Win, “A belief propagation algorithm for multipath-based slam,” IEEE Transactions on Wireless Communications, vol. 18, p. 5613–5629, Dec. 2019.DOI
  7. [7]X. Li, E. Leitinger, F. Tufvesson, and F. Meyer, “Probabilistic occupancy grid for radio-based slam,” 2026.arxiv.org/abs/2603.03559
  8. [8]J. D. Carroll and A. J. Canciani, “Terrain-referenced navigation using a steerable laser measurement sensor,” NAVIGATION: Journal of the Institute of Navigation, vol. 68, no. 1, pp. 115–134, 2021.DOI
  9. [9]J. H. Ramos, J. Shin, K. Volle, P. Buzaud, K. Brink, and P. Ganesh, “Information-aware guidance for magnetic anomaly based navigation,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 6347–6354, 2022.arxiv.org/abs/2208.00988
  10. [10]A. Cunningham, V. Indelman, and F. Dellaert, “Ddf-sam 2.0: Consistent distributed smoothing and mapping,” in 2013 IEEE International Conference on Robotics and Automation, pp. 5220–5227, 2013.
  11. [11]O. Dagan and N. R. Ahmed, “Exact and approximate heterogeneous bayesian decentralized data fusion,” Trans. Rob., vol. 39, p. 1136–1150, Apr. 2023.DOI
  12. [12]O. Dagan and N. R. Ahmed, “Conservative filtering for heterogeneous decentralized data fusion in dynamic robotic systems,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 5840–5847, 2022.DOI

Paper details

vlnc:W_77sezxpr
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY-SA 4.0
Usage

views · 14

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Computer Science

Not recorded for this paper

View statement
Looking for related papers…
Contents