Square-Root Multi-State Constraint Kalman Filter for VINS
Find Innovative SolutionsGenerate Solutions
Solution Overview
Problem
Existing vision-aided inertial navigation systems (VINS) face challenges in accurately tracking the six-degrees-of-freedom position and orientation of a sensing platform, especially in GPS-denied environments, due to limitations in handling multiple feature observations and maintaining numerical stability.
Innovation Solution
The implementation of a square-root multi-state constraint Kalman filter (SR-MSCKF) that computes state estimates for position and orientation by geometrically relating multiple poses from which features were observed, using an Extended Kalman Filter (EKF)-based estimator that maintains uncertainty data as a square root factor of a covariance matrix.
Engineering Contradictions & Design Principles
Engineering Contradiction Analysis
1Reliability
If traditional Kalman filter methods are used to compute state estimates for position and orientation, then the system can process visual and motion data, but numerical instability occurs when handling multiple feature observations from multiple poses
Solution Approach 1:
The patent transforms the covariance matrix into its square root form (Cholesky decomposition), changing the mathematical representation from the original covariance matrix P to its square root factor R where P = R*R^T. This parameter transformation maintains all necessary information while improving numerical stability when computing state estimates from multiple feature observations across multiple poses.
Solution Approach 2:
The patent replaces the traditional Kalman filter computational mechanics with a square-root formulation. Instead of directly manipulating covariance matrices through standard Kalman filter equations, the system uses square root factors and applies triangular decomposition methods, substituting the computational mechanism to avoid numerical instability while preserving localization accuracy.
2Measurement precision
If the system processes multiple feature observations from multiple poses to improve localization accuracy, then measurement precision increases, but the condition number of the covariance matrix increases leading to numerical instability
Solution Approach 1:
The patent changes the parameter representation from the covariance matrix itself to its square root factor. When processing multiple feature observations from multiple poses, this parameter change ensures that the condition number remains manageable, allowing the system to maintain both high localization accuracy and numerical stability simultaneously.
Solution Approach 2:
The patent segments the covariance matrix computation into separate square root factor computations for each pose and feature observation. By processing contributions from multiple poses through individual square root factor updates rather than accumulating in the full covariance matrix, the system avoids condition number escalation while maintaining precision.
3Ease of operation
If the system uses standard covariance matrix representation, then computation is straightforward, but numerical errors accumulate in stereoscopic applications with close eigenvalues
Solution Approach 1:
The patent substitutes the standard covariance matrix manipulation mechanics with square-root filter mechanics. In stereoscopic applications where eigenvalues are close together, this substitution prevents numerical error accumulation by using numerically stable triangular decomposition operations instead of direct covariance matrix operations, while maintaining computational feasibility.
Data Source
AI summary
A vision-aided inertial navigation system (VINS) implements a square-root multi-state constraint Kalman filter (SR-MSCKF) for navigation. In one example, a processor of a VINS receives image data and motion data for a plurality of poses of a frame of reference along a trajectory. The processor executes an Extended Kalman Filter (EKF)-based estimator to compute estimates for a position and orientation for each of the plurality of poses of the frame of reference along the trajectory. For features observed from multiple poses along the trajectory, the estimator computes constraints that geometrically relate the multiple poses of the respective feature. Using the motion data and the computed constraints, the estimator computes state estimates for the position and orientation of the frame of reference. Further, the estimator determines uncertainty data for the state estimates and maintains the uncertainty data as a square root factor of a covariance matrix.