PaperlayerView exact revision on arXiv

Source-linked AI summary

IMU-Free Body-Frame State Estimation with Sparse Scene Flow for Quadcopters

Daniel Grønhaug, Sofie Markeset, Mathias Kolberg

arXiv:2608.20891v1cs.ROcs.CV

TL;DR

The paper addresses body-frame state and scene-flow estimation for quadcopters without inertial or world-frame infrastructure. It combines a manifold EKF with a nonlinear joint point-velocity solver using stereo imagery and thrust commands. The system reports centimetre-scale median velocity error but shows transient errors during takeoff and landing, while several calibration and implementation limitations remain.

  • Problem

    The system targets vision-only quadcopter state and scene-flow estimation without IMU, GPS, or world-frame infrastructure.

  • Method

    A manifold continuous-discrete EKF and nonlinear MAP solver estimate body-frame state, pose change, and point positions and velocities from stereo images, thrusts, stationary scene points, and priors.

  • Results

    Median velocity error is approximately 1 cm/s, while the 95th percentile is 0.5 m/s and errors concentrate in takeoff and landing transients.

  • Takeaways & Limitations

    The solver-derived pose change is recommended for propagating a downstream world model anchored in the current body frame.

  • Takeaways & Limitations

    Estimated inertia, drag, and camera-to-body rotation remain educated guesses, and identifying them properly is outstanding work.

Abstract

from arXiv · show

We present a vision-only state estimation system for X-configuration quadcopters equipped with a canonical stereo camera pair and no inertial sensors. The system operates entirely in the body frame, requiring only synchronised stereo images and motor thrust commands. A continuous-discrete extended Kalman filter on a composite manifold state $\langle SE(3), \mathbb{R}^3, \ldots \rangle$ maintains estimates of body-frame pose, velocity, angular velocity, gravity, and disturbances, using stationary scene points as implicit inertial references. Feature points are detected (FAST, Shi-Tomasi), tracked temporally (SSD, Lucas-Kanade) and matched across cameras (NCC), with search regions predicted from filter-derived pose and point uncertainty. Chi-squared gating on the normalised innovation admits only stationary points to the filter. The system also produces a sparse 3D point cloud carrying per-point position, velocity and joint covariance. These come from a 4-view (two stereo pairs at two timestamps) full bundle adjustment that jointly estimates position and velocity from stereo disparity and temporal parallax, with the filter-derived relative pose as a prior. Feature points in the EKF do not enter the solver; their information is reflected through the pose prior. Point cloud density is spatially adaptive: an external focus point directs allocation, producing dense coverage in the region of attention and sparse coverage elsewhere. The output is a body-frame state estimate, a calibrated pose change, and a sparse scene flow. It is intended as a measurement source for a downstream world model anchored in the current body frame, without dependence on GPS, IMU, or any world-frame infrastructure, though the architecture accommodates their future integration.

1 Introduction

The system estimates quadcopter state, frame-to-frame motion, and sparse scene flow from synchronised stereo images and motor thrusts, entirely in the body frame. Coupled filtering and joint point-velocity solving support downstream local-world-model propagation without IMU, GPS, or world-frame infrastructure.

  • Synchronised stereo images and motor thrust commands produce a covariance-bearing body-frame state, pose change, and sparse 3D point cloud with per-point velocity and uncertainty.The state includes pose, velocity, angular velocity, gravity, and disturbances.
  • A continuous-discrete EKF on a composite manifold uses stationary scene points as inertial references, with pixel measurements correcting the state through covariance cross-correlations.A gravity-magnitude pseudo-measurement prevents drift in the gravity estimate.
  • A nonlinear MAP solver jointly estimates point positions, velocities, and pose change using filter-derived pose and stereo-triangulated point priors.EKF feature points remain outside the solver, so their information enters through the pose prior rather than being double-counted.
  • Feature, pre-admission, and focus-steered interest points are combined into a uniform output cloud, allowing task-directed dense coverage around an external focus.Interest points provide spatially adaptive coverage while all candidate and interest points enter the joint solver.
  • The body-frame formulation avoids IMU, GPS, and world-frame infrastructure, while the solver’s pose change propagates a downstream world model anchored in the current body frame.The recommended transform is the solver-derived frame-to-frame pose change rather than an accumulating absolute pose.

