Algorithm 1. Summary of the SINS/WSN FATCI algorithm.

1: Initialize state vector ${\widehat{\mathit{x}}}_{\mathbf{0}}$ and covariance matrix P0

2: for k = 1, 2, 3, … do

3:   Obtain $\widehat{\mathit{z}}$ and $\widehat{\mathit{\sigma }}$ from WSN measurement

4:   if $\widehat{\mathit{z}}\ne {\mathit{z}}_{\mathit{k}-\mathbf{1}}$ then

5:     Evaluate $\widehat{\mathit{z}}$ and get the number of measurement

6:         ${\mathit{z}}_{\mathit{k}}=\widehat{\mathit{z}}$, ${\mathit{\sigma }}_{\mathit{W},\mathit{k}}=\widehat{\mathit{\sigma }}$

7:     Calculate ${\mathbf{S}}_{\mathit{k}}$ from FIS based on the $\widehat{\mathit{\sigma }}$

8:     Update the measurement covariance error matrix           ${\mathit{R}}_{\mathit{k}+\mathit{1}}={\mathit{S}}_{\mathit{k}+\mathit{1}}^{\mathit{b}}{\mathit{R}}_{\mathit{k}}$

9:     Compute Kalman gain matrix        ${\mathit{K}}_{\mathit{k}}={\mathit{P}}_{\mathit{k}|\mathit{k}-\mathit{1}}{\mathit{H}}_{\mathit{k}}^T\left[{\mathit{H}}_{\mathit{k}}{\mathit{P}}_{\mathit{k}|\mathit{k}-\mathit{1}}{\mathit{H}}_{\mathit{k}}^T+{\mathit{R}}_{\mathit{k}}\right]$

10:      Update measurement equations using (28) and (29)

11:      Predict state equations using (26) and (27)

12:   end if

13: end for