Fast Block Kalman Filter For Visual-Inertial Navigation

This paper presents a Fast Block Kalman Filter (FBKF) for visual-inertial navigation. The filter recursively estimates the state vector that includes the navigation parameters of a moving object and the coordinates of N visual features, with computational complexity reduced to O(N) by decomposing the estimation algorithm. Moreover, the O(N) complexity is maintained even when all N features are observed simultaneously for an arbitrary time interval. Through applying the special procedure for expanding the original state vector, based on the principal component analysis, the estimates of the block filter are made close to those of the Extended Kalman Filter (EKF), which, as shown previously, provides high estimation accuracy when the error models are consistent. A comparison with the EKF in terms of computational time and produced estimates is carried out by simulation of a visual aided INS. The obtained results show that negligible deviations from the EKF estimates are obtained when the dimensionality of the expansion of the original state vector has only a minor effect on the computational burden. The ability of processing hundreds of features in real time in single-threaded mode is also demonstrated.