Consistent invariant mathematical models for a visualinertial navigation system with a block fusion filter

Mathematical models for monocular visual-inertial navigation systems (VINS) are consi¬dered in the context of well-known limitations of recursive filtering algorithms, namely consistency issues and computational efficiency. The aim of the study is to develop consistent VINS error models with tracking of two types of visual features: point landmarks parameterized by inverse depth and ArUco fiducial markers used as known landmarks. The proposed models are adapted for use in a VINS based on a Fast Block Kalman Filter (FBKF), a fully recursive algorithm that approximates the estimates of the Extended Kalman Filter (EKF) at significantly lower computational cost. Materials and Methods. The mathematical models of the VINS are linearized with respect to right-invariant errors, and the behavior of the models is analyzed under unobservable transformations of the VINS parameters. Numerical testing of the FBKF-based VINS with the developed models was carried out in the MATLAB environment using standard implementations of a KLT point tracker and an ArUco marker detector. Results. The effectiveness of the developed models for the FBKF-based VINS is confirmed by numerical simulation of a landing scenario using a KLT tracker and an ArUco marker detector. Consistency between the predicted 3σ confidence bounds of the fusion filter estimation errors and the actual error values is demonstrated, along with the absence of false observability and an acceleration of FBKF computations compared to the Extended Kalman Filter while maintaining comparable estimation accuracy. Conclusion. The conducted study confirms that, when applied in recursive filtering algorithms, the proposed models eliminate false observability of absolute position and yaw estimation errors and enable reliable estimation under conditions of large initial uncertainty. Future work will focus on the analysis of more complex and longer-duration motion scenarios.