Square-Root Schmidt-Kalman Filtering for Large-Scale Visual-Inertial SLAM
Find Innovative SolutionsGenerate Solutions
Solution Overview
Problem
Existing vision-aided inertial navigation systems face challenges in efficiently computing state estimates for large-scale simultaneous localization and mapping (SLAM) due to high computational and memory requirements, especially in resource-constrained devices and real-time applications.
Innovation Solution
The implementation of a square-root inverse form of a Schmidt-Kalman Filter (SR-ISF) estimator, which maintains uncertainty data as a Cholesky factor of a Hessian matrix, allowing for efficient computation of state estimates and reducing computational complexity by increasing the size of the state vector linearly with processing and memory requirements.
Engineering Contradictions & Design Principles
Engineering Contradiction Analysis
1Measurement precision
If traditional Kalman filter methods are used for large-scale SLAM, then estimation accuracy can be maintained, but computational complexity and memory requirements increase exponentially
Solution Approach 1:
The patent segments the state vector into two parts: a sliding window of recent states and a map of all observed features. This segmentation allows the filter to process only the relevant recent states in the sliding window while maintaining information about all features in the map, reducing computational complexity from exponential to linear with respect to the state vector size.
Solution Approach 2:
The patent transitions from maintaining the full covariance matrix to maintaining only the Cholesky factor of the Hessian matrix. This dimensional reduction transforms the computational problem from managing a complete n×n matrix to managing a more compact factorization, significantly reducing memory requirements and computational burden while preserving estimation accuracy.
2Area of stationary object
If the state vector size increases to cover large-scale environments, then mapping coverage improves, but processing and memory requirements increase exponentially
Solution Approach 1:
The patent divides the state vector into a sliding window portion containing recent states and a map portion containing all observed features. This segmentation enables the system to handle large-scale environments by processing only the relevant recent states in the sliding window, reducing computational complexity to linear with respect to the state vector size while maintaining extensive mapping coverage.
Solution Approach 2:
The patent implements a sliding window mechanism that discards older state information beyond the window size while recovering and maintaining essential feature map information. This allows the system to manage large-scale environments efficiently by selectively retaining only the necessary recent states for processing while preserving global feature knowledge in the map.
3Loss of information
If standard covariance matrix methods are used, then uncertainty representation is complete, but numerical stability deteriorates when eigenvalues are close to zero
Solution Approach 1:
The patent transforms the representation from the full covariance matrix to the Cholesky factor of the Hessian matrix. This transformation improves numerical stability by avoiding direct manipulation of the covariance matrix when eigenvalues are close to zero, while the Cholesky factorization inherently handles the uncertainty information in a numerically stable manner.
Solution Approach 2:
The patent changes the mathematical representation from covariance matrix elements to Cholesky factor elements. This parameter transformation maintains complete uncertainty information while improving numerical stability, as the Cholesky factorization provides a more robust representation that avoids the numerical issues associated with near-singular covariance matrices.
Data Source
AI summary
A vision-aided inertial navigation system comprises an image source to produce image data for poses of reference frames along a trajectory, a motion sensor configured to provide motion data of the reference frames, and a hardware-based processor configured to compute estimates for a position and orientation of the reference frames for the poses. The processor executes a square-root inverse Schmidt-Kalman Filter (SR-ISF)-based estimator to compute, for features observed from poses along the trajectory, constraints that geometrically relate the poses from which the respective feature was observed. The estimator determines, in accordance with the motion data and the computed constraints, state estimates for position and orientation of reference frames for poses along the trajectory and computes positions of the features that were each observed within the environment. Further, the estimator determines uncertainty data for the state estimates and maintains the uncertainty data as a square root factor of a Hessian matrix.