Continuous-Time Ultra-Wideband-Inertial Fusion

We introduce SFUISE, a novel Spline Fusion-based Ultra-wideband-Inertial State Estimation scheme (Sec. II). Quaternion-based cubic cumulative B-splines serve as the backbone of state representation, with systematic and unified derivations of analytic kinematic interpolations and Jacobians (Sec. III). Based thereon, an efficient sliding-window spline fitting scheme is established to fuse UWB-inertial readings at raw timestamps with an added option of online calibration (Sec. IV). A dedicated study is first conducted to validate the viability of the quaternion spline fitting scheme. Afterward, we evaluate SFUISE for UWB-inertial tracking in various real-world scenarios using public data sets and experiments, including comparisons with state-of-the-art discrete-time fusion schemes (Sec. V). The proposed system delivers real-time and superior performance over the discrete-time counterparts. Considering the generality of the proposed scheme to extensive scenarios, we open-source our implementation together with own experimental data sets.

We deploy cubic (fourth-order) cumulative B-splines for continuous-time state representation. Being established upon a set of control points $\{({\underline{\mathscr{p}}}_{i},t_{i})\}_{i=1}^{n}$ associated with time $t_{i}$ of uniform interval, it allows for fusing sensor readings up to the second order of motion (e.g., from accelerometer) [26]. State at an arbitrary timestamp $t\in[\,t_{i},t_{i+1}\,)$ can be interpolated w.r.t. a local set of knots $\{{\underline{\mathscr{p}}}_{i+j-2}\}_{j=0}^{3}$ according to

being the normalized time and ${\underline{\delta}}_{j}={\underline{\mathscr{p}}}_{i+j-2}-{\underline{\mathscr{p}}}_{i+j-3}$ the distance between neighboring knots. Note that we use calligraphic fonts to denote spline knots, highlighting that they do not lie on the trajectory itself. The cumulative basis functions $\{\lambda_{j}(u)\}_{j=1}^{3}$ follow $[\,\lambda_{1}(u),\lambda_{2}(u),\lambda_{3}(u)\,]^{\top}=\Phi\,{\underline{u}}$ , with

endowing cubic B-splines with $\mathcal{C}^{2}$-continuity. Expression (2) can be applied to model positions and IMU biases in (1). Based on Riemannian geometry, the concept of cumulative B-splines can be naturally extended to the manifold of unit quaternions [34]. It follows

with $\otimes$ denoting the Hamilton product and $\lambda_{j}(u)$ the basis functions in (2). Distance between adjacent knots is quantified via logarithm map ${\underline{\delta}}_{j}=\mathrm{Log}_{\mathds{1}}\big{(}{{\underline{\mathscr{q}}}_{i+j-3}^{-1}\otimes{\underline{\mathscr{q}}}_{i+j-2}}\big{)}$ at identity $\mathds{1}=[1,0,0,0]^{\top}$ and contributes to the on-manifold interpolation via exponential map $\mathrm{Exp}_{\mathds{1}}(\cdot)$ [35]. Unless otherwise specified, the term ‘B-spline’ in the following content refers to the cumulative formulation of uniform intervals.

On modeling motion-related states, knots are optimally estimated by fitting the spline to measurements in the least squares sense. Constructing residuals in the objective refers to kinematic interpolations on cubic B-splines w.r.t. related sensory modalities. This can be computationally expensive due to large data volume and complexity in deriving motion derivatives. Meanwhile, solving nonlinear least squares requires Jacobians of on-manifold kinematic interpolations w.r.t. knots. In the remainder of this section, we provide these theoretical building blocks for quaternion-based B-splines towards high-performance continuous-time sensor fusion.

We now present time derivatives of cubic B-splines for interpolating linear and angular velocities, and acceleration.

On solving the nonlinear least squares for spline fitting, the gradient of the objective can be computed via the chain rule, where the major complexity goes to the kinematic terms. For that, we consider a spline segment ranging over $t\in[\,t_{i},t_{i+1})$ as introduced in Sec. III-A. Jacobians of interpolated kinematic states ${\underline{x}}$ w.r.t. knots $\{{\underline{\mathscr{x}}}_{i+j-2}\}_{j=0}^{3}$ are in principle expressed as

with ${\mathbb{I}}$ being the indicator function. Depending on the knot indices, Jacobian components in (10) are calculated via