2 Mathematical Preliminaries

The preliminaries establish Lie-group and composite-manifold operations, camera projection and stereo geometry, rigid-body dynamics, and covariance identities used by the estimator.

  • Lie algebra and composite manifolds: SE(3) poses use Lie algebra coordinates ξ ∈ R6, with exponential and logarithmic maps connecting tangent vectors and poses.Rotations are represented in angle-axis form, and right-plus/right-minus define pose perturbations and differences.
  • Lie algebra and composite manifolds: The composite state combines an SE(3) body pose with Euclidean R3 components, using block-wise manifold or standard derivatives.Composite addition and subtraction apply the appropriate operation to each state block.
  • Camera and stereo geometry: Camera projection maps 3D points through perspective division, distortion, and intrinsics into pixels, while inverse projection recovers a ray with unknown depth.A single image cannot recover depth because projection is many-to-one.
  • Camera and stereo geometry: Canonical rectified stereo constrains correspondences to the same scanline, with horizontal disparity encoding depth and depth uncertainty increasing quadratically with distance.The stereo baseline determines the camera-to-camera transformation.
  • Dynamics and uncertainty: The dynamics use body-frame transport, rigid-body rotation, torque, thrust, drag, and covariance identities to model motion and uncertainty.World-fixed vectors expressed in the rotating body frame follow the transport theorem.

3 Extended Kalman Filter

The estimator is a continuous-discrete extended Kalman filter that maintains a Gaussian belief while extending Euclidean filtering to a composite manifold state.

  • Filter formulation: The filter maintains a Gaussian approximation represented by the state estimate X̂ and covariance P, supporting innovation-based chi-squared tests.This approximation underlies the filter’s statistical gating procedure.
  • Filter formulation: Continuous-time system dynamics are combined with discrete-time measurements in an EKF formulation extended from Euclidean states to a composite manifold.The formulation follows prior manifold-filter treatments while retaining familiar Euclidean EKF notation.

3.1 Filter Equations

The filter propagates state and covariance continuously or discretely, handles variable-length feature states, and maintains feature-state correlations during augmentation and removal.

  • Filter equations: The proposed composite-manifold continuous-discrete formulation is presented as an Ansatz rather than a proved result.The authors state that it is verified to reduce to cited formulations.
  • Propagation: Continuous covariance propagation uses FPF^T and process-noise injection GQG^T, while discrete propagation uses first-order Forward Euler integration.The discrete transition uses Φ_k = I + Δt_k·F_k.
  • Propagation: The actual frame interval Δt_k is used rather than assuming a constant timestep.This makes propagation depend on the measured interval between consecutive frames.
  • Propagation: Pose propagation uses left-acting dynamics, with right-acting perturbations obtained through the adjoint when needed.The paper avoids restating pose propagation as right acting because doing so adds computational cost without benefit.
  • Feature-state management: Feature states enter and leave the variable-length filter through augmentation, marginalisation, and visibility transitions, while cross-correlations are preserved or seeded.Visibility changes alter measurement rows and bookkeeping labels but require neither augmentation nor marginalisation.

3.2 System Model

