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

The acquisition of attitude, velocity, and position is an essential task in inertial navigation, achieved by integrating inertial sensor measurements. Recently, the functional iteration approach (iNavFIter) has emerged as a solution to achieving ultra-precision inertial navigation computation, remarkably mitigating noncommutativity errors to almost machine precision. However, the practicality of iNavFIter is significantly constrained by its high computational demands. This paper proposes a matrix formulation of the functional iteration approach, named as iNavFIter-M. In contrast to the implicit iterative relationship in the original iNavFIter, the Chebyshev polynomial coefficients between successive iterations are explicitly connected through the matrix formulation. The explicit coefficients iteration and matrix construction reduces theoretical complexity, enables straightforward algorithmic implementation and pre-computation of constant matrix factors, further saves computational resources required for polynomial truncation, and offers potential for parallel computation as well. Simulation results demonstrate that the iNavFIter-M algorithm successfully reduces the computational cost of the original iNavFIter by about one order to a level approaching that of traditional algorithms, while retaining its ultra-high computational precision. The algorithm has also been deployed on a FPGA board within an online system, capable of real-time processing of inertial sensor data at sampling rate up to 2000 Hz.