TANGO-VIO

Overview

The TANGO-VIO safety filter at a glance

In vision-aided navigation and visual-inertial odometry, the quality of triangulated three-dimensional feature positions is a fundamental prerequisite for state estimation accuracy. Triangulation becomes ill-conditioned or even impossible when a camera undergoes pure rotation without translation, or when the observed bearing vectors provide insufficient parallax. Even though visual-inertial odometry has been extensively studied, the active maintenance of feature-observability during navigation has not been sufficiently addressed in the literature. To address this gap, this study presents TANGO-VIO, a triangulation-aware navigation framework that embeds a log-determinant metric of the feature-wise stacked-bearing matrix into a control barrier function. The observability guarantee is established in the feature-geometric sense by enforcing a lower bound on the aggregate triangulation-information metric through a nominal-direction-weighted minimum-deviation velocity correction. The proposed architecture is evaluated through software-in-the-loop simulations and real flight experiments: the results show improved triangulation conditioning under low-parallax motion, while the flight response closely reproduces the corresponding simulation behavior and confirms the practical realizability of the proposed safety filter.

visual navigation control barrier functions triangulation feature-geometry conditioning

TANGO-VIO wraps a standard VIO-guided architecture with a navigation-level safety filter. It leaves the nominal velocity command untouched whenever the bearing geometry is informative enough — and applies the minimum weighted correction that generates additional parallax the moment triangulation quality approaches its prescribed lower bound.

TANGO-VIO in action during a straight-climbing trajectory: on the left, live feature tracks and triangulated landmarks from the VIO front end; on the right, the nominal versus CBF-corrected body-frame velocity commands alongside the running counts of total tracked features and newly triangulated features per frame.

TANGO-VIO information-flow architecture diagram — Information flow of the TANGO-VIO architecture. The VIO front end tracks features and maintains the sliding-window poses and bearing vectors $\bm{b}_{i,j}$; the triangulation-geometry module builds the stacked-bearing matrices $\bm{M}_i$ and the average log-det barrier $\bar{\ell}(\bm{x})$. Combined with the nominal command from the B-spline mission planner, the barrier-derivative module forms the control-affine CBF condition, solved in closed form by the safety filter. The corrected command $\bm{v}_{B}^{*}$ drives the ArduPilot velocity controller, and the executed motion closes the loop through new images, IMU data, and pose feedback.

1

Online velocity filtering

Triangulation-aware navigation is formulated as an online velocity-filtering problem. Instead of treating feature-observability as a passive byproduct of the executed motion, the framework actively modifies the commanded motion when parallax becomes insufficient for well-conditioned triangulation.

2

Log-det barrier on triangulation quality

The average log-determinant of the feature-wise stacked-bearing matrices defines a safe set that lower-bounds the aggregate triangulation informativeness over the active camera-pose window — encoded directly as a Control Barrier Function constraint.

3

Hard & soft operation

A nominal-direction-weighted minimum-deviation safety filter admits both a hard-CBF mode that prioritizes triangulation quality, and a slack-enabled soft-CBF mode that trades triangulation maintenance against fidelity to the nominal command.

Method

From bearing geometry to a certified velocity command

The barrier

Each tracked feature $f_i$, observed from the $n$ poses of the sliding window, contributes a stacked-bearing matrix $\bm{M}_i = \sum_{j=1}^{n}\left(\eta_{i,j}^{2}\bm{I} - \bm{b}_{i,j}\bm{b}_{i,j}^{\top}\right)$ that summarizes its multi-view parallax geometry. Well-distributed bearing rays make $\bm{M}_i$ well-conditioned; nearly parallel rays drive it toward rank deficiency. The barrier lower-bounds the geometric mean of the feature-wise determinants:

Triangulation-quality barrier

$$ h(\bm{x}) \;=\; \underbrace{\frac{1}{s}\sum_{i=1}^{s}\log\det\bm{M}_{i}(\bm{x})}_{\bar{\ell}(\bm{x})\;=\;\text{avg log-det}} \;-\; \bar{\ell}_{\min} \;\ge\; 0 $$

The safe set $\mathcal{C}=\{\bm{x}: h(\bm{x})\ge 0\}$ collects all states whose aggregate bearing geometry supports reliable triangulation.

The control-affine CBF condition

Differentiating the barrier through the bearing-rate dynamics $\dot{\bm{b}}_{i,j} = -\tfrac{1}{\rho_{i,j}\eta_{i,j}^{2}}\,\bm{\pi}_{i,j}\bm{R}_{C_j}^{A}\bm{R}_{B}^{C}\bm{v}_{B,j}$ splits its derivative into a drift term from the recorded past-pose velocities and a control term in the current body-frame command $\bm{v}_B$:

