Fusion algorithm of SINS/vision integrated navigation system

The fusion algorithm of SINS/vision integrated navigation system is mainly based on Kalman filter. The prerequisite for the optimal estimation of Kalman filter is that the system model has to be accurately known. To obtain measurement informations, SINS/vision integrated navigation system needs to process images, extract and match feature points and so on. It makes the measurement noise statistical model inaccurately know, thus causing decrease of the Kalman filter estimation accuracy. In order to solve this problem, an improved adaptive two-stage kalman filter is proposed according to the method of solving genetic factor. The improved algorithm respectively applies in two cases, one is that the system noise statistical model cannot be accurately known and the other is that the measurement noise statistical models cannot be accurately known while both have a unified filter framework. The simulation results show that the improved adaptive two-stage kalman filter is more accurate than Kalman filter and it can effectively solve the accuracy decline problem of SINS/vision integrated navigation system caused by the inaccurate noise model.