The paper proposes an algorithm based on the Multi-State Constraint Kalman Filter (MSCKF) algorithm to construct the map for robots special in the poor GPS signal environment. We can calculate the position of the robots with the data collected by inertial measurement unit and the features extracted by the camera with MSCKF algorithm in a tight couple way. The paper focuses on the way of optimizing the position because we adopt it to compute Kalman gain for updating the state of robots. In order to reduce the processing time, we design a novel fast Gauss–Newton MSCKF algorithm to complete the nonlinear optimization. Compared with the performance of conventional MSCKF algorithm, the novel fast-location algorithm can reduce the processing time with the kitti datasets.

The odometers of robots can divide into two parts: motion propagation and state updating end [

In order to reduce the processing time, we design the novel fast Gauss–Newton MSCKF algorithm without additional hardware cost. The key point of our work is calculating the position and the attitude optimized by the novel nonlinear method. Then, we use the result to compute the Kalman gain in EKF framework.

MSCKF algorithm is based on the EKF architecture. The algorithm is composed of three parts: propagation, augmentation, and update [

Considering the association between the features, MSCKF algorithm updates the propagation by the collection of the observed feature. At the same time, the MSCKF algorithm eliminates the features that are unobserved and worthless. Then, we can compute the reprojection errors of the observed features with nonlinear optimization to update the state matrix. Now, the observation equation denotes as follows:

In this way,

Now, we can use

The value of

At the first stage, the difference is so big that we use the gradient descent method to optimize. We make the Taylor expansion of the equation as follows:

We can get the gradient descent of

At the second stage, we use the Gauss–Newton method to get

Then, we can compute the value of

In order to compute the real value of motion matrix, we bring the prior value of the coordinate into the above equations. If we describe the prior-estimated value of position of the No.

In order to make clearer of the expression, we describe as follows:

Now, we can update

Now, we update the error between the estimated value and the prior value as follows:

The prior value of

In the above equation,

At the first stage, when

If

The experiment of the paper based on kitti datasets. The preprocess of the datasets is extracting the features and checking the external parameters of the cameras [

The performance of the novel MSCKF algorithm with No.1 datasets.

Process time (s) | Final error (m) | |
---|---|---|

Conventional MSCKF with the data of IMU alone | 19.02 | 2.32 |

Conventional MSCKF with the data of IMU and the camera | 19.02 | 1.01 |

Fast Gauss–Newton MSCKF with the data of IMU and the camera | 15.09 | 1.04 |

There are 98 frames of No. 1 datasets at Table

There are 239 frames of No. 2 datasets at Table

The performance of improved MSCKF algorithm with No. 2 datasets.

Process time (s) | Final error (m) | |
---|---|---|

Conventional MSCKF with the data of IMU alone | 58.80 | 2.29 |

Conventional MSCKF with the data of IMU and the camera | 58.80 | 3.32 |

Fast Gauss–Newton MSCKF with the data of IMU and the camera | 55.30 | 3.52 |

Then, we can compare the trajectory computed by the conventional MSCKF algorithm with the one computed by the novel MSCKF algorithm. In order to make clear the result, we also compare the trajectory computed by the conventional MSCKF using the data of IMU alone with the global truth trajectory.

There are the trajectories calculated by different algorithms. The blue trajectory calculated by the conventional MSCKF algorithm is shown in Figure

The trajectories by conventional MSCKF algorithm with No. 1 datasets.

The trajectories by novel MSCKF algorithm with No.1 datasets.

We also calculate the trajectories by the above algorithms with No. 2 datasets to test the performance of the novel MSCKF algorithm when the VIO runs longer. The red trajectories means the ones calculated by the conventional MSCKF algorithm with the IMU data alone under No. 2 datasets in Figure

The trajectories of MSCKF with No. 2 datasets.

The trajectories of improved MSCKF with No. 2 datasets.

Comparing the performances of the novel MSCKF algorithm with the conventional MSCKF algorithm, the processing time of the novel MSCKF algorithm decreases by 20.7%∼6.0%. The final error calculated by the novel MSCKF algorithm increases by 2.0%∼6.0% than the one calculated by the conventional MSCKF algorithm. We can get the conclusion that the novel MSCKF can reduce the processing time. However, the final error computed by the fast Gauss–Newton MSCKF becomes bigger when the running time is longer. It means the novel MSCKF will increase the final error. We can employ it at the low precision and fast speed occasion. Then, there is obvious hip change at the trajectories calculated by the novel MSCKF algorithm because of the switching of the two optimization methods. We also find that the error calculated by the novel MSCKF becomes bigger when the VIO runs longer. In order to reduce the accumulated error, we can add the close-loop detection.

The data of the manuscript is from the kitti datasets which can be accessed through MATLAB. The data can be obtained from

The authors declare that they have no conflicts of interest.

This work was supported by the Department of the Fujian Science and Technology (Grant 2018H0003), Fujian University of Technology (Grant GY-Z17011), Quanzhou Science and Technology Bureau (Grant 2017G012), Department of Education of Fujian Province (Grant JT180339), and Fujian Universtiy of Technology (JGBK201911).