Fast Monocular Visual-Inertial Initialization with an Improved Iterative Strategy

Algorithm 2

Computing the step .

Inputs: ,,

Output:

Step 1: If ,then output:

Step 2: Else if , then output:

Step 3: Else output: , where is calculated by equation (17).