Algorithm 2. Specific steps of the MFCKF.

Input:${\mathit{X}}_{\mathit{k}}$, ${\mathit{P}}_{\mathit{k}}$, ${\mathit{Q}}_{\mathit{k}}$

Output:${\mathit{X}}_{\mathit{k}}+1$, ${\mathit{P}}_{\mathit{k}}+1$, ${\mathit{Q}}_{\mathit{k}+1}$

1. Decompose $X_k$ and $P_k$ and obtain sub-states (i.e., $X_k^{imu}$, $X_k^{uwb}$, and $X_k^{odm}$)

and their covariance (i.e., $P_k^{imu}$, $P_k^{uwb}$, and $P_k^{odm}$);

2. Filter the sub-filters based on the sub-states and sub-covariances as well as the measured values and

obtain the sub-states and sub-covariances at time $k+1$ ($X_{k+1}^{imu}$, $X_{k+1}^{odm}$, $X_{k+1}^{uwb}$ and $P_{k+1}^{imu}$, $P_{k+1}^{odm}$, $P_{k+1}^{uwb}$, $Q_{k+1}$)

(based on the filter shown in Algorithm 1);

3. Eliminate redundant information and get sub-states for fusion (i.e., $X_{k+1}^{imu}$, $X_{k+1}^{uwb}$, and $X_{k+1}^{odm}$)

and their variances (i.e., $P_{k+1}^{imu}$, $P_{k+1}^{uwb}$, and $P_{k+1}^{odm}$);

4. Fuse the sub-states and covariance based on the FKF framework and thus obtain $X_{k+1}^{Velocity}$ and $X_{k+1}^{Position}$

as well as their covariance $P_{k+1}^{Velocity}$ and $P_{k+1}^{Position}$;

5. Recombine $X_{k+1}^{imu}$, $X_{k+1}^{Velocity}$ and $X_{k+1}^{Position}$ to obtain the final status update $X_{k+1}$;

6. Recombine $P_{k+1}^{imu}$, $P_{k+1}^{Velocity}$ and $P_{k+1}^{Position}$ to obtain the final status update $P_{k+1}$;

7. Output $X_{k+1}$, $P_{k+1}$, $Q_{k+1}$ as the initial value for the next moment, and repeat Steps 1–6.

End.