Visual-inertial state estimation based on Chebyshev polynomial optimization

This paper proposes an innovative state estimation method for visual-inertial fusion based on Chebyshev polynomial optimization. Specifically, the pose is modeled as a Chebyshev polynomial of a certain order, and its time derivatives are used to calculate linear acceleration and angular velocity, which, along with inertial measurements, constitute dynamic constraints. This is coupled with a visual measurement model to construct a visual-inertial bundle adjustment formulation. Simulation and public dataset experiments show that the proposed method has better accuracy than the discrete-form preintegration method.

Linear Relative Pose Estimation Founded on Pose-only Imaging Geometry

How to efficiently and accurately handle image matching outliers is a critical issue in two-view relative estimation. The prevailing RANSAC method necessitates that the minimal point pairs be inliers. This paper introduces a linear relative pose estimation algorithm for n $( n \geq 6$) point pairs, which is founded on the recent pose-only imaging geometry to filter out outliers by proper reweighting. The proposed algorithm is able to handle planar degenerate scenes, and enhance robustness and accuracy in the presence of a substantial ratio of outliers. Specifically, we embed the linear global translation (LiGT) constraint into the strategies of iteratively reweighted least-squares (IRLS) and RANSAC so as to realize robust outlier removal. Simulations and real tests of the Strecha dataset show that the proposed algorithm achieves relative rotation accuracy improvement of 2 $\sim$ 10 times in face of as large as 80% outliers.

Clifford Algebra-Based Iterated Extended Kalman Filter with Application to Low-Cost INS/GNSS Navigation

The traditional GNSS-aided inertial navigation system (INS) usually exploits the extended Kalman filter (EKF) for state estimation, and the initial attitude accuracy is key to the filtering performance. To spare the reliance on the initial attitude, this work generalizes the previously proposed trident quaternion within the framework of Clifford algebra to represent the extended pose, IMU biases and lever arms on the Lie group. Consequently, a quasi-group-affine system is established for the low-cost INS/GNSS integrated navigation system, and the right-error Clifford algebra-based EKF (Clifford-RQEKF) is accordingly developed. The iterated filtering approach is further applied to significantly improve the performances of the Clifford-RQEKF and the previously proposed trident quaternion-based EKFs. Numerical simulations and experiments show that all iterated filtering approaches fulfill the fast and global convergence without the prior attitude information, whereas the iterated Clifford-RQEKF performs much better than the others under especially large IMU biases.

Inertial-based Navigation by Polynomial Optimization: Inertial-Magnetic Attitude Estimation

Inertial-based navigation refers to the navigation methods or systems that have inertial information or sensors as the core part and integrate a spectrum of other kinds of sensors for enhanced performance. Through a series of papers, the authors attempt to explore information blending of inertial-based navigation by a polynomial optimization method. The basic idea is to model rigid motions as finite-order polynomials and then attacks the involved navigation problems by optimally solving their coefficients, taking into considerations the constraints posed by inertial sensors and others. In the current paper, a continuous-time attitude estimation approach is proposed, which transforms the attitude estimation into a constant parameter determination problem by the polynomial optimization. Specifically, the continuous attitude is first approximated by a Chebyshev polynomial, of which the unknown Chebyshev coefficients are determined by minimizing the weighted residuals of initial conditions, dynamics and measurements. We apply the derived estimator to the attitude estimation with the magnetic and inertial sensors. Simulation and field tests show that the estimator has much better stability and faster convergence than the traditional extended Kalman filter does, especially in the challenging large initial state error scenarios.

Investigation of Enhanced Inertial Navigation Algorithms by Functional Iteration

The defects of the traditional strapdown inertial navigation algorithms become well acknowledged and the enhanced traditional algorithms were quite recently proposed trying to mitigate both theoretical and algorithmic defects. In this paper, the accuracies of the traditional algorithms, the enhanced algorithms, and the velocity algorithm based on the velocity translation vector are re-investigated in the common case of two samples, for the first time against the true reference provided by the functional iteration approach that has provable convergence and essentially reduces the noncommutativity errors to machine precision. Notably, the analyses by the help of MATLAB symbolic toolbox reveal the marginal effect of the enhanced algorithms, and the error orders of all algorithms analyzed against functional iteration are consistent with the existing literatures. Numerical results under coning motions agree with analyses that the enhanced algorithms have little significant accuracy improvement over the traditional algorithms, while the functional iteration approach possesses significant accuracy superiority even in sustained lowly dynamic conditions.

iNavFIter-M: Matrix Formulation of Functional Iteration for Inertial Navigation Computation