The system model represents body-frame pose, motion, gravity, disturbances, and stationary feature positions, driven by rotor thrust and stochastic process models.

  • State representation: The composite state contains the body pose and Euclidean components for velocity, angular velocity, gravity, disturbances, and feature positions.All feature positions are expressed in the body frame, with visibility determining their measurement contributions.
  • State representation: Gravity and disturbances are modeled separately because gravity is approximately world-constant while wind disturbances are stochastic.Separate process-noise tuning assigns gravity a small spectral density and disturbances a larger one.
  • Motion dynamics: Rotor thrust and torque mappings drive translational and rotational dynamics, with additional noise representing actuator uncertainty and unmodelled aerodynamic torques.The rotational model assumes a diagonal inertia tensor for the symmetric X-configuration.
  • Motion dynamics: The pose dynamics recover rotation kinematics and transport-theorem translation dynamics for the body-frame expression of a world-fixed origin.The pose uncertainty is inherited from velocity and angular velocity through integration rather than direct pose noise.
  • Feature dynamics: World-stationary feature points evolve deterministically in the body frame, and chi-squared testing removes points that violate the stationarity assumption.The same transport relationship applies across stereo, mono-left, and mono-right visibility groups.
  • Process model: The complete system model combines pose, velocity, angular velocity, gravity, disturbance, and feature dynamics with actuator, torque, gravity, and disturbance process noise.Setting w = 0 yields the nominal dynamics used for state propagation.

3.2.6 Noise Jacobian G

The noise Jacobian G maps Euclidean process noise into the filter’s perturbation space, while the composite state Jacobian F captures deterministic error dynamics independently of noise.

  • Noise mapping: G maps 13-dimensional process noise into an 18 + 3Nf-dimensional perturbation space.Its column groups correspond to actuator, angular-velocity, gravity, and disturbance noise.
  • Noise mapping: Actuator noise enters velocity through Bv and angular velocity through the structured mixing matrix Bω.Angular velocity also receives unmodelled torque noise wω.
  • State Jacobian: Pose error dynamics couple to velocity and angular velocity through −Ad ˆT −1, while pose does not receive direct noise.The pose row is zero in the direct noise mapping, and the pose-error column is zero in the other R3 dynamics.
  • State Jacobian: The noise terms do not affect F because they enter linearly through G without coupling to the state.F is evaluated at zero process noise.
  • Feature-point dynamics: Each feature-point state has a diagonal self-dynamics block and angular-velocity coupling, with no cross-point terms.All points share the same velocity coupling Fpv = −I3.

3.3 Measurement Model

The measurement model projects body-frame feature positions into one or both cameras and uses pixel observations to update the filter through propagated covariance correlations.

  • Observations: Each frame supplies LK-tracked pixel coordinates, with four measurements for stereo features and two for mono features.The assembled measurement stacks the appropriate stereo or mono projection for every point.
  • Projection model: The prediction maps each body-frame point to pixel coordinates by applying fixed camera extrinsics and direct geometric projection.Stereo observations are represented by the two camera projections rather than a separate disparity row.
  • Noise model: Pixel noise is additive, with a shared tuning range of approximately 0.3–1.0 px and block-diagonal covariance across points and cameras.The model attributes the dominant error source to LK sub-pixel localisation.
  • Jacobians: The measurement Jacobian is nonzero only in the columns of the observed point, making the assembled matrix block-sparse.A point’s measurement does not directly depend on pose, velocity, angular velocity, gravity, disturbance, or other points.
  • Filter update: Pose and dynamic states are corrected indirectly through covariance cross-correlations formed during propagation from point, velocity, and angular-velocity dynamics.Feature-point innovations therefore influence states absent from the direct measurement Jacobian.
  • Gravity constraint: A gravity-magnitude pseudo-measurement constrains ∥gB∥ to the known value g^2 and is intended to prevent magnitude drift.The measurement noise should accommodate small spatial and altitude-related gravity variations, while unmodelled forces remain in dB.

3.4 Relative Pose from Filter State

The filter derives a frame-to-frame relative pose and covariance by combining pose perturbations at consecutive times, with post-update covariance providing the preferred solver prior when available.

  • Relative pose: The relative pose ∆T = TBk,Bk−1 and its covariance are extracted from the filter state.The relative-pose perturbation is expressed in the preceding body frame Bk−1.
  • Perturbation mapping: The relative-pose perturbation combines the current and previous pose perturbations through the adjoint of the previous estimated pose.At the identity pose, this reduces to the difference between the two perturbations.
  • Covariance: The joint covariance requires cross-covariance between consecutive filter perturbations, not just separate single-time pose blocks.Propagation couples pose with velocity and angular velocity, contributing to this cross-covariance.
  • Covariance: Propagation increases pose covariance because process noise is added before the measurement update.The propagated pose block is therefore larger than the preceding post-update block.
  • Solver prior: The post-update covariance is tighter and more informative, so it should be used when the joint solver runs after the filter update.Its improvement depends on the quality of feature matches because pose is corrected indirectly through feature cross-correlations.
  • Numerical caveat: The post-update covariance can develop eigenvalues near −10^-7, which the implementation clips at 10^-12 rather than fully resolving.The reported likely sources are selector construction across changing state dimensions and the J^-1 approximation.