CBF condition (hard mode)

$$ f(\bm{x}) + \bm{g}(\bm{x})^{\top}\bm{v}_{B} + \gamma\, h(\bm{x}) \;\ge\; 0 $$

Any command satisfying this inequality keeps the triangulation-quality safe set forward invariant — feature-observability is guaranteed, not hoped for.

The closed-form safety filter

The corrected command solves a weighted minimum-deviation QP with slack $\delta_T$ (soft mode) and weighting $\bm{W}_B$ that penalizes lateral redirection more than speed adjustment along the nominal direction. It admits a closed-form solution — no numerical QP solver in the loop:

Closed-form corrected velocity

$$ \bm{v}_{B}^{*} = \begin{cases} \bm{v}_{B,\mathrm{nom}}, & \mu \ge 0,\\[10pt] \bm{v}_{B,\mathrm{nom}} - \dfrac{\mu}{S(\bm{x}) + \frac{1}{p_T}}\,\bm{W}_{B}^{-1}\bm{g}(\bm{x}), & \mu < 0, \end{cases} $$

with residual $\mu = f(\bm{x}) + \bm{g}(\bm{x})^{\top}\bm{v}_{B,\mathrm{nom}} + \gamma h(\bm{x})$ and $S(\bm{x}) = \bm{g}(\bm{x})^{\top}\bm{W}_{B}^{-1}\bm{g}(\bm{x})$. When the nominal command already satisfies the barrier ($\mu\ge0$), it passes through untouched.

Hard CBF — triangulation first

The constraint is enforced without relaxation ($\delta_T = 0$). Maintaining the prescribed log-det threshold takes priority over terminal stopping or exact path tracking: the vehicle keeps generating parallax even if the mission says "hover".

Soft CBF — mission aware

A penalized slack $\delta_T \ge 0$ permits controlled relaxation when strict enforcement conflicts with the mission. The slack value explicitly quantifies the instantaneous conflict between triangulation maintenance and nominal tracking.

Bearing-vector geometry over the active camera-pose window: a static feature $f_i$ observed from multiple camera poses $C_j$; the body-frame velocity changes the bearing geometry and drives the bearing-rate dynamics.

Results · Software-in-the-Loop

What the filter does when parallax runs out

TANGO-VIO is evaluated in Gazebo/ArduPilot SITL with OpenVINS providing the sliding-window VIO structure. Two scenarios expose low-parallax motion, terminal stopping, and altitude-dependent triangulation degradation. Three modes are compared: nominal baseline, hard CBF, and soft CBF.

Gazebo/RViz SITL environment with live CBF safety-filter monitor — Software-in-the-loop environment used for repeatable evaluation of TANGO-VIO, integrating the Gazebo scene and sensor models with the ArduPilot SITL flight stack and ROS 2/MAVROS communication.

Gazebo viewport of the quadrotor during a climbing-trajectory run — Additional Gazebo viewport during a climbing-trajectory run, with the tracked visual features supplied to the OpenVINS front end.

At each update, the active camera-pose window and tracked feature bearings are used to compute the stacked-bearing matrices, the average log-det barrier, and the minimum-deviation body-frame velocity command. The same operating point is used throughout the SITL scenarios below:

Parameter	Value
Update rate	20 Hz
γ (linear class-K∞ gain)	3
w_∥ / w_⊥ (tangent / perpendicular weight)	1 / 10
ℓ̄_min (log-det threshold)	−2.5
Velocity-command limit	10 m/s
p_T (soft-CBF priority weight, soft cases only)	5

Two representative scenarios are considered: a square trajectory with low-velocity turns and a terminal stop, and a climbing trajectory whose required parallax grows with altitude. For each, the nominal baseline (TANGO-VIO disabled) is compared against the hard-CBF and soft-CBF modes.

Scenario I — Square trajectory

100 m side · 60 m altitude · 3 m/s · terminal stop at start point

The principal degradation source is the low-velocity turning portions at the corners and the terminal stop, where nominal motion provides little translational baseline. The hard CBF refuses to let the vehicle stop where triangulation would collapse; the soft CBF stays close to the nominal square while still injecting parallax-enhancing motion in the degraded intervals.

Hard CBF response

Square trajectory, hard CBF: velocity command modification — Velocity command modification.

Square trajectory, hard CBF: barrier-constraint history — Barrier-constraint history.

Square trajectory, hard CBF: three-dimensional response — Three-dimensional response.

