DEPTRUM's ToF-Wheel Odometry Research Published in IEEE RA-L

DEPTRUM’s research ToF-Wheel Odometry: Navigating Degeneration of Localization With Limited Sensing Capabilities has been published in IEEE Robotics and Automation Letters (RA-L), a leading journal in robotics.

To tackle the localization challenges that ground robots equipped only with a ToF depth camera and wheel encoders face under motion anomalies and insufficient geometric constraints, the study proposes a tightly coupled filtering framework, ToF-Wheel Odometry (TWO). Through two key designs — degeneration-aware directional covariance adjustment and intra-frame feedback — the method achieves robust localization under wheel slip, terrain-induced attitude changes, and geometric degeneracy without relying on an IMU for motion priors.

Research Background

Wheel odometry offers low latency, low computational cost, and good short-term consistency, making it the most widely used motion prior for ground robots. However, it rests on assumptions such as planar motion and locally consistent attitude. On ramps, across changing floor materials, or on uneven ground, wheel slip and tilt introduce short-lived but significant deviations between the predicted trajectory and the true motion.

Consumer robots typically supplement geometric constraints with a ToF depth camera, which is compact, low-power, and cost-effective. Yet ToF cameras have limited range and a forward-only field of view, so their geometric observability is limited. In a long corridor, for example, the side walls constrain lateral position but barely constrain movement along the corridor.

Left: wheel slip on a wet floor, cable thresholds, and a ramp. Right: a real corridor scene and the top-down view of the ToF point cloud

Localization degradation in such systems is therefore rarely a single-sensor failure — it is typically the compounding of both limitations. The situation differs from lidar-inertial odometry (LIO), which benefits from wide-field, long-range lidar observations plus an IMU providing a six-DoF motion prior.

Existing filtering methods mostly adopt fixed or adaptive process-noise covariance, and adaptive variants are usually scalar or diagonal scalings. This does not suit ground robots: slip and tilt are sudden, pulse-like disturbances, not stationary noise. Inflating uncertainty globally weakens confidence in directions that were actually reliable, while keeping the covariance unchanged breeds overconfidence at critical moments. Robustifying the observation side does not solve the problem either, because the uncertainty stems from the motion model itself breaking down.

TWO decomposes process-covariance adaptation into four questions: how much to inflate, in which direction, whether the geometry supports it, and when it takes effect.

Method

TWO system overview. Wheel propagation provides the prior, the ToF update provides the geometric correction, and directional covariance adjustment together with intra-frame feedback repairs the process noise within the same frame

TWO is built on an EKF with four components: wheel propagation, a ToF update with observability analysis, directional covariance adjustment, and intra-frame feedback. The ToF update reuses the probabilistic point-to-plane constraints of DEPTRUM’s IROS 2024 work, C³P-VoxelMap.

Subspace Decomposition of the Correction

In every frame, wheel odometry first produces a predicted pose (the prior); the ToF point cloud is then registered against the local map to produce a corrected pose (the posterior). The difference is how far the ToF observation “pulls the robot back”, and it is the direct evidence TWO uses to judge whether the wheels went wrong. This correction, however, mixes two fundamentally different kinds of error. A differential-drive chassis can only move forward and backward or turn in place, and wheel encoders measure only these two degrees of freedom; lateral shift, vertical heave, roll, and pitch cannot be expressed by the wheel model at all. TWO therefore first projects onto the basis determined by the chassis kinematics and splits it into two parts:

where is the magnitude of the correction in the forward and yaw directions.

Wheel-explainable component : the correction along the forward and yaw directions, indicating that the wheels “measured wrong”; its typical cause is slip.

Orthogonal residual : the correction along the lateral, vertical, roll, and pitch directions, corresponding to motion beyond the wheel model, such as side slip or the body lifting and tilting while traversing obstacles. It may also be mere observation noise under degenerate geometry.

The former directly measures how wrong the wheels were; whether the latter can be trusted depends on whether ToF has enough geometric constraints in those directions.

Degeneration-Aware Observability Scoring