4 Joint MAP Solver

The joint MAP solver estimates a pose change together with per-point position and velocity from tracked multi-view observations, using manifold-aware optimization and a filter-derived pose prior.

  • MAP optimization: The optimization minimizes whitened residuals with Gauss–Newton updates, combining measurement terms with priors as virtual measurements.The covariance is approximated by the inverse approximate Hessian at the solution.
  • Priors: Manifold priors use prior ⊖ estimate residuals and tangent-space Jacobians, while partial priors constrain only the selected state components.This supports a pose prior on SE(3) alongside Euclidean point states.
  • Joint state: The solver jointly estimates one SE(3) pose change and Euclidean position-velocity states for all tracked points.It is formulated as a four-view bundle adjustment over two stereo pairs at two timestamps.
  • Priors: The filter-derived pose change supplies the solver prior, while stereo triangulation supplies priors for point positions.The pose prior is expressed in Lie algebra coordinates near the identity for typical frame intervals.
  • Visibility limitation: MM points require camera-centre motion and are currently excluded because of unresolved numerical instability.Pure rotation makes the perpendicular epipolar-plane construction degenerate.
  • Measurement model: The measurement model transports each point between body frames using ∆T and then projects it into the relevant camera.SS, SM, and MS correspondences use full velocity, while MM uses only perpendicular velocity.

5 Measurement Pipeline

The measurement pipeline detects, selects, matches, reconstructs, and tracks image points using uncertainty-aware search regions. It then tests motion consistency, admitting only points that pass stationarity checks while recognizing an MM observability limitation.

  • Detection and selection: FAST and Shi-Tomasi detect and rank candidate features, while score thresholds reject unreliable or absent detections.Shi-Tomasi favors patches with strong gradients in two independent directions.
  • Temporal and stereo matching: NCC matches stereo patches along horizontal epipolar lines, with thresholding and optional parabolic sub-pixel refinement.For rectified stereo, the search is one-dimensional over the disparity range, whose disparity encodes depth.
  • Detection and selection: Feature allocation combines global and grid-based Shi-Tomasi selection, whereas interest-point allocation concentrates samples around an externally supplied focus.The focus distribution is controlled by σF: smaller values concentrate points, while larger values spread them.
  • 3D reconstruction: Stereo observations reconstruct 3D points and propagate pixel uncertainty, accounting for the shared effect of uL on both image coordinates and disparity depth.Mono features instead receive initial point and covariance estimates from the point-velocity solver.
  • Temporal and stereo matching: Temporal tracking uses SSD for coarse matching, pyramidal Lucas-Kanade for sub-pixel refinement, and forward-backward error to reject inconsistent tracks.Search candidates are projected from sampled pose, position, velocity, and uncertainty sets before matching.
  • Feature admission: A velocity-consistency gate rejects points whose normalised innovation exceeds its threshold, while MM motion within the epipolar plane remains unobservable to this test.The later NIS gate is described as catching such points.

6 Algorithm Overview