Soft CBF response

Square trajectory, soft CBF: velocity command modification — Velocity command modification.

Square trajectory, soft CBF: relaxed barrier and slack history — Relaxed barrier and slack history.

Square trajectory, soft CBF: three-dimensional response — Three-dimensional response.

Trajectory-level comparison

Resulting visual-information quality

Square trajectory: feature availability, nominal baseline — Nominal: new triangulations drop sharply in low-parallax intervals.

Square trajectory: feature availability, hard CBF — Hard CBF: commanded motion restores new triangulations in degraded intervals.

Square trajectory: feature availability, soft CBF — Soft CBF: usable supply of new triangulations with less path distortion.

Scenario II — Climbing trajectory

3 m/s forward · 1 m/s up · from 30 m altitude · 100 m forward traverse

Here the dominant effect is altitude-dependent: as the camera moves away from the scene, a larger translational displacement is needed to produce an informative change in bearing geometry — a constant nominal velocity becomes insufficient beyond a certain altitude, even before the terminal segment.

Hard CBF response

Climbing trajectory, hard CBF: velocity command modification — Velocity command modification.

Climbing trajectory, hard CBF: barrier-constraint history — Barrier-constraint history.

Climbing trajectory, hard CBF: three-dimensional response — Three-dimensional response.

Soft CBF response

Climbing trajectory, soft CBF: velocity command modification — Velocity command modification.

Climbing trajectory, soft CBF: relaxed barrier and slack history — Relaxed barrier and slack history.

Climbing trajectory, soft CBF: three-dimensional response — Three-dimensional response.

Trajectory-level comparison

Resulting visual-information quality

Climbing trajectory: feature availability, nominal baseline — Nominal: the climb progressively loses feature-observability with altitude.

Climbing trajectory: feature availability, hard CBF — Hard CBF: strongest triangulation-priority response.

Climbing trajectory: feature availability, soft CBF — Soft CBF: same qualitative benefit with reduced deviation from the climb.

Flight Tests

From simulation to the field

Same γ, thresholds & velocity limit as SITL — zero retuning

Real-flight experiments use a custom multicopter with a T-Motor propulsion system, a MATEKSYS flight controller and magnetometer, an NVIDIA Jetson Orin Nano onboard computer, and a downward-facing 80° field-of-view CSI camera. Only the climbing scenario was flown, in nominal and hard-CBF modes.

Custom quadrotor platform, body view — Multicopter platform: GPS, magnetometer, and flight-controller placement.

Downward-facing fisheye camera module and onboard computer — Onboard computer and downward-facing CSI fisheye camera installation.

Hard-CBF response in the climbing flight test

The command-level response closely reproduces the corresponding SITL behavior. Early in the climb, available parallax is sufficient and the corrected command stays close to nominal; as altitude increases, the same translational motion produces progressively weaker bearing variation, the barrier approaches its boundary, and the hard CBF becomes increasingly active — concentrated mainly in the forward and vertical velocity components, with the corrected speed rising toward the 10 m/s limit and the barrier regulated near zero once the constraint is active.

Flight test, hard CBF: velocity command modification — Velocity command modification.

Flight test, hard CBF: barrier-constraint history — Barrier-constraint history.

Flight test, hard CBF: three-dimensional response — Three-dimensional response, closely reproducing the SITL trajectory-level deviation.

Resulting visual-information quality

In the nominal flight, newly triangulated features become sparse during the low-parallax interval, followed by a pronounced drop in the persistent SLAM-feature population. Under the hard CBF, the additional parallax-generating motion sustains new feature initialization and keeps the persistent-feature population close to its available maximum over the same interval — using the unmodified SITL parameterization, with no retuning for real sensing, estimation, or vehicle-response effects.

Flight test: feature availability, nominal baseline — Nominal flight: new triangulations and persistent SLAM features both decline in the low-parallax interval.

Flight test: feature availability, hard CBF — Hard CBF: sustained new triangulations and near-maximal persistent-feature population over the same interval.

Citation

BibTeX

@article{sanlan2026tangovio,
  title   = {{TANGO-VIO}: Triangulation-Aware Navigation with Guaranteed
             Feature-Observability for Visual-Inertial Odometry},
  author  = {{\c{S}}anlan, Abd{\"u}lbaki and Altunkaya, Ege C. and
             Ba{\u{g}}c{\i}, Hasan Taha and Koyuncu, Emre and {\"O}zkol, {\.I}brahim},
  journal = {Under review},
  year    = {2026},
  note    = {Istanbul Technical University}
}