that are concretized for the kinematic types as follows.

Shown in Fig. 3, we exploit cubic B-splines to parameterize state (1) continuously over time with knots concatenated as ${\underline{\mathscr{x}}}=[\,{\underline{\mathscr{q}}}^{\top},{\underline{\mathscr{p}}}^{\top},{\underline{\mathscr{b}}}^{\top}]^{\top}\in\mathbb{R}^{13}$. To keep the computational intensity tractable for online performance, we bound the spline fitting problem over a window of recent $\tau_{\text{w}}$ knots ${\mathcal{{X}}}_{\text{w}}=[\,{\underline{\mathscr{x}}}_{1},...,{\underline{\mathscr{x}}}_{\tau_{\text{w}}}\,]\in\mathbb{R}^{13\times\tau_{\text{w}}}$, namely,

with the objective function formulated as ${\mathscr{F}}({\mathcal{{X}}}_{\text{w}})=$

${\mathscr{E}}_{{\texttt{u}}}$ and ${\mathscr{E}}_{{\texttt{i}}}$ denote residual terms built upon UWB and IMU measurements, $\{{\hat{z}}_{{{\texttt{u}}},i}\}_{i=1}^{m}$ and $\{\hat{\underline{z}}_{{{\texttt{i}}},k}\}_{k=1}^{n}$, respectively, each observed at raw timestamps. ${\mathbf{C}}_{\circ}$ and ${\mathscr{v}}_{\circ}$ indicate the sensor noise covariances and tunable weights, respectively, for each error term (subscript ’$\circ$’ stands for ’u’ or ’i’ denoting UWB or IMU, respectively). As the window slides, the so-called active knots within the current window are updated over time until to be dropped out. Meanwhile, the three knots that are most recently removed from the window are turned into an idle state. They are no longer being optimized, however, still participate in computing residuals in the current window via kinematic interpolations, which enables motion continuation across sliding windows over time [29]. Note that timestamps of residuals and underlying knots are not to be aligned. This allows for flexible multi-sensor fusion and efficient motion representation compared with discrete-time paradigm.

We now specify the residual terms in (16). Throughout the following derivations, we use B-splines to describe the $6$-DoF motion of the IMU body (I) w.r.t. the world frame (W). Anchor positions are given w.r.t. UWB frame (U) with a transformation ${\mathbf{T}}_{{\texttt{W}}}^{{\texttt{U}}}\in\text{SE(}3\text{)}$ from the world frame.

In general, the transformation ${\mathbf{T}}_{{\texttt{W}}}^{{\texttt{U}}}$ (parameterized by a quaternion ${\underline{q}}_{{\texttt{W}}}^{{\texttt{U}}}\in\mathbb{S}^{3}$ and a translation vector ${\underline{t}}_{{\texttt{W}}}^{{\texttt{U}}}\in\mathbb{R}^{3}$) from world to UWB frames in (17) is unknown and typically dependent on the anchor coordinates and the tag pose at system initialization. Also, the gravity orientation $\bar{{\underline{g}}}^{{\texttt{W}}}$ (obtained via normalizing the gravity ${\underline{g}}^{{\texttt{W}}}=\|{\underline{g}}^{{\texttt{W}}}\|\,\bar{{\underline{g}}}^{{\texttt{W}}}$) in (18) is in general not available upon navigation. To approximate it, a common practice is to average the first several accelerometer readings under the assumption of a static motion at start. This can be easily violated by an undesirable starting condition (e.g., on the fly). To address these issues, we concatenate $({\underline{q}}_{{\texttt{W}}}^{{\texttt{U}}},{\underline{t}}_{{\texttt{W}}}^{{\texttt{U}}})$ and $\bar{{\underline{g}}}^{{\texttt{W}}}$ after the knots in the state vector (15) during window-growing phase and estimate them via spline fitting.