The algorithm couples an EKF for body-frame state estimation with a joint solver for point motion and pose change, while managing feature, pre-admission, and focus-steered interest points. Each iteration propagates, tracks, gates, solves, replenishes, and outputs the state, pose change, and point cloud.

  • Point-set roles: Feature points F support EKF state estimation as stationary inertial references, while Fpre and interest points I enter the joint solver.Feature information reaches the solver through the pose prior rather than by reusing EKF feature points directly.
  • Point-set roles: Each point stores observations, 3D state, covariance, identity, and age, with these fields determining its stage, correspondence type, and search strategy.The accumulator persists point sets, the EKF posterior, previous images, focus, and timestamp across iterations.
  • Iteration: Each iteration receives stereo images, timing, thrust, focus, and spread, then propagates the EKF and computes a predicted relative pose.Tracking uses predicted candidate sets, SSD, pyramidal LK, forward-backward validation, and stereo promotion.
  • Outputs: The output comprises a core EKF state, solver pose change with covariance, and a role-tagged point cloud combining EKF, joint-solver, and stereo-only estimates.The solver pose change is recommended for propagating a downstream world model anchored in the current body frame.
  • Iteration: The EKF applies per-feature and joint consistency gates, demotes moving or inconsistent features, and updates surviving inliers with a gravity pseudo-measurement.The filter state retains feature rows internally but strips them from the core output.
  • Joint solve and admission: The joint solver classifies SS, SM, MS, and MM correspondences, uses stereo-derived point priors and a filter-derived pose prior, and estimates point motion with pose change.Newly solved Fpre points undergo velocity consistency testing before EKF augmentation or demotion to I.
  • Outputs: The implementation intends age-based point retirement, but a code comparison against the global maximum prevents retirement from firing in the reported results.Consequently, those results have no maximum point lifetime.

7 Evaluation

Evaluation on a simple indoor hover sequence shows generally accurate state estimates, with errors concentrated around takeoff and landing. The filter recovers after disturbances but remains vulnerable to transient divergence and model-tuning limitations.

  • 7.2 State Estimate Accuracy: Overall state estimates appear accurate, but errors increase sharply during takeoff and landing transients.The authors attribute the transient degradation to the discontinuous normal force and suggest larger disturbance process noise or accelerometer measurements as possible remedies.
  • 7.3 State Diagnostics: The filter can become overconfident because process-noise spectral densities are unvalidated priors, and it collapses when measurements pause for approximately 10 frames.The authors identify a held-out-flight NEES sweep as the way to assess this calibration issue.
  • 7.1 Setup: The VID sequence contains 4733 frames over 79.0 seconds, with ground contact, takeoff, hover flight, and landing in a no-wind, no-payload setting.The sequence was selected as a first validation because its dynamics are simple and dominated by hover.
  • 7.2 State Estimate Accuracy: Median velocity error is approximately 1 cm/s, while the 95th percentile is 0.5 m/s, indicating that errors concentrate in a small fraction of transient frames.The velocity RMSE is much larger than the median, and the affected fraction is identified as the takeoff and landing transient.
  • 7.2 State Estimate Accuracy: Angular-velocity errors show a more uniform noise floor, with pitch rate ωy worst, yaw rate ωz best, and roll rate ωx intermediate.The observed asymmetry is associated with possible camera-body extrinsic or marker-to-body alignment errors, or an intrinsic limitation of vision-only rotation-rate estimation.
  • 7.3 State Diagnostics: Gravity magnitude remains locked at 9.79, while its per-axis estimate wanders by approximately 0.5 m/s2 as gravity rotates with vehicle tilt.The disturbance estimate changes from approximately (0, 0, −9.8) while on the floor to near zero after lift-off, with a 3–5 second transition.

8 Future Work

The paper identifies a future architecture that separates ego-state estimation from environment-state management and points toward a hardware-oriented implementation. It also notes that the current system remains work in progress, with outstanding items documented throughout the paper.

  • Open work: The authors describe the current system as work in progress, with a second version in preparation.Outstanding items are recorded in the relevant preceding sections rather than collected in one list.
  • Future architecture: The proposed future architecture splits the estimator into an ego-state filter and an environment state with a defined interface.The ego-state process would estimate motion-related quantities, while the environment state would manage correspondence, depth, and gating.
  • Future implementation: A subsequent implementation direction is a C++ port with vision primitives running on programmable hardware.
Loading 2608.20891v1…