The acquisition of attitude, velocity, and position is an essential task in the field of inertial navigation, achieved by integrating the measurements from inertial sensors. Recently, the ultra-precision inertial navigation computation has been tackled by the functional iteration approach (iNavFIter) that drives the non-commutativity errors almost to the computer truncation error level. This paper proposes a computationally efficient matrix formulation of the functional iteration approach, named the iNavFIter-M. The Chebyshev polynomial coefficients in two consecutive iterations are explicitly connected through the matrix formulation, in contrast to the implicit iterative relationship in the original iNavFIter. By so doing, it allows a straightforward algorithmic implementation and a number of matrix factors can be pre-calculated for more efficient computation. Numerical results demonstrate that the proposed iNavFIter-M algorithm is able to achieve the same high computation accuracy as the original iNavFIter does, at the computational cost comparable to the typical two-sample algorithm. The iNavFIter-M algorithm is also implemented on a FPGA board to demonstrate its potential in real time applications.

ChevOpt: Continuous-time State Estimation by Chebyshev Polynomial Optimization

In this paper, a new framework for continuous-time maximum a posteriori estimation based on the Chebyshev polynomial optimization (ChevOpt) is proposed, which transforms the nonlinear continuous-time state estimation into a problem of constant parameter optimization. Specifically, the time-varying system state is represented by a Chebyshev polynomial and the unknown Chebyshev coefficients are optimized by minimizing the weighted sum of the prior, dynamics and measurements. The proposed ChevOpt is an optimal continuous-time estimation in the least squares sense and needs a batch processing. A recursive sliding-window version is proposed as well to meet the requirement of real-time applications. Comparing with the well-known Gaussian filters, the ChevOpt better resolves the nonlinearities in both dynamics and measurements. Numerical results of demonstrative examples show that the proposed ChevOpt achieves remarkably improved accuracy over the extended/unscented Kalman filters and RTS smoother, close to the Cramer-Rao lower bound.

Attitude Reconstruction from Inertial Measurement: Mitigating Runge Effect for Dynamic Applications

Time-equispaced inertial measurements are practically used as inputs for motion determination. Polynomial interpolation is a common technique of recovering the gyroscope signal but is subject to a fundamentally numerical stability problem due to the Runge effect on equispaced samples. This paper reviews the theoretical results of Runge phenomenon in related areas and proposes a straightforward borrowing-and-cutting (BAC) strategy to depress it. It employs the neighboring samples for higher-order polynomial interpolation but only uses the middle polynomial segment in the actual time interval. The BAC strategy has been incorporated into attitude computation by functional iteration, leading to accuracy benefit of several orders of magnitude under the classical coning motion. It would potentially bring significant benefits to the inertial navigation computation under sustained dynamic motions.

A Pose-only Solution to Visual Reconstruction and Navigation

Visual navigation and three-dimensional (3D) scene reconstruction are essential for robotics to interact with the surrounding environment. Large-scale scenes and critical camera motions are great challenges facing the research community to achieve this goal. We raised a pose-only imaging geometry framework and algorithms that can help solve these challenges. The representation is a linear function of camera global translations, which allows for efficient and robust camera motion estimation. As a result, the spatial feature coordinates can be analytically reconstructed and do not require nonlinear optimization. Experiments demonstrate that the computational efficiency of recovering the scene and associated camera poses is significantly improved by 2-4 orders of magnitude. This solution might be promising to unlock real-time 3D visual computing in many forefront applications.

A Trident Quaternion Framework for Inertial-based Navigation Part II: Error Models and Application to Initial Alignment

This work deals with error models for trident quaternion framework proposed in the companion paper "A Trident Quaternion Framework for Inertial-based Navigation Part I: Motion Representation and Computation" and further uses them to investigate the static and in-motion alignment for land vehicles. Specifically, the zero-velocity and odometer velocity measurements are applied in the static and in-motion alignment process, respectively. By linearizing the trident quaternion kinematic equation, the right and left trident quaternion error models are obtained. The resultant models are found to be equivalent to those derived from profound group affine. Then the two models are used to design the corresponding extended Kalman filters (EKF), namely, the left-quaternion EKF (LQEKF) and the right-quaternion EKF (RQEKF). Simulations and field tests are conducted to evaluate their actual performances. For the static alignment, owing to their high consistency, the L/RQEKF converge much faster than the EKF even without any heading information. For the in-motion alignment, however, the two filters still need the assistance of the analytical/optimization-based in-motion alignment methods at the very start to avoid extremely large attitude errors, although they possess much larger convergence region than the traditional EKF does.