The proposed spline fusion-based UWB-inertial state estimation (SFUISE) scheme is developed in C++ using ROS [36]. The system composes two individual nodes corresponding to the two functional blocks in the system pipeline Fig. 2. Besides basic computational tools such as Eigen (https://eigen.tuxfamily.org), no further external dependency is required. We customize Levenberg-Marquardt (LM) algorithm to solve the nonlinear least squares in (15) iteratively. The closed-form gradients w.r.t. quaternion states are further established in the tangent space at estimate $\check{{\underline{\mathscr{q}}}}$ w.r.t. a local perturbation ${\underline{\phi}}\in\mathbb{R}^{3}$ by multiplying with $\frac{\partial{({\check{{\underline{\mathscr{q}}}}}\otimes\mathrm{Exp}_{\mathds{1}}({{\underline{\phi}}}))}}{\partial{{\underline{\phi}}}}\big{|}_{{\underline{\phi}}={\underline{0}}}={{\mathscr{Q}}}^{\llcorner}(\check{{\underline{\mathscr{q}}}})\frac{\partial{\mathrm{Exp}_{{\mathds{1}}}({\underline{\phi}})}}{\partial{{\underline{\phi}}}}\big{|}_{{\underline{\phi}}={\underline{0}}}$ . Solving the linearized system yields an increment ${\underline{\phi}}^{*}$, which updates the estimate through $\check{{\underline{\mathscr{q}}}}\leftarrow\check{{\underline{\mathscr{q}}}}\otimes\mathrm{Exp}_{{\mathds{1}}}({\underline{\phi}}^{*})$ [37, 2]. Furthermore, the gravitation orientation $\bar{{\underline{g}}}^{{\texttt{W}}}\in\mathbb{S}^{2}$ is handled in a similar fashion as introduced in [15].

The major theoretical complexity for the proposed spline fusion schemes lies in the part for orientation. Thus, we adopt the same synthesis as presented in [26, Sec. 6.1] for batchwise continuous-time $\text{SO(}3\text{)}$ estimation using both orientation and angular velocity measurements. We compare our quaternion-based scheme with the one using rotation matrices in [26] for fitting cubic cumulative B-splines. Both schemes are equipped with the same objective and stopping criteria. For each scheme, we exploit the Ceres Solver (http://ceres-solver.org) using LM algorithm with auto-differentiation for gradient computation, and the custom LM solvers using corresponding analytic gradients. In addition, we integrate our quaternion-based analytic gradient into Ceres for validation. We scale up the number of knots and measurements in the original sequence by a factor of $\{1,5,10\}$ and compute average runtime over $500$ runs with knot initializations around identities perturbed by a small random noise. All methods have converged with an averaged RMSE of ${{2.21}\times 10^{-4}}$ w.r.t. the ground truth in terms of the $\text{SO(}3\text{)}$ metric in [38, Eq. 19]. As shown in Tab. I, the proposed quaternion spline fitting scheme achieves superior runtime efficiency, with gradients obtained from both auto-differentiation and analytic expressions. By utilizing closed-form gradients, our quaternion-based scheme runs slightly faster within Ceres than the custom LM in [26], which employs splines defined on rotation matrices.

In the upcoming sections, the proposed system is evaluated in real-world scenarios based on the public data set UTIL [7] and own experiments, incorporating both ToA and TDoA ultra-wideband data. Two major discrete-time sensor fusion schemes from the state of the art are considered for comparison. An own composition of graph-based UWB-inertial fusion (GUIF) system is developed with reference to [39] and [1]. Here, discrete-time states are estimated via sliding window optimization with residuals of IMU preintegration, UWB ranging, and the prior factor from marginalization. Online calibration of ${\mathbf{T}}_{{\texttt{W}}}^{{\texttt{U}}}$ is enabled during window-growing stage. Further, we deploy the error-state Kalman filter (ESKF) provided by [7] with default calibration parameters for evaluation against the recursive estimation scheme. In order to achieve functional UWB ranging under signal interference, the three systems are equipped with a simple thresholding step to reject outliers in UWB measurements. Throughout the benchmark, SFUISE is configured with a sliding window of $100$ knots at $10$ Hz. All UWB readings are exploited for state estimation without downsampling. In order to devote the focus to investigating the core sensor fusion scheme, we avoid fine-tuning of system configuration parameters.

We exploit UTIL flight data set for evaluating the proposed UWB-inertial fusion scheme with TDoA ultra-wideband ranging. Overall $79$ sequences are collected onboard a quadcopter mounted with a UWB tag and an IMU ($1000$ Hz). The UWB sensor network is low-cost and operated at both centralized (tdoa2) and decentralized (tdoa3) modes under four different anchor constellations (const1-4). The TDoA measurements are collected with data rates ranging from $200$ to $500$ Hz including numerous challenging scenarios created by static and dynamic obstacles of various types of materials. Some sequences also exhibit an absence of IMU measurements. Given knot estimates from SFUISE, we obtain the global pose trajectory via interpolation at the frame rate of ground truth ($200$ Hz), w.r.t. which we compute the RMSE of the absolute position error (APE) to quantify the tracking accuracy.

As shown in Fig. 4, results given by the three systems are summarized w.r.t. UWB operation modes and anchor constellations. The proposed spline fusion scheme delivers superior performance over discrete-time approaches using recursive estimation or graph-based optimization. In particular, it exhibits a fairly good robustness against challenging conditions, e.g., in sequences with const4, where the tracking space is cluttered with static and dynamic obstacles of different materials including metal. In the face of time-varying NLOS and multi-path interference, discrete-time approaches are more susceptible to these effects, especially during online calibration, leading to large tracking errors or even complete failure. For demonstration, we plot results of a few representative runs of SFUISE in Fig. 1. Another qualitative comparison of spline fusion with discrete-time schemes is shown in Fig. 5 based on a representative sequence.

To further evaluate the proposed scheme on UWB-inertial fusion using ToA ranging, a miniature sensor suite has been instrumented as shown in Fig. 6-(A). It is composed of a UWB tag provided by Fraunhofer IOSB-AST and an IMU embedded on Sense HAT (B), both mounted to a Raspberry Pi (https://www.waveshare.com) for sensor coordination and data recording. An additional VIVE tracker (https://www.vive.com) is added to provide the ground truth. Overall three sequences, ISAS-Walk1, ISAS-Walk2 and ISAS-Walk3, are recorded during indoor walks with inertial and ultra-wideband (including five anchors) readings both at a frame rate of about $80$ Hz. We list the APE (RMSE) in meters given by the three systems in Fig. 6-(B). The proposed scheme SFUISE produces the best tracking accuracy consistently throughout the data set.

As verified in the mass evaluation based on UTIL and ISAS-Walk, SFUISE shows superior performance over discrete-time fusion schemes, particularly, in unfavorable scenarios of signal interference and complex noise patterns that are hard or infeasible to detect and model. Since any kinematic interpolation refers to four consecutive knots (for cubic B-splines), the induced pose trajectory inherently guarantees motion continuation up to the second order. In comparison with discrete-time fusions, this implicitly brings extra constraints to solving the nonlinear optimization problem in spline fitting, while still acknowledging the locality of observations. As a result, sensor fusion exhibits better stability and robustness under challenging conditions even together with online calibration. To further highlight the strength of spline fusion in motion estimation, we select const1-trial1-tdoa2 of UTIL, and discard the IMU readings for UWB-only navigation. We run SFUISE and GUIF online at estimation frame rates of $1$ Hz and $10$ Hz, respectively, with sliding windows both configured as $10$ seconds to guarantee same amount of TDoA measurements. No explicit kinematic constraint, such as motion smoothness, is involved into state estimation. A qualitative comparison is demonstrated in Fig. 7. Due to signal noise and interference, graph-based approach produces physically-infeasible motion estimates. The proposed spline-based scheme delivers a smooth trajectory with an efficient state representation using knots estimated at $1$ Hz ($1/10$ memory consumption of the discrete-time states delivered at $10$ Hz by GUIF).

For any point ${\underline{v}}\in\mathbb{R}^{3}$ in the tangent space at identity ${\mathds{1}}$ on the manifold of unit quaternions, it can be retracted to $\mathbb{S}^{3}$ via exponential map according to

with $\|\cdot\|$ denoting the $\mathscr{L}_{2}$ norm [35]. The Jacobian of exponential map w.r.t. ${\underline{v}}$ then follows

with the two items expressed as

Given any unit quaternion $\underline{q}=[\,q_{0},\underline{q}_{\text{v}}^{\top}]^{\top}\in\mathbb{S}^{3}$ with $q_{0}$ and $\underline{q}_{\text{v}}$ denoting the scalar and vector components, the rotation angle and axis can be retrieved by definition as $\theta=2\arctan(\|\underline{q}_{\text{v}}\|/q_{0})$ and ${\underline{u}}=\underline{q}_{\text{v}}/\|\underline{q}_{\text{v}}\|$, respectively. It can be mapped to the tangent space at ${\mathds{1}}$ via logarithm map

The Jacobian of logarithm map follows