LAUSR

For more than a decade, the multi-state constraint Kalman filter is used for visual-inertial navigation. Its advantages are the light-weight calculations, consistency, and similarity to the current mature GPS/INS Kalman filters. For using it in an airborne platform, an important deficiency exists. It diverges while the object stops moving. In this work, this deficiency is accounted for, by changing the state augmentation and measurement update policy from a time-based to horizontal travel-based scheme, and by reusing the oldest tracked point over and over. Besides the computational savings, it works infinitely with no extra errors in full-stops and with minor error build up in very low speeds.