Whether ToF can reliably correct a given direction depends on the geometric structure in the current field of view. In a long corridor, the two side walls lock the lateral position but provide almost no constraint along the corridor; in a room full of furniture, all directions are well constrained. TWO builds a measurement information matrix from the Jacobian of the point-to-plane residuals and the observation covariance , and converts it into an information-gain operator :

Each eigenvalue of lies between 0 and 1 and indicates how much information the observation provides along the corresponding direction: close to 1 when constraints are sufficient, and close to 0 when they are nearly absent.

For convenient use inside the filter, TWO compresses into a scalar: the multiple of the identity closest to in the Frobenius norm, . Its closed-form solution is the average of the per-direction gains:

where is the pose dimension and are the eigenvalues of . can be read as the “geometric credibility” of the current frame: near 1 when all six degrees of freedom are locked; dropping as constraints are lost along the corridor — the more weakly constrained directions, the larger the drop. Restricting the same computation to the four directions of the orthogonal residual yields the residual observability score , which answers one question: is there geometric evidence supporting the lateral, vertical, and attitude corrections?

Directional Covariance Adjustment

With the correction decomposition and the two scores, TWO decides separately “how much” and “where” to inflate the process noise.

How much. If the wheel model’s uncertainty is set reasonably, the magnitude of the wheel-explainable component should match the filter’s expectation. TWO assumes follows a Gaussian whose covariance is inflated by a factor , and obtains the inflation factor in closed form by minimizing the negative log-likelihood:

where is the squared Mahalanobis distance of , measuring how large this correction is relative to the filter’s expectation, and is the effective dimension. When the correction exceeds expectation (), the process noise is inflated; within expectation it stays at the baseline and is never reduced. Under geometric degeneracy the correction itself may be unreliable, so further smooths :

When geometric credibility is high, follows the current frame’s estimate quickly; when it is low, the previous frame’s value is mostly kept, avoiding being misled by degenerate observations.

Where. The orthogonal residual is first scaled by and then combined with the explainable component to form the direction of uncertainty inflation:

where is the normalized direction of , is the number of wheel-speed readings within the frame, and maps the same direction onto the -th propagation step via the SE(3) adjoint transform. In other words, the extra process noise is injected only along the single direction and is spread evenly across every wheel propagation step of the frame.

Whether the residual is kept is thus decided by geometric evidence:

When the geometry supports side slip or attitude change (), the residual is accepted and uncertainty is inflated toward the slip or tilt direction;

when the geometry degenerates (), the residual is suppressed as sensor noise and uncertainty is inflated only along the forward and yaw directions.

Moreover, only the wheel-explainable component can drive upward — the residual affects direction only. This prevents spurious corrections arising from degenerate geometry from inflating uncertainty on their own.

Intra-Frame Feedback

In a standard Kalman filter, the process noise is fixed before propagation, so anomalies detected in the current frame can only affect the next one. Slip is precisely sudden: in the very frame where it happens, the prior covariance is still small, the filter over-trusts the wheel prediction, and the ToF correction is suppressed. Exactly when the error is largest, the filter’s estimate of that error is smallest.

TWO completes the repair within the current frame:

1. Propagate with the current process noise and perform one ToF update;

2. Compute the new from the resulting correction, as described above;

3. Starting from the previous posterior covariance, re-propagate this frame’s covariance with the new process noise and perform another ToF update;

4. Stop when prior and posterior converge, or when the iteration cap is reached.

During feedback, the propagated mean and the local map stay unchanged — only the covariance is repaired; the map is updated only after the posterior is accepted. The iteration count is capped, so the extra latency is bounded within a few EKF updates. Covariance repair completes within the current frame instead of waiting for a multi-frame adaptation window to converge.

Experimental Results

The experimental platform is a differential-drive chassis with a Raspberry Pi 4B as the onboard computer; the test camera is DEPTRUM’s in-house Nebula 280. Built on sToF technology and combining 3D structured light with mToF, the camera delivers high-precision depth sensing at both near and medium-to-far range, supports a 98° (H) × 60° (V) field of view, and measures up to 12 m indoors. Ground truth for the collected sequences comes from an independently installed Livox Mid-360 lidar-inertial system, which TWO does not use. All parameters remain identical across all sequences.

Comparison with Mainstream Methods

The compared methods are VoxelMap, AKF-LIO, Faster-LIO, and LIWO. On the self-collected sequences, all methods consume the same ToF point cloud; the only difference is the motion prior: the baselines use the module’s built-in IMU, while TWO uses only wheel encoders.

On the hall, office, and long-corridor sequences, TWO’s ATE is 0.072 m, 0.177 m, and 0.249 m respectively — the lowest of all methods. The average ATE across the five sequences is 0.190 m, versus 0.411 m for the runner-up.

Trajectory comparison on the three self-collected sequences; (d) is a close-up of the dashed box in (a)(b)

In terms of trajectories, TWO follows the polyline path through slipping turns in the hall, preserves the rectangular loop shape in the office, and avoids longitudinal overshoot in the corridor, while the baseline methods exhibit path distortion, rotational drift, or longitudinal overshoot.

On the public M3DGR dataset (wheels lifted off the ground, sharp turns), where no ToF camera is available, TWO switches to lidar point clouds as its geometric input. AKF-LIO and Faster-LIO, which use a gyroscope, achieve better short-term RPE, but the ATE gaps are small (0.190 vs 0.209 m, and 0.200 vs 0.242 m). On the wheels-lifted sequence, TWO outperforms LIWO, another wheel-fusion method.

Comparison of Covariance Adaptation Mechanisms

To isolate the effect of sensor configuration, the study compares fixed covariance and scalar adaptive covariance under identical ToF and wheel-encoder inputs:

Compared with the fixed-covariance baseline, TWO reduces ATE by about 82%. Notably, scalar adaptation performs even worse than the fixed covariance: every correction inflates uncertainty isotropically, injecting uncertainty into directions the wheel prior had already constrained well. Merely tuning the magnitude is not only insufficient — it can backfire.

The study also scales the baseline process noise from 0.1× to 10×. At every scale, TWO reduces ATE by 74.3%–83.2% over the static configuration, confirming that this gain cannot be obtained by simply inflating the process noise globally.

Ablation Study

Residual gating: forcibly suppressing the residual (η=0) raises the ATE of the side-slip and lift/attitude-disturbance scenarios to 0.164 m and 0.115 m (full system: 0.072 m and 0.042 m); forcibly accepting all residuals (η=1) degrades the forward-slip-with-weak-geometry scenario. The residual should be suppressed only when its subspace is poorly observable — not simply because it violates the wheel model.

Magnitude inflation: with β fixed at 0, the average ATE across four sequences rises from 0.046 m to 0.219 m — the most degraded variant.

Intra-frame feedback: removing feedback increases the ATE of forward-slip-with-weak-geometry and lift/attitude disturbance to 1.60× and 1.50× that of the full system.

Selective gating behavior in the ablation study

Embedded Real-Time Performance

Most frames satisfy the convergence condition right after the first update; extra iterations concentrate at moments of motion anomalies, averaging fewer than 0.5 per frame. A single feedback pass captures most of the accuracy gain.

Conclusion

Following its earlier work on 3D map representation and update efficiency in C³P-VoxelMap, the proposed TWO goes further into robot localization in complex environments: by assessing the reliability of ambient geometric constraints, it dynamically adjusts the uncertainty in motion estimation and improves localization robustness when motion anomalies and geometric degeneracy coincide. The method requires no IMU motion prior and has been verified on a Raspberry Pi platform, offering a new approach to robust localization under constrained resources.

From 3D mapping to robust localization, DEPTRUM keeps exploring innovation in robotic 3D vision. The localization technology behind TWO has already been deployed at scale. Going forward, DEPTRUM will continue pairing fundamental research with real-world deployment, providing lighter and more reliable perception support for autonomous mobility and spatial understanding of robots in complex environments.