A Novel Loosely Coupled Collaborative Localization Method Utilizing Integrated IMU-Aided Cameras for Multiple Autonomous Robots

Abstract

IMUs (inertial measurement units) and cameras are popular sensors for autonomous localization due to their convenient integration. This article proposes a collaborative localization method, the CICEKF (collaborative IMU-aided camera extended Kalman filter), with a loosely coupled and two-step structure for the autonomous locomotion estimation of collaborative robots. The first step is for single-robot localization estimation, fusing and connecting the IMU and visual measurement data on the velocity level, which can improve the robustness and adaptability of different visual measurement approaches without redesigning the visual optimization process. The second step is for estimating the relative configuration of multiple robots, which further fuses the individual motion information to estimate the relative translation and rotation reliably. The simulation and experiment demonstrate that both steps of the filter are capable of accomplishing locomotion estimation missions, standalone or collaboratively.

1. Introduction

1.1. Motivation

The autonomous localization of multiple robots working on ground and aerial space is well-explored and is achieved with different sensor setups such as visual measurement [1], GNSS (global navigation satellite system) [2], and visual–inertial equipment [3]. However, for search and rescue robots facing emergent or indoor situations [4,5], collaborative autonomous navigation under unknown and GNSS-denied environments [6] aiming for group closed-loop motion control [7,8] is still challenging due to the need for accuracy against environmental uncertainty. Visual–inertial approaches are popular for their robustness and real-time performance against interferences [9,10] to solve localization problems for both individual and group robots. Although the vision-based odometry and SLAM (simultaneous localization and mapping) methods have achieved great accuracy for their batch-optimization capability [11,12,13], the vision-only approaches are genuinely weak in solving the collaborative localization problem owing to the lack of a common world coordinate frame as an anchorage [14,15]. On the other hand, IMUs have good real-time responsiveness, while the inherent drift affects the long-term performance.

Therefore, it is necessary to explore approaches with an open and flexible data fusion framework to improve the accuracy of cooperative group localization, which can balance the instantaneity of the IMU sensors and the accuracy of visual measurement for collaborative agents under GNSS-restricted situations.

1.2. Related Work

The research on autonomous localization based on the data fusion of the IMU and visual measurement mainly focuses on tightly and loosely coupled approaches.

The tightly coupled methods are typically biased towards visual measurement when integrating the IMU’s real-time output from the gyroscope and accelerometer into the batch-optimization process. The state-of-the-art single-robot visual–inertial localization methods, such as ORB-SLAM v3 [16] and MSCKF 2.0 [17], have shown great robustness and accuracy. TH Nguyena et al. [18] proposed a two-step optimization-based collaborative visual–inertial range localization method that separately estimates the relative transformation and single-robot localization. Tian et al. [19] proposed a distributed multi-robot system for robust and dense metric semantic SLAM that detects loop closures and performs distributed trajectory estimation when communication among individuals is available. Though achieving great performance in the tests with the benefits of post-optimization, such tightly coupled methods usually lack both openness due to the fixed optimization structure and low real-time performance due to the high computing power demand. These result in inefficiency in dealing with uncertain topology and different mounted sensors of multi-agents.

Loosely coupled methods are naturally fit to perform relative localization due to their flexible algorithm structure and timeliness by emphasizing the IMU update [20]. The main weakness is their unsatisfactory accuracy owing to the inevitable drift of the IMU measurement, which should be carefully handled.

Kelly and Sukhatme [21] achieved the self-calibration of a monocular visual–inertial system with a specifically designed UKF (unscented Kalman filter). Following a similar approach, Weiss and Siegwart [22] introduced the estimation of the world coordinate frame drift of the visual measurement into a multi-level EKF (extended Kalman filter) for a better trajectory estimation for a single drone. Based on further research, Achtelik and Weiss [23] achieved the estimation of the relative configuration of two drones by utilizing a multi-level EKF to handle the loosely coupled visual–inertial data fusion. Researchers have introduced new filter techniques, such as the invariant EKF based on Lie algebra [24], the multi-state constraint Kalman filter [25], and the equivariant filter [26,27], into the trajectory estimation with visual–inertial equipment.

These loosely coupled methods usually treat visual measurement as a black box, thus achieving a flexible structure with great portability across different sensor setups. However, owing to only considering the update with IMU data on the acceleration and velocity levels, the final correction on the position level with the visual measurement result may lose accuracy when a sudden visual measurement instability is encountered.

1.3. Our Approach

This article presents a loosely coupled filter-based approach, the CICEKF, with two steps aiming for both the autonomous localization of a single robot and the collaborative estimation of relative multi-robot localization. The time complexity of the method is $O\left(n\right)$. The SR-CICEKF (single-robot CICEKF) step focuses on the fusion of the data on the stereo vision and IMU velocity levels from the integrated sensors for a robust locomotive estimation. The MR-CICEKF (multi-robot CICEKF) step fuses outcomes of the SR-CICEKF from multiple robots to estimate the real-time relative position and attitude from each slave robot to the master robot. The two steps can run separately as standalone filters or serially as a complete CICEKF. Furthermore, to demonstrate the feasibility of the CICEKF, the observability is proven.

This article is organized as follows: The design of the state vectors of the CICEKF is described in Section 2, a detailed description of the propagation and update processes of the filter is provided in Section 3, the local weak observability of the CICEKF is demonstrated in Section 4, simulation with quintic curves is presented in the Section 5, and real-world experiments were carried out and are analyzed in Section 6. The conclusions of this study are provided in Section 7.

2. Design of the State Vector

2.1. Definition of Variables

To focus the discussion on the filter-based data fusion process, the stereo vision is treated as black-box autonomous locomotion measurement equipment. It is assumed that each robot is equipped with an IMU-aided stereo-vision sensor suite, of which the relative position and attitude of the IMU and the cameras are already calibrated without further drifting. The locomotive variables and relative transformations are defined as shown in Figure 1, and the coordinate frames and the notation of variables used in this study are provided in

Table 1. Coordinate frames and notations.

Symbol	Description
w	Coordinate frame of the fixed world
i	Coordinate frame attached to the IMU’s rigid body
c	Coordinate frame attached to the rigid body of the dual-camera stereo vision
ic	Coordinate frame attached to the virtual rigid body of the IMU-aided camera system
${\mathit{x}}_A^B$	$\mathit{x}$ represents a general viable vector, $A$ and $B$ are the reference coordinate frames, e.g., ${\mathit{p}}_A^B$ denotes the linear translation in the coordinate frame $B$ measured with respect to the coordinate frame $A$
${}_C{}^D\mathit{x}$	$\mathit{x}$ represents a general viable vector, $C$ and $D$ are the robot indexes, e.g., ${}_C{}^D\mathit{p}$ denotes the linear translation of the $D$-th robot measured with respect to the $C$-th robot, particularly, ${}_C\mathit{x}={}_C{}^C\mathit{x}$.
$⌊\mathit{x}\times ⌋$	Skew-symmetric matrix of $\mathit{x}$, and $⌊\mathit{x}\times ⌋\mathit{y}=-\mathit{x}⌊\mathit{y}\times ⌋$ [28] holds
$\mathit{p}$	The linear translation vector of rigid bodies along 3 axes, of which the quasi-quaternion description is $\overline{\mathit{p}}={[0,{\mathit{p}}^T]}^T$
$\overline{\mathit{q}}$	The unit quaternion following the Hamilton notation [21], written as $\overline{\mathit{q}}={[q_0,q_1,q_2,q_3]}^T={[q_0,{\mathit{q}}^T]}^T$
${\overline{\mathit{q}}}^{*}$	The conjugate form of $\overline{\mathit{q}}$, and ${\overline{\mathit{q}}}^{\ast }\otimes \overline{\mathit{q}}=1$ holds
$\mathit{b}$	The uncertain bias of the measurement result
$\mathit{R}$	The equivalent rotation matrix of the quaternion $\overline{\mathit{q}}$, e.g., ${\mathit{R}}_w^i=\mathit{R}\left({\overline{\mathit{q}}}_w^i\right)$
$\mathit{n}$	White Gaussian noise vector with zero mean and covariance ${\mathit{\sigma }}^2$
$\dot{\mathit{x}}$,$\widehat{\mathit{x}}$	The first-order time derivative form and the estimated form of the vector $\mathit{x}$, respectively
$\tilde{\mathit{x}}$	The error form of the vector $\mathit{x}$, which is defined as $\tilde{\mathit{x}}=\mathit{x}-\widehat{\mathit{x}}$
$\delta \overline{\mathit{q}}$	The error of the quaternion $\overline{\mathit{q}}$, and $\delta \overline{\mathit{q}}={\widehat{\overline{\mathit{q}}}}^{\ast }\otimes \overline{\mathit{q}}$ holds

.

The fixed index of the master robot is set as 0, and the indexes of the slave robots are numbered from 1 to $n$.${}_n\mathit{p}_c^i$ and ${}_n\overline{\mathit{q}}_c^i$ represent the calibrated relative position and attitude, respectively, between the IMU and the stereo-vision coordinate frames of the $n$-th robot. The $n$-th robot’s linear translation ${}_n\mathit{p}_w^c$ and the rotation ${}_n\overline{\mathit{q}}_w^c$ obtained from the black-box stereo vision are measured by referring to the world coordinate frame. ${}_n\mathit{p}_w^i$ and ${}_n\overline{\mathit{q}}_w^i$ are the linear translation and the rotation of the IMU inspected in the world coordinate frame, respectively. The coupled position and attitude fusion results of each IMU–camera suite, i.e., ${}_n\mathit{p}_w^{ic}$ and ${}_n\overline{\mathit{q}}_w^{ic}$, are attached to its IMU coordinate frame to simplify the filtering process. The raw measurement outputs of the $n$-th IMU are the linear acceleration ${}_n\mathit{a}_{mi}$ and angular velocity ${}_n\mathit{\omega }_{mi}$, which are measured in its body-following coordinate frame. The relative configuration consists of the relative position and attitude between the $n$-th slave robot and the master robot, which are ${}_0{}^n\mathit{p}_r$ and ${}_0{}^n\overline{\mathit{q}}_r$, respectively. The units used in this article are described in the SI. This design of the master/slave relationship among multiple agents allows for convenient modification of the topology under practical situations.

In this study, the drift of the vision’s world coordinate frame is not considered since it is slow enough to be treated as noise. Furthermore, all noise variables are modeled as a random walk·. The small-angle assumption is applied, which means letting the rotation angle be converted from $\overline{\mathit{q}}$ be $\mathit{\theta }$, and the error of $\overline{\mathit{q}}$ is written as $\delta \overline{\mathit{q}}={\left[q_0,\delta {\mathit{q}}^T\right]}^T\approx {\left[1,\frac{1}{2}\delta {\mathit{\theta }}^T\right]}^T$ when $\mathit{\theta }\ll 1^{\circ }$ [28].

2.2. Construction of the SR-CICEKF State Vector

From this point onwards, for the filter construction considering the IMU-aided stereo-vision suite of any single robot, the left corner markers are omitted for simplification, such as ${}_0\mathit{p}_w^i={\mathit{p}}_w^i$.

Thus, the vector with 29 elements representing the state of the SR-CICEKF is defined as follows:

$$\mathit{X}={\left\{{\mathit{p}}_w^{icT} {\mathit{v}}_w^{cT} {\mathit{v}}_w^{iT} {\overline{\mathit{q}}}_w^{icT} {\overline{\mathit{q}}}_w^{iT} {\mathit{\omega }}_c^{cT} {\mathit{\omega }}_i^{iT} \mathit{b}_{ai}^T \mathit{b}_{\omega i}^T\right\}}^T,$$

(1)

where ${\mathit{v}}_w^c$ is the derivative of ${\mathit{p}}_w^c$; ${\mathit{v}}_w^i$ is integrated from ${\mathit{a}}_{mi}$, which possesses a bias ${\mathit{b}}_{ai}$; ${\mathit{\omega }}_c^c$ is the angular velocity of the stereo vision body, which is inspected in the IMU coordinate frame rather than the vision coordinate frame; and ${\mathit{\omega }}_i^i$ is the angular velocity of the IMU body in its own coordinate frame with a bias ${\mathit{b}}_{\omega i}$. Additionally, there are ${\dot{\mathit{v}}}_w^i={\mathit{a}}_w^i-\mathit{g}={\mathit{R}}_w^i\left({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\right)-\mathit{g}$ and ${\mathit{\omega }}_i^i={\mathit{\omega }}_{mi}-{\mathit{b}}_{\omega i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}}$, where ${\mathit{n}}_{{\mathit{a}}_{mi}}$ and ${\mathit{n}}_{{\mathit{\omega }}_{mi}}$ are the measurement noises, and $\mathit{g}$ is the gravity vector with respect to the world coordinate frame.

2.2.1. Coupling Process for the SR-CICEKF

The independent coupling coefficients of the linear and angular velocities are ${\mu }_v\in [0,1]$ and ${\mu }_{\omega }\in [0,1]$, respectively, ensuring adjustability while enhancing motion estimation accuracy without affecting observability.

The linear velocity coupling is straightforward and written as follows:

$${\mathit{v}}_w^{ic}={\mu }_v{\mathit{v}}_w^i+(1-{\mu }_v){\mathit{R}}_c^i{\mathit{v}}_w^c.$$

(2)

The coupling process of angular velocities cannot be conducted directly since the rotations of the IMU and vision body are inspected in different coordinate frames. Assuming ${\overline{\mathit{q}}}_w^{ic}={\overline{\mathit{q}}}_w^{c^{\prime }}{\overline{\mathit{q}}}_w^i$, the following equations are obtained referring to [29]:

$$\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}=\frac{1}{2}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes \left({\mu }_{\omega }{\overline{\mathit{\omega }}}_i^i\right),$$

(3)

$$\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}=\frac{1}{2}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}\otimes \left(\left(1-{\mu }_{\omega }\right){\overline{\mathit{\omega }}}_c^c\right),$$

(4)

where ${\overline{\mathit{q}}}_w^{c^{\prime }}$ is achieved by inspecting the rotation of the stereo vision body in the IMU coordinate frame, and ${\overline{\mathit{q}}}^{\mu }$ is the $\mu$-th power of $\overline{\mathit{q}}$, which denotes a unit quaternion scaling the rotation angle around the virtual axis with $\mu \in \left[0,1\right]$ [30].

According to Appendix A, Equations (3) and (4), ${\overline{\mathit{q}}}_w^{ic}$ can thus be derived as follows:

$$\begin{array}{l}{\dot{\overline{\mathit{q}}}}_w^{ic}=\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes {\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}+{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes \frac{d}{dt}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}\\ =\frac{{\mu }_{\omega }}{2}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes {\overline{\mathit{\omega }}}_i^i\otimes {\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}+\frac{1-{\mu }_{\omega }}{2}{\overline{\mathit{q}}}_w^{ic}\otimes {\overline{\mathit{\omega }}}_c^c\end{array},$$

(5)

where $\overline{\mathit{\omega }}=[0,\mathit{\omega }]$.

2.2.2. Simplification of the SR-CICEKF State Vector

To simplify the further discussion, the SR-CICEKF state vector in Equation (1) is rewritten as follows:

$$\mathit{X}={\left\{{\mathit{p}}_{ic}{}^T {\mathit{v}}_c{}^T {\mathit{v}}_i{}^T {\overline{\mathit{q}}}_{ic}{}^T {\overline{\mathit{q}}}_i{}^T\mathit{\omega }_c{}^T\mathit{\omega }_i{}^T {\mathit{b}}_a^T\mathit{b}_{\omega }^T\right\}}^T.$$

(6)

The corresponding derivatives are as follows:

$$\begin{gathered} {\dot{\mathit{p}}}_{ic}={\mathit{v}}_{ic}={\mu }_v{\mathit{v}}_i+(1-{\mu }_v){\mathit{R}}_c^i{\mathit{v}}_c, {\dot{\mathit{v}}}_c={\mathit{n}}_{{\mathit{v}}_c}, {\dot{\mathit{v}}}_i={\mathit{a}}_i-\mathit{g}={\mathit{R}}_i\left({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\right)-\mathit{g}, \\ {\dot{\overline{\mathit{q}}}}_{ic}=\frac{{\mu }_{\omega }}{2}{\overline{\mathit{q}}}_i{}^{{\mu }_{\omega }}\otimes {\overline{\mathit{\omega }}}_i\otimes {\overline{\mathit{q}}}_c{}^{1-{\mu }_{\omega }}+\frac{1-{\mu }_{\omega }}{2}{\overline{\mathit{q}}}_{ic}\otimes {\overline{\mathit{\omega }}}_c, {\dot{\overline{\mathit{q}}}}_i=\frac{1}{2}{\overline{\mathit{q}}}_i\otimes {\overline{\mathit{\omega }}}_i, {\dot{\mathit{\omega }}}_c={\mathit{n}}_{{\omega }_c}, \\ {\dot{\mathit{\omega }}}_i=\frac{d}{dt}\left({\mathit{\omega }}_{mi}-{\mathit{b}}_{{\mathit{\omega }}_i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}}\right), {\dot{\mathit{b}}}_{ai}={\mathit{n}}_{{\mathit{b}}_{ai}}, {\dot{\mathit{b}}}_{\omega i}={\mathit{n}}_{{\mathit{b}}_{\omega i}}, \end{gathered}$$

(7)

where ${\overline{\mathit{q}}}_c={\overline{\mathit{q}}}_w^{c^{\prime }}{\overline{\mathit{q}}}_c^i\otimes {\overline{\mathit{q}}}_w^c$.

2.2.3. Error of the SR-CICEKF State Vector

The error state contributes to the propagation matrices and is essential for the filtering process. $\tilde{\mathit{X}}$ contains 27 elements, written as follows:

$$\tilde{\mathit{X}}={\left\{\Delta {\mathit{p}}_{ic}{}^T \Delta {\mathit{v}}_c{}^T \Delta {\mathit{v}}_i{}^T \delta {\mathit{\theta }}_{ic}{}^T \delta {\theta }_i{}^T \Delta {\mathit{\omega }}_c{}^T \Delta {\mathit{\omega }}_i{}^T \Delta {\mathit{b}}_{ai}{}^T\Delta {{\mathit{b}}_{\omega i}}^T\right\}}^T.$$

(8)

The update rate of this filter-based approach is determined by the IMU refresh rate. Thus, small high-order terms in the process can be omitted, such as $\delta \mathit{q}\cdot \Delta \mathit{\omega }$, $\delta \mathit{q}\cdot \delta \mathit{q}$, and $\delta \mathit{q}\cdot \mathit{n}$ since the IMU can usually run faster than 100 Hz.

To build the connection from the state vector to the propagation process, the derivative form of $\tilde{\mathit{X}}$ should be inspected.

The derivative of the coupled translation error can be obtained directly as follows:

$$\stackrel{.}{\Delta {\mathit{p}}_{ic}}={\mu }_v\Delta {\mathit{v}}_i+(1-{\mu }_v){\mathit{R}}_c^i\Delta {\mathit{v}}_c.$$

(9)

Let ${\widehat{\mathit{a}}}_i={\mathit{a}}_{mi}-{\widehat{\mathit{b}}}_{ai}$ and $\Delta {\mathit{b}}_{ai}={\mathit{b}}_{ai}-{\widehat{\mathit{b}}}_{ai}$, and according to ${\mathit{R}}_i=\mathit{R}\left({\overline{\mathit{q}}}_i\right)\approx {\widehat{\mathit{R}}}_i\left({\mathit{I}}_3+⌊\delta {\mathit{\theta }}_i\times ⌋\right)$ achieved by following Appendix A, the equations are as follows:

$$\begin{array}{l}\stackrel{.}{\Delta {\mathit{v}}_i}={\mathit{a}}_i-{\widehat{\mathit{a}}}_i={\mathit{R}}_i({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}})-\mathit{g}-{\widehat{\mathit{R}}}_i({\mathit{a}}_{mi}-{\widehat{\mathit{b}}}_{ai})+\mathit{g}\\ \approx -{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋\delta {\mathit{\theta }}_i-{\widehat{\mathit{R}}}_i\Delta {\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\end{array},$$

(10)

where the higher-order terms are omitted.

According to Appendix A, the equations are as follows:

$$\left\{\begin{array}{l}{\overline{\mathit{q}}}_{ic}^{*}={\left({\overline{\mathit{q}}}_c\right)}^{1-{{\mu }_{\omega }}^{*}}\otimes {\left({\overline{\mathit{q}}}_i\right)}^{{{\mu }_{\omega }}^{*}}\\ {\left({{\overline{\mathit{q}}}_i}^{{\mu }_{\omega }}\right)}^{*}\otimes {\overline{\mathit{q}}}_i^{{\mu }_{\omega }}=1\\ {\left({\widehat{\overline{\mathit{q}}}}_c^{1-{\mu }_{\omega }}\right)}^{*}\otimes {\overline{\mathit{p}}}_{ic}\otimes {\widehat{\overline{\mathit{q}}}}_c{}^{1-{\mu }_{\omega }}\stackrel{\wedge}{=}\widehat{\mathit{R}}{\left({\widehat{\overline{\mathit{q}}}}_c^{1-{\mu }_{\omega }}\right)}^T{\mathit{p}}_{ic}={\widehat{\mathit{R}}}_{{\mu }_{\omega }c}{}^T\mathit{p}_{ic}\end{array}\right..$$

(11)

By subjecting Equation (11) to Equation (5), there is obtained as follows:

$$\stackrel{.}{\delta {\overline{\mathit{q}}}_{ic}}\approx -\frac{{\mu }_{\omega }}{2}\left\{\left[\begin{array}{l}0\\ 2⌊\left({\mathit{R}}_{{\mu }_{\omega }c}{}^T\widehat{\mathit{\omega }}_i\right)\times ⌋\delta {\mathit{q}}_{ic}\end{array}\right]-\left[\begin{array}{l}0\\ q_0{\mathit{R}}_{{\mu }_{\omega }c}{}^T\Delta {\mathit{\omega }}_i\end{array}\right]\right\}-\frac{1-{\mu }_{\omega }}{2}\left\{\left[\begin{array}{l}0\\ 2⌊{\widehat{\mathit{\omega }}}_c\times ⌋\delta {\mathit{q}}_{ic}\end{array}\right]-\left[\begin{array}{l}0\\ q_0\Delta {\mathit{\omega }}_c\end{array}\right]\right\},$$

(12)

where $\Delta \overline{\mathit{\omega }}=\overline{\mathit{\omega }}-\widehat{\overline{\mathit{\omega }}}$ is applied, and the high-order terms are disregarded.

Following the small-angle assumption, let $\delta {\overline{\mathit{q}}}_{ic}={\left[q_0,\delta {{\mathit{q}}_{ic}}^T\right]}^T\approx {\left[1,\frac{1}{2}\delta {\mathit{\theta }}_{ic}^T\right]}^T$, and there is as follows:

$$\stackrel{.}{\delta {\mathit{\theta }}_{ic}}=-{\mu }_{\omega }\left[⌊\left({\mathit{R}}_{{\mu }_{\omega }c}{}^T\widehat{\mathit{\omega }}_i\right)\times ⌋\delta {\mathit{\theta }}_{ic}-{\mathit{R}}_{{\mu }_{\omega }c}{}^T\Delta {\mathit{\omega }}_i\right]-\left(1-{\mu }_{\omega }\right)\left[⌊{\widehat{\mathit{\omega }}}_c\times ⌋\delta {\mathit{\theta }}_{ic}-\Delta {\mathit{\omega }}_c\right].$$

(13)

Similarly, there is as follows:

$$\stackrel{.}{\delta {\overline{\mathit{q}}}_i}\approx \left[\begin{array}{l}0\\ -⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{q}}_i\end{array}\right]-\left[\begin{array}{l}0\\ \frac{1}{2}{q}_{i0}\Delta {\mathit{\omega }}_i\end{array}\right],$$

(14)

where ${\widehat{\mathit{\omega }}}_i={\mathit{\omega }}_{mi}-{\widehat{\mathit{b}}}_{\omega i}$ and $\Delta {\mathit{b}}_{\omega i}={\mathit{b}}_{\omega i}-{\widehat{\mathit{b}}}_{\omega i}$ are applied.

Following the small-angle assumption, let $\delta {\overline{\mathit{q}}}_i={\left[q_0,\delta {{\mathit{q}}_i}^T\right]}^T\approx {\left[1,\frac{1}{2}\delta {\mathit{\theta }}_i^T\right]}^T$, and there is as follows:

$$\stackrel{.}{\delta {\mathit{\theta }}_i}=-⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{\theta }}_i-\Delta {\mathit{\omega }}_i=-⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{\theta }}_i-\Delta {\mathit{b}}_{\omega i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}}.$$

(15)

For other terms in the error state, there is no change compared to the state vector since they are obtained directly from the measurement. Therefore, there is as follows:

$${\stackrel{.}{\Delta \mathit{v}}}_c={\mathit{n}}_{{\mathit{v}}_c},\stackrel{.}{\Delta {\mathit{\omega }}_c}={\mathit{n}}_{{\mathit{\omega }}_c},\stackrel{.}{\Delta {\mathit{\omega }}_i}=\frac{d}{dt}\left(\Delta {\mathit{b}}_{\omega i}+{\mathit{n}}_{{\mathit{\omega }}_{mi}}\right)={\mathit{n}}_{\omega i},\stackrel{.}{\Delta {\mathit{b}}_{ai}}={\mathit{n}}_{{\mathit{b}}_{ai}},\stackrel{.}{\Delta {\mathit{b}}_{\omega i}=}{\mathit{n}}_{{\mathit{b}}_{\omega i}}.$$

(16)

2.3. Construction of the MR-CICEKF State Vector

One primary purpose of this study is to achieve a relative configuration between two robots. Therefore, the IMU coordinate frame, which is inherent and fixed after calibration, is chosen as the anchorage to establish the filter. For the conditions with multiple robots running simultaneously, the relative configuration chain can be easily developed with a convenient adjustment to the topology.

The construction process of the MR-CICEKF state equations is partially similar to SR-CICEKF. The input of the MR-CICEKF is obtained from the output of SR-CICEKF, and the state vector with 19 elements is written as follows:

$$\mathit{Y}={\left\{{}_0\mathit{p}_r{}^T _0{\mathit{v}}_w^i{}^T _1{\mathit{v}}_w^i{}^T _0{\overline{\mathit{q}}}_r{}^T _0{\mathit{\omega }}_i^i{}^T _1{\mathit{\omega }}_i^{iT}\right\}}^T,$$

(17)

where ${}_0\mathit{p}_r^T$ and ${}_0\overline{\mathit{q}}_r^T$ have been described previously, ${}_0\mathit{v}_w^i$ and ${}_0\mathit{\omega }_i^i$ are the IMU’s linear velocity and angular velocity obtained after applying the SR-CICEKF to the master robot’s sensor suite, respectively; and ${}_1\mathit{v}_w^i$ and ${}_1\mathit{\omega }_i^i$ are the outputs of the slave robot.

The derivative form of $\mathit{Y}$ connects the variables in the state vector. The derivative of the relative translation is obtained as follows:

$$\begin{array}{l}{}_1\mathit{p}_w^i{-}_0{\mathit{p}}_w^i=\mathit{R}\left({}_0\overline{\mathit{q}}_r\right)\cdot {}_0\mathit{p}_r\iff \\ \frac{d}{dt}\left({}_1\mathit{p}_w^i{-}_0{\mathit{p}}_w^i\right)={\mathit{R}}_r\cdot ⌊{}_0\mathit{\omega }_i^i\times ⌋\cdot {}_0\mathit{p}{}_r+{\mathit{R}}_r\cdot {}_0\dot{\mathit{p}}_r\\ \iff {}_0\dot{\mathit{p}}_r={}_r\mathit{v}_w^i-⌊{}_0\mathit{\omega }_i^i\times ⌋\cdot {}_0\mathit{p}_r\end{array},$$

(18)

where ${}_r\mathit{v}_w^i={\mathit{R}}_r^T\cdot \left({}_1\mathit{v}_w^i-{}_0\mathit{v}_w^i\right)$ is the relative linear velocity of the robots.

According to ${}_0\overline{\mathit{q}}_r{=}_0{\overline{\mathit{q}}}_w^{i*}\otimes _1{\overline{\mathit{q}}}_w^i$, the derivative of the relative rotation is written as follows:

$$\begin{array}{l}{}_0\dot{\overline{\mathit{q}}}_r=\frac{d}{dt}\left({}_0\overline{\mathit{q}}_w^{i*}\right){\otimes }_1{\overline{\mathit{q}}}_w^i{+}_0{\overline{\mathit{q}}}_w^{i*}\otimes \frac{d}{dt}\left({}_1\overline{\mathit{q}}_w^i\right)\\ =\frac{1}{2}\left({}_0\overline{\mathit{q}}_r{\otimes }_1{\mathit{\omega }}_i^i{-}_0{\mathit{\omega }}_i^i{\otimes }_0{\overline{\mathit{q}}}_r\right)\end{array}.$$

(19)

2.3.1. Simplification of the MR-CICEKF State Vector

To simplify the further discussion, the MR-CICEKF state vector is rewritten as follows:

$$\mathit{Y}={\left\{{\mathit{p}}_r{}^T {\mathit{v}}_{i0}{}^T {\mathit{v}}_{i1}{}^T {\overline{\mathit{q}}}_r{}^T {\mathit{\omega }}_{i0}{}^T {{\mathit{\omega }}_{i1}}^T\right\}}^T.$$

(20)

Additionally, there is ${}_r\mathit{v}_w^i={\mathit{v}}_r$. The derivatives of the relative motion variables are written as follows:

$$\begin{gathered} {\dot{\mathit{p}}}_r={\mathit{v}}_r={\mathit{R}}_r^T\cdot \left({\mathit{v}}_{i1}-{\mathit{v}}_{i0}\right)-⌊{\mathit{\omega }}_{i0}\times ⌋\cdot {\mathit{p}}_r, {\dot{\overline{\mathit{q}}}}_r=\frac{1}{2}\left({\overline{\mathit{q}}}_r\otimes {\mathit{\omega }}_{i1}-{\mathit{\omega }}_{i0}\otimes {\overline{\mathit{q}}}_r\right), {\dot{\mathit{v}}}_{i0}={\mathit{n}}_{{\mathit{v}}_{i0}}, \\ {\dot{\mathit{v}}}_{i1}={\mathit{n}}_{{\mathit{v}}_{i1}}, {\dot{\mathit{\omega }}}_{i0}={\mathit{n}}_{{\mathit{\omega }}_{i0}}, {\dot{\mathit{\omega }}}_{i1}={\mathit{n}}_{{\mathit{\omega }}_{i1}}. \end{gathered}$$

(21)

2.3.2. Error of the MR-CICEKF State Vector

$\tilde{\mathit{Y}}$ contains 18 elements and is written as follows:

$$\tilde{\mathit{Y}}={\left\{\Delta {\mathit{p}}_r{}^T \Delta {\mathit{v}}_{i0}{}^T \Delta {\mathit{v}}_{i1}{}^T \delta {\mathit{\theta }}_r{}^T \Delta {\mathit{\omega }}_{i0}{}^T \Delta {{\mathit{\omega }}_{i1}}^T\right\}}^T.$$

(22)

Following Appendix A, ${\mathit{R}}_r^T=\left({\mathit{I}}_3-⌊\delta {\mathit{\theta }}_r\times ⌋\right){{\widehat{\mathit{R}}}_r}^T$ holds. According to ${\mathit{p}}_r={\widehat{\mathit{p}}}_r+\Delta {\mathit{p}}_r$, by omitting high-order terms, there is as follows:

$$\begin{array}{l}\Delta {\dot{\mathit{p}}}_r=\left({\mathit{R}}_r{}^T\left({\mathit{v}}_{i1}-{\mathit{v}}_{i0}\right)-⌊{\mathit{\omega }}_{i0}\times ⌋{\mathit{p}}_r\right)-\left({\widehat{\mathit{R}}}_r{}^T\left({\widehat{\mathit{v}}}_{i1}-{\widehat{\mathit{v}}}_{i0}\right)-⌊{\widehat{\mathit{\omega }}}_{i0}\times ⌋{\widehat{\mathit{p}}}_r\right)\\ \approx {\widehat{\mathit{R}}}_r{}^T\Delta {\mathit{v}}_{i1}-{\widehat{\mathit{R}}}_r{}^T\Delta {\mathit{v}}_{i0}+⌊\left({\widehat{\mathit{R}}}_r{}^T\left({\mathit{v}}_{i1}-{\mathit{v}}_{i0}\right)\right)\times ⌋\delta {\mathit{\theta }}_r+⌊{\widehat{\mathit{p}}}_r\times ⌋\Delta {\mathit{\omega }}_{i0}-⌊{\widehat{\mathit{\omega }}}_{i0}\times ⌋\Delta {\mathit{p}}_r\end{array},$$

(23)

where ${\mathit{\omega }}_{i0}={\widehat{\mathit{\omega }}}_{i0}+\Delta {\mathit{\omega }}_{i0}$, ${\mathit{v}}_{i0}={\widehat{\mathit{v}}}_{i0}+\Delta {\mathit{v}}_{i0}$ and ${\mathit{v}}_{i1}={\widehat{\mathit{v}}}_{i1}+\Delta {\mathit{v}}_{i1}$ are applied.

Resembling the deduction process of Equations (11) and (12), the error of the relative rotation can be obtained as follows:

$$\delta {\dot{\overline{\mathit{q}}}}_r=\left[\begin{array}{l}0\\ -⌊{\widehat{\mathit{\omega }}}_{i1}\times ⌋\delta {\mathit{q}}_r\end{array}\right]+\left[\begin{array}{l}0\\ \frac{1}{2}\Delta {\mathit{\omega }}_{i1}\end{array}\right]-\left[\begin{array}{l}0\\ \frac{1}{2}{\widehat{\mathit{R}}}_r{}^T\Delta {\mathit{\omega }}_{i0}\end{array}\right],$$

(24)

After applying the small-angle assumption, the result is as follows:

$$\delta {\dot{\mathit{\theta }}}_r=-⌊{\widehat{\mathit{\omega }}}_{i1}\times ⌋\delta {\mathit{\theta }}_r+\Delta {\mathit{\omega }}_{i1}-{\widehat{\mathit{R}}}_r{}^T\Delta {\mathit{\omega }}_{i0},$$

(25)

Since the linear and angular velocities are generated from the SR-CICEKF process, which can be seen as a measurement process, there is as follows:

$$\Delta {\mathit{v}}_{i0}={\mathit{n}}_{{\mathit{v}}_{i0}},\Delta {\mathit{v}}_{i1}={\mathit{n}}_{{\mathit{v}}_{i1}},\Delta {\mathit{\omega }}_{i0}={\mathit{n}}_{{\mathit{\omega }}_{i0}},\Delta {\mathit{\omega }}_{i1}={\mathit{n}}_{{\mathit{\omega }}_{i1}}.$$

(26)

2.4. Relationships Among the Variables in the CICEKF

By analyzing the construction of the state vectors, it can be concluded that the SR-CICEKF and the MR-CICEKF are tightly connected in the inheritance of variables and the synchronous update, which are assembled as an entire CICEKF process. The relationships among the variables in the CICEKF states are shown in Figure 2. The whole filter is divided into three parts: data preparation, filter process, and data output. The data preparation contains the data input and fusion for both the master and slave robots as individuals. The filter process is separated into two steps, which are SR-CICEKF and MR-CICEKF.

3. Propagation and Update of the CICEKF

The propagation process is key to connecting different layers of the filter. The update process introduces measurement data to correct the entire data fusion process.

3.1. Propagation and Measurement of the SR-CICEKF

3.1.1. Propagation of the SR-CICEKF

Let the continuous state transition matrix of the SR-CICEKF be ${\mathit{F}}_c$ and the continuous noise transition matrix be ${\mathit{G}}_c$, which are constant over each integration time step. By linearizing the continuous-time errors of a CICEKF state, there is as follows [31]:

$$\dot{\tilde{\mathit{X}}}={\mathit{F}}_c\tilde{\mathit{X}}+{\mathit{G}}_c{\mathit{n}}_X,$$

(27)

where the state noise vector is ${\mathit{n}}_X={[{\mathit{n}}_{{\mathit{\omega }}_c}^T {\mathit{n}}_{{\mathit{a}}_{mi}}^T {\mathit{n}}_{{\mathit{\omega }}_c}^T {\mathit{n}}_{{\mathit{\omega }}_{mi}}^T {\mathit{n}}_{{\mathit{b}}_{ai}}^T {\mathit{n}}_{{\mathit{b}}_{\omega i}}^T]}^T$.

By applying Taylor’s formula, the discrete form of Equation (27) is as follows [31]:

$$\left\{\begin{array}{l}{\mathit{F}}_d=\exp ({\mathit{F}}_c\Delta t)={\mathbf{I}}_d+{\mathit{F}}_c\Delta t+\frac{1}{2!}{\mathit{F}}_c^2\Delta t^2+\dots \\ {\mathit{Q}}_d={\displaystyle {\int }_t^{t+\Delta t}{\mathit{F}}_d(\tau ){\mathit{G}}_c{\mathit{Q}}_c{\mathit{G}}_c^T{\mathit{F}}_d{(\tau )}^{T}d\tau }\end{array}\right.,$$

(28)

where ${\mathit{F}}_d$ is the discrete form of ${\mathit{F}}_c$, and the continuous noise covariance matrix ${\mathit{Q}}_c=diag({\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{v}}_c}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{a}}_i}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{\omega }}_c}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{\omega }}_i}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{b}}_{ai}}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{b}}_{\omega \mathrm{i}}}}^2)$ is a diagonal matrix with ${\mathit{Q}}_d$ being its discrete form.

By considering the first-order expansion, after computing the Jacobians $\frac{\partial \dot{\tilde{\mathit{X}}}}{\partial \tilde{\mathit{X}}}$ to obtain ${\mathit{F}}_c$, the expression of ${\mathit{F}}_d$ can be written as follows:

$${\mathit{F}}_d={\left[\begin{array}{llllllllll}{\mathit{I}}_3& (1-{\mu }_v){\mathit{R}}_c^i\Delta t& & & & & {\mu }_v{\mathit{I}}_3\Delta t& \dots & & {\mathbf{0}}_{3\times 18}\\ {\mathbf{0}}_3& {\mathit{I}}_3& & & & & \dots & & & {\mathbf{0}}_{3\times 21}\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3& {\mathbf{0}}_3& -{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋\Delta t& {\mathbf{0}}_3& {\mathbf{0}}_3& -{\widehat{\mathit{R}}}_i\Delta t& & {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& 0_3& {\mathit{I}}_3& \mathit{A}& 0_3& \left(1-{\mu }_{\omega }\right)\Delta t{\mathit{I}}_3& {\mu }_{\omega }{\mathit{R}}_{{\mu }_{\omega }c}{}^T\Delta t& 0_3& 0_3\\ 0_3& 0_3& 0_3& 0_3& {\mathit{I}}_3& -⌊{\widehat{\mathit{\omega }}}_i\times ⌋\Delta t& 0_3& 0_3& 0_3& -\Delta t{\mathit{I}}_3\\ 0_{12\times 15}& & & & & \dots & & & & {\mathit{I}}_{12\times 12}\end{array}\right]}_{27*27},$$

(29)

where $\mathit{A}={\mathit{I}}_3-\left({\mu }_{\omega }⌊\left({\mathit{R}}_{{\mu }_{\omega }c}{}^T\widehat{\mathit{\omega }}_i\right)\times ⌋+\left(1-{\mu }_{\mathit{\omega }}\right)⌊{\widehat{\omega }}_c\times ⌋\right)\Delta t$.

Following Equations (28) and (29), ${\mathit{Q}}_d$ can be obtained from ${\mathit{G}}_c$, which is derived according to $\frac{\partial \dot{\tilde{\mathit{X}}}}{\partial {\mathit{n}}_X}$ and written as follows:

$${\mathit{G}}_c={\left[\begin{array}{cccccc}& & 0_{3\times 18}& & & \\ {\mathit{I}}_3& 0_3& 0_3& 0_3& 0_3& 0_3\\ 0_3& -{\widehat{\mathit{R}}}_i& 0_3& 0_3& 0_3& 0_3\\ & & 0_{3\times 18}& & & \\ 0_3& 0_3& 0_3& -{\mathit{I}}_3& 0_3{0}_3\\ 0_3& 0_3& {\mathit{I}}_3& 0_3& 0_3& 0_3\\ 0_3& 0_3{0}_3& 0_3& {\mathit{I}}_3& 0_3\\ & & 0_{6\times 18}& & & \end{array}\right]}_{27*18}.$$

(30)

3.1.2. Measurement of the SR-CICEKF

For an autonomous robot, the keyframes of the visual measurement can possess high localization accuracy for its post-batch optimization with visual features. Let the measurement vector of the SR-CICEKF at the $k$-th step be ${\mathit{z}}_k$ with its error form as ${\tilde{\mathit{z}}}_k={\left[{\tilde{\mathit{z}}}_p^T\widetilde{\overline{\mathit{z}}}_q^T\tilde{\mathit{z}}_v^T\tilde{\mathit{z}}_{\omega }^T\right]}^T$, where ${\tilde{\mathit{z}}}_P$ is the error of the linear position, the quaternion ${\widetilde{\overline{\mathit{z}}}}_q$ is the error of the rotation, ${\tilde{\mathit{z}}}_v$ is the error of the linear velocity, and ${\tilde{\mathit{z}}}_{\omega }$ is the error of the angular velocity. The measurement noise of the variables is simplified as ${\mathit{n}}_m={\left[\mathit{n}_p^T\mathit{n}_{\theta }^T\mathit{n}_v^T\mathit{n}_{\omega }^T\right]}^T$.

Following $\tilde{\mathit{x}}=\mathit{x}-\widehat{\mathit{x}}$ and $\delta \overline{\mathit{q}}={\widehat{\overline{\mathit{q}}}}^{\ast }\otimes \overline{\mathit{q}}$, the entire description of ${\tilde{\mathit{z}}}_P$,${\widetilde{\overline{\mathit{z}}}}_q$, and ${\tilde{\mathit{z}}}_v$ based on variables in the error state vector of the SR-CICEKF can be deducted as follows:

$$\left\{\begin{array}{l}{\tilde{\mathit{z}}}_p=\Delta \mathit{p}+{\widehat{\mathit{R}}}_{ic}⌊\left({\mathit{p}}_i^c\right)\times ⌋\delta {\mathit{\theta }}_{ic}+{\mathit{n}}_p\\ {\widetilde{\overline{\mathit{z}}}}_{\mathit{q}}={\widehat{\overline{\mathit{q}}}}_{ic}{}^{*}\otimes \left({\overline{\mathit{q}}}_c^i\otimes {\overline{\mathit{q}}}_c\right)=\delta {\overline{\mathit{q}}}_{ic}\approx \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathit{\theta }}_{ic}+{\mathit{n}}_{\theta }\end{array}\right]\\ {\tilde{\mathit{z}}}_v=-{\mu }_v\Delta t{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋\delta {\mathit{\theta }}_i-{\mu }_v\Delta t{\widehat{\mathit{R}}}_i\Delta {\mathit{b}}_a+(1-{\mu }_v){\mathit{R}}_c^i\Delta {\mathit{v}}_c+{\mathit{n}}_v\end{array}\right..$$

(31)

In particular, the angular velocities of both sensors cannot be directly subtracted since they do not share the same coordinate frame. Therefore, ${\tilde{\mathit{z}}}_{\omega }$ is obtained indirectly from Equation (15) as follows:

$$\begin{array}{l}{\tilde{\mathit{z}}}_{\omega }=\stackrel{.}{\delta {\mathit{\theta }}_{ic}}\Delta t+{\mathit{n}}_{\omega }\\ =-\Delta t\left[{\mu }_{\omega }⌊\left({\mathit{R}}_{{\mu }_{\omega }c}{}^T\widehat{\mathit{\omega }}_i\right)\times ⌋+\left(1-{\mu }_{\omega }\right)⌊{\widehat{\mathit{\omega }}}_c\times ⌋\right]\delta {\mathit{\theta }}_{ic}+\left(1-{\mu }_{\omega }\right)\Delta t\Delta {\mathit{\omega }}_c+{\mu }_{\omega }\Delta t{\mathit{R}}_{{\mu }_{\omega }c}{}^T\Delta {\mathit{\omega }}_i+{\mathit{n}}_{\omega }\end{array}.$$

(32)

The measurement matrix ${\mathit{H}}_k$ can be recovered from ${\tilde{\mathit{z}}}_k\simeq {\mathit{H}}_k{\tilde{\mathit{X}}}_k+{\mathit{n}}_m$, which is written as follows:

$${\mathit{H}}_k={\left[\begin{array}{ccccccc}{\mathit{I}}_{3\times 3}& & 0_{3\times 6}& & {\widehat{\mathit{R}}}_{ic}⌊\left(p_i^c\right)\times ⌋& & 0_{3\times 15}\\ 0_{3\times 9}& & & \frac{1}{2}{\mathit{I}}_{3\times 3}& & & 0_{3\times 15}\\ 0_{3\times 3}& \Delta t(1-{\mu }_v){\mathit{R}}_c^i& 0_{3\times 6}& -{\mu }_v\Delta t{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋& 0_{3\times 6}& -{\mu }_v\Delta t{\widehat{\mathit{R}}}_i& 0_{3\times 3}\\ 0_{3\times 9}& \mathit{B}& 0_{3\times 3}& \left(1-{\mu }_{\omega }\right)\Delta t{\mathit{I}}_{3\times 3}& {\mu }_{\omega }\Delta t{{\mathit{R}}_{{\mu }_{\omega }c}}^T& & 0_{3\times 6}\end{array}\right]}_{12*27},$$

(33)

where $\mathit{B}=-\Delta t\left[{\mu }_{\omega }⌊\left({\mathit{R}}_{{\mu }_{\omega }c}{}^T\widehat{\mathit{\omega }}_i\right)\times ⌋+\left(1-{\mu }_{\omega }\right)⌊{\widehat{\mathit{\omega }}}_c\times ⌋\right]$.

3.2. Propagation and Measurement of the MR-CICEKF

The processes of the propagation and measurement of the MR-CICEKF are similar to those of the SR-CICEKF and focus on relative motion estimation.

Following the deduction of the propagation matrix from Equation (27) to (28), let the noise vector of the state variables of the MR-CICEKF be ${\mathit{n}}_r={\left[{{\mathit{n}}_{{\mathit{v}}_{i0}}}^T{{\mathit{n}}_{{\mathit{v}}_{i1}}}^T{{\mathit{n}}_{{\mathit{\omega }}_{i0}}}^T{{\mathit{n}}_{{\mathit{\omega }}_{i1}}}^T\right]}^T$. And the discrete relative state transition matrix is written as follows:

$${\mathit{F}}_d^r={\left[\begin{array}{llllll}\left({\mathit{I}}_3-⌊{\widehat{\mathit{\omega }}}_{i0}\times ⌋\right)& -{\widehat{\mathit{R}}}_r^T& \widehat{\mathit{R}}_r^T& ⌊\left({\widehat{\mathit{R}}}_r{}^T\left({\mathit{v}}_{i1}-{\mathit{v}}_{i0}\right)\right)\times ⌋& ⌊{\widehat{\mathit{p}}}_r\times ⌋& 0_3\\ 0_3& {\mathit{I}}_3& & \dots & & 0_{3\times 12}\\ 0_3& 0_3& {\mathit{I}}_3& \dots & & 0_{3\times 9}\\ 0_{3\times 9}& \dots & & {\mathit{I}}_3\left(-⌊{\widehat{\mathit{\omega }}}_{i1}\times ⌋\right)& -{\widehat{\mathit{R}}}_r^T& {\mathit{I}}_3\\ 0_{3\times 12}& \dots & & & {\mathit{I}}_3& 0_3\\ 0_{3\times 15}& \dots & & & & {\mathit{I}}_3\end{array}\right]}_{18\times 18}.$$

(34)

The discrete noise covariance matrix is ${\mathit{Q}}_d^r={\displaystyle {\int }_t^{t+\Delta t}{\mathit{F}}_d^r(\tau ){\mathit{G}}_c^r{\mathit{Q}}_c^r{\mathit{G}}_c^r{}^T\mathit{F}_d^r{(\tau )}^{T}d\tau }$, where there is as follows:

$${\mathit{G}}_c^r={\left[\begin{array}{llll}{0}_3& 0_3& { 0}_3& { 0}_3\\ {\mathit{I}}_3& 0_3& { 0}_3& { 0}_3\\ 0_3& {\mathit{I}}_3& { 0}_3& { 0}_3\\ 0_3& 0_3& { 0}_3& { 0}_3\\ 0_3& 0_3& {\mathit{I}}_3& { 0}_3\\ 0_3& 0_3& { 0}_3& {\mathit{I}}_3\end{array}\right]}_{18\times 12}.$$

(35)

By defining the measurement noise vector of the MR-CICEKF as ${\mathit{n}}_{rm}={\left[{\mathit{n}}_{rp}^T\mathit{n}_{r\theta }^T\right]}^T$, let the measurement error of the MR-CICEKF at the $k$-th step be ${\tilde{\mathit{z}}}_{rk}={\left[{{\tilde{\mathit{z}}}_{rp}}^T\tilde{\mathit{z}}_{rq}^T\right]}^T$, then, there is as follows:

$$\left\{\begin{array}{l}{\tilde{\mathit{z}}}_{rp}=\left({}_1\mathit{p}_c-{}_0\mathit{p}_c\right)-{\widehat{\mathit{p}}}_r\\ {\tilde{\mathit{z}}}_{rq}=\delta {\overline{\mathit{q}}}_r=\left({}_0\overline{\mathit{q}}_c^{*}\otimes {}_1\overline{\mathit{q}}_c\right)\otimes {\widehat{\overline{q}}}_r\approx \left[\begin{array}{c}1\\ \frac{1}{2}\cdot \delta {\mathit{\theta }}_r+{\mathit{n}}_{r\theta }\end{array}\right]\end{array}\right..$$

(36)

After inspecting the relationship between the measurement and the desired output of the filter through ${\tilde{\mathit{z}}}_k^r\simeq {\mathit{H}}_k^r{\tilde{\mathit{Y}}}_k+{\mathit{n}}_{rm}$, the measurement matrix ${\mathit{H}}_k^r$ can be deducted as follows:

$${\mathit{H}}_k^r={\left[\begin{array}{cccccc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \frac{1}{2}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}_{6\times 18}.$$

(37)

3.3. Entire CICEKF Process

From the analysis above, it can be concluded that the CICEKF has two major EKF-based loops to update and correct the relevant states. The first loop, i.e., the SR-CICEKF, is shown in Algorithm 1, and the second loop, i.e., the MR-CICEKF, is shown in Algorithm 2, where ${\mathit{R}}_k$ and ${\mathit{R}}_k^r$ are the measurement noise matrices pre-established as described in [32,33], ${\mathit{K}}_k$ and ${\mathit{K}}_k^r$ are the Kalman gains, ${\mathit{P}}_{k+1|k}$ and ${\mathit{P}}_{k+1|k}^r$ are the prior covariance matrices of error, and ${\mathit{P}}_{k+1|k+1}$ and ${\mathit{P}}_{k+1|k+1}^r$ are the posterior covariance matrices of the error.

The first loop governs the propagation, measurement, and update of a single robot’s motion state by fusing the measurement data of the stereo vision and the IMU on the velocity level. The second loop is focused on the estimation of the relative positions and rotations of multiple slave robots with respect to the master robot. This dual-loop design keeps the independence of either algorithm for application in different motion estimation situations. When the two algorithms work serially to yield ${\widehat{\mathit{X}}}_{k+1}$ and ${\widehat{\mathit{Y}}}_{k+1}$ successively, the entire algorithm complexity can be restricted to $\mathit{O}\left(n\right)$.

Algorithm 1: SR-CICEKF
	Input: ${\widetilde{\dot{\mathit{X}}}}_k$,${\widehat{\mathit{X}}}_k$,${\mathit{H}}_k$;
1	While true do
2	Update ${\mathit{F}}_d$,${\mathit{Q}}_d$ according to $\widetilde{\dot{\mathit{X}}}$ and ${\widehat{\mathit{X}}}_k$; ($\leftarrow$Propagation process in Section 3.1.1)
3	Update ${\mathit{P}}_{k+1|k}={\mathit{F}}_d{\mathit{P}}_{k|k}{\mathit{F}}_d+{\mathit{Q}}_d$;
4	Update ${\mathit{K}}_k={\mathit{P}}_{k+1|k}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_{k+1|k}{\mathit{H}}_k^T+{\mathit{R}}_k\right)}^{-1}$;
5	Calculate ${\tilde{\mathit{z}}}_k$ according to ${\mathit{H}}_k$; ($\leftarrow$Measurement process in Section 3.1.2)
6	Update the current state ${\widehat{\mathit{X}}}_{k+1|k+1}={\widehat{\mathit{X}}}_{k+1|k}+{\widehat{\tilde{\mathit{X}}}}_k$;
7	Update ${\mathit{P}}_{k+1|k+1}=({\mathit{I}}_{27\times 27}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k+1|k}{({\mathit{I}}_{27\times 27}-{\mathit{K}}_k{\mathit{H}}_k)}^T+{\mathit{K}}_k{\mathit{R}}_k{\mathit{K}}_k^T$;
8	$k=k+1$;
9	Output: ${\widehat{\mathit{X}}}_{k+1}$.

Algorithm 2: MR-CICEKF
	Input: ${\widetilde{\dot{\mathit{Y}}}}_k$,${\widehat{\mathit{Y}}}_k$,${\mathit{H}}_k^r$,${}_0\widehat{\mathit{X}}_k$,${}_1\widehat{\mathit{X}}_k$;
1	While true do
2	Update ${\widehat{\mathit{Y}}}_k$ according to ${}_0\widehat{\mathit{X}}_k$ and ${}_1\widehat{\mathit{X}}_k$;
3	Update ${\mathit{F}}_d^r$,${\mathit{Q}}_d^r$ according to ${\widetilde{\dot{\mathit{Y}}}}_k$ and ${\widehat{\mathit{Y}}}_k$; ($\leftarrow$Propagation process in Section 3.2)
4	Update ${\mathit{P}}_{k+1|k}^r={\mathit{F}}_d^r{\mathit{P}}_{k|k}^r{\mathit{F}}_d^r+{\mathit{Q}}_d^r$;
5	Update ${\mathit{K}}_k^r={\mathit{P}}_{k+1|k}^r{\mathit{H}}_k^r{}^T\left({\mathit{H}}_k^r{\mathit{P}}_{k+1|k}^r{\mathit{H}}_k^r{}^T+{\mathit{R}}_k^r\right)^{-1}$;
6	Calculate ${\tilde{\mathit{z}}}_{rk}$ according to ${\mathit{H}}_k^r$; ($\leftarrow$Measurement process in Section 3.2)
7	Update the current state ${\widehat{\mathit{Y}}}_{k+1|k+1}={\widehat{\mathit{Y}}}_{k+1|k}+{\widehat{\tilde{\mathit{Y}}}}_k$;
8	Update ${\mathit{P}}_{k+1|k+1}^r=({\mathit{I}}_{18\times 18}-{\mathit{K}}_k^r{\mathit{H}}_k^r){\mathit{P}}_{k+1|k}^r{({\mathit{I}}_{18\times 18}-{\mathit{K}}_k^r{\mathit{H}}_k^r)}^T+{\mathit{K}}_k^r{\mathit{R}}_k^r{\mathit{K}}_k^{rT}$;
9	$k=k+1$;
10	Output: ${\widehat{\mathit{Y}}}_{k+1}$.

When updating each current state, the quaternion error is recovered from the angular errors using the following equations [28]:

$$\left\{\begin{array}{l}\delta {\widehat{\mathit{q}}}_k\approx \frac{1}{2}\delta {\widehat{\mathit{\theta }}}_k\\ {\widehat{\overline{\mathit{q}}}}_{k+1}=\left\{\begin{array}{l}{\left[\sqrt{1-\delta {\widehat{\mathit{q}}}_k{}^T\delta {\widehat{\mathit{q}}}_k},\delta {{\widehat{\mathit{q}}}_k}^T\right]}^T \mathrm{if} \delta {\widehat{\mathit{q}}}_k{}^T\delta {\widehat{\mathit{q}}}_k\le 1 \\ \frac{1}{\sqrt{1+\delta {\widehat{\mathit{q}}}_k{}^T\delta {\widehat{\mathit{q}}}_k}}{\left[1,\delta {{\widehat{\mathit{q}}}_k}^T\right]}^T \mathrm{if} \delta {\widehat{q}}_k{}^T\delta {\widehat{\mathit{q}}}_k>1 \end{array}\right.\end{array}\right..$$

(38)

4. Nonlinear Observability Analysis

4.1. Observability Analysis of the SR-CICEKF

When treating the filter as a nonlinear multi-input and multi-output system, its prerequisite for the filter to achieve correct estimation is that the system is observable. Researchers have proven that a filter possessing local weak observability can function properly, which is equivalent to the constructed observability matrix having full rank [21,34]. To simplify the discussion, a virtual measured angle velocity ${\mathit{\omega }}_{mic}$ and its bias ${\mathit{b}}_{{\mathit{\omega }}_{ic}}$ are assumed and partially coupled based on the IMU and visual measurement. Due to ${\overline{\mathit{q}}}_{ic}$ being in the filter state, and thus treated as paired with ${\mathit{\omega }}_{mic}$, this assumption does not affect the observability analysis. Thus, following Appendix A, the nonlinear measurement process of the SR-CICEKF can be expressed as follows:

$$f\left(\mathit{X}\right)=\dot{\mathit{X}}={\mathit{f}}_0+{\mathit{f}}_1{\mathit{\omega }}_{mi}+{\mathit{f}}_2{\mathit{\omega }}_{mic}+{\mathit{f}}_3{\mathit{a}}_{mi},$$

(39)

where for a general unit quaternion $\overline{\mathit{q}}$, there is $\Xi \left(\overline{\mathit{q}}\right)={\left[-{\mathit{q}}^T{\left(q_0{\mathit{I}}_{3\times 3}-⌊\mathit{q}\times ⌋\right)}^T\right]}^T$ [21], and

$$\left\{\begin{array}{l}{\mathit{f}}_{\mathbf{0}}={\left[{\left({\mu }_v{\mathit{v}}_i+(1-{\mu }_v){\mathit{R}}_c^i{\mathit{v}}_c\right)}^T {\mathbf{0}}_{1\times 3} {\left(-{\mathit{R}}_i{\mathit{b}}_{a_i}-\mathit{g}\right)}^T{\left(\frac{1}{2}\Xi \left({\overline{\mathit{q}}}_{ic}\right){\mathit{b}}_{{\mathit{\omega }}_{ic}}\right)}^T {\left(\frac{1}{2}\Xi \left({\dot{\overline{\mathit{q}}}}_i\right){\mathit{b}}_{{\mathit{\omega }}_i}\right)}^T {\mathbf{0}}_{1\times 12}\right]}^{\mathrm{T}}\\ {\mathit{f}}_1={\left[{\mathbf{0}}_{3\times 13} {\left(\frac{1}{2}\Xi \left({\overline{\mathit{q}}}_i\right)\right)}^T {\mathbf{0}}_{3\times 12}\right]}^{\mathrm{T}}\\ {\mathit{f}}_2={\left[{\mathbf{0}}_{3\times 9} {\left(\frac{1}{2}\mathit{\Xi }\left({\overline{\mathit{q}}}_{ic}\right)\right)}^T{\mathbf{0}}_{3\times 16} \right]}^{\mathrm{T}}\\ {\mathit{f}}_3={\left[{\mathbf{0}}_{3\times 6} {\mathit{R}}_i{}^T\mathbf{0}_{3\times 20}\right]}^{\mathrm{T}}\end{array}\right..$$

(40)

The measurement functions for the SR-CICEKF are designed as $\mathit{h}\left(\dot{\mathit{X}}\right)={\left[{\mathit{h}}_1{}^T,\dots ,{{\mathit{h}}_8}^T\right]}^T$, with ${\mathit{h}}_1=({\mathit{p}}_{ic}-{\mathit{R}}_{ic}{\mathit{p}}_i^c)\lambda$, ${\mathit{h}}_2={\overline{\mathit{q}}}_{ic}$, ${\mathit{h}}_3={\overline{\mathit{q}}}_i$, ${\mathit{h}}_4={\overline{\mathit{q}}}_{ic}{}^T\overline{\mathit{q}}_{ic}$, ${\mathit{h}}_5={\overline{\mathit{q}}}_i{}^T\overline{\mathit{q}}_i$, ${\mathit{h}}_6={\mathit{v}}_c$, ${\mathit{h}}_7={\mathit{\omega }}_c$, and ${\mathit{h}}_8={\mathit{\omega }}_i$.

Following the Lie derivative rules in Appendix A and in [21], the observability matrix is as follows:

$$\mathbf{\Omega }=\left[\begin{array}{l}\nabla L^0{\mathit{h}}_1\\ \nabla L^0{\mathit{h}}_2\\ \nabla L^0{\mathit{h}}_3\\ \nabla L^0{\mathit{h}}_4\\ \nabla L^0{\mathit{h}}_5\\ \nabla L^0{\mathit{h}}_6\\ \nabla L^0{\mathit{h}}_7\\ \nabla L^0{\mathit{h}}_8\\ \nabla L_{f_0}^1{\mathit{h}}_1\\ \nabla L_{f_0}^1{\mathit{h}}_3\\ \nabla L_{f_0}^2{\mathit{h}}_1\end{array}\right]={\left[\begin{array}{ccccccccc}\stackrel{{\mathit{p}}_{ic}}{\overbrace{{\mathbf{I}}_{3\times 3}}}& \stackrel{{\mathit{v}}_c}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{v}}_i}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\overline{\mathit{q}}}_{ic}}{\overbrace{{\mathit{G}}_{[1,4]}}}& \stackrel{{\overline{\mathit{q}}}_i}{\overbrace{{\mathbf{0}}_{3\times 4}}}& \stackrel{{\mathit{\omega }}_c}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{\omega }}_i}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{b}}_a}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{b}}_{\omega }}{\overbrace{{\mathbf{0}}_{3\times 3}}}\\ {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{I}}_{4\times 4}& {\mathbf{0}}_{4\times 4}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 4}& {\mathbf{I}}_{4\times 4}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathit{G}}_{[4,4]}& {\mathbf{0}}_{1\times 4}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 4}& {\mathit{G}}_{[5,4]}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 4}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{G}}_{[9,2]}& {\mu }_v{\mathbf{I}}_{3\times 3}& {\mathit{G}}_{[9,4]}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{G}}_{[9,8]}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 4}& {\mathbf{0}}_{4\times 4}& {\mathit{G}}_{[10,6]}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& 0.5\Xi \left({\overline{\mathit{q}}}_i\right)\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{G}}_{[11,4]}& {\mathit{G}}_{[11,5]}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& -{\mu }_v{\mathit{R}}_i& {\mathit{G}}_{[11,9]}\end{array}\right]}_{31\times 29},$$

(41)

where the matrices $\mathit{G}$ with the row and column indexes as subscripts do not contribute to the rank analysis.

To calculate the column rank, block Gaussian elimination can be applied. Once the corresponding block is an identity matrix, the relative column can be eliminated. Thus, only the last two columns are needed for the analysis and can be simplified as follows:

$${\mathbf{\Omega }}^{\prime }=\left[\begin{array}{cc}0& 0.5\Xi \left({\dot{\overline{\mathit{q}}}}_i\right)\\ -{\mu }_v{\mathit{R}}_i& {\mathit{G}}_{[11,9]}\end{array}\right].$$

(42)

Therefore, if ${\mu }_v$ were non-zero, and any axis of the IMU was excited in any direction, ${\mathbf{\Omega }}^{\prime }$ has full column rank, and $\mathbf{\Omega }$, thus, has full column rank. This means the SR-CICEKF has local weak observability [21,34]. The above conditions of achieving full rank are easily fulfilled during the application.

4.2. Observability Analysis of the MR-CICEKF

Resembling the previous analysis, the nonlinear system representing the measurement results of the MR-CICEKF can be expressed as follows:

$$f\left(\mathit{Y}\right)=\dot{\mathit{Y}}={\left\{{\dot{\mathit{p}}}_r{}^T {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\dot{\overline{\mathit{q}}}}^T {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\right\}}^T.$$

(43)

The measurement functions for MR-CICEKF are designed as ${\mathit{h}}_r\left(\dot{\mathit{Y}}\right)={\left[{\mathit{h}}_{r1}{}^T,\dots ,{{\mathit{h}}_{r6}}^T\right]}^T$, with ${\mathit{h}}_{r1}={\mathit{p}}_r$, ${\mathit{h}}_{r2}={\overline{\mathit{q}}}_r$, ${\mathit{h}}_{r3}={\mathit{v}}_{i0}$, ${\mathit{h}}_{r4}={\mathit{v}}_{i1}$, ${\mathit{h}}_{r5}={\mathit{\omega }}_{i0}$, and ${\mathit{h}}_{r6}={\mathit{\omega }}_{i1}$.

After applying Lie derivative rules, the observability matrix of the MR-CICEKF system is constructed as follows:

$$\Theta =\left[\begin{array}{l}\nabla L^0{\mathit{h}}_{r1}\\ \nabla L^0{\mathit{h}}_{r2}\\ \nabla L^0{\mathit{h}}_{r3}\\ \nabla L^0{\mathit{h}}_{r4}\\ \nabla L^0{\mathit{h}}_{r5}\\ \nabla L^0{\mathit{h}}_{r6}\end{array}\right]={\left[\begin{array}{cccccc}\stackrel{{\mathit{p}}_r}{\overbrace{{\mathbf{I}}_{3\times 3}}}& \stackrel{{\mathit{v}}_{i0}}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{v}}_{i1}}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\overline{\mathit{q}}}_r}{\overbrace{{\mathbf{0}}_{3\times 4}}}& \stackrel{{\mathit{\omega }}_{i0}}{\overbrace{{\mathbf{0}}_{3\times 3}}}& \stackrel{{\mathit{\omega }}_{i1}}{\overbrace{{\mathbf{0}}_{3\times 3}}}\\ {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{I}}_{4\times 4}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 4}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]}_{19\times 19}.$$

(44)

All the block columns in $\Theta$ have full column rank when the single-robot measurement yields non-zero outcomes, which means that the system has local weak observability [21,34].

5. Data Test

Two sets of motion simulations were performed to test the feasibility of the CICEKF algorithm. The motion curve was chosen as quintic, which is convenient for calculating the linear accelerations.

5.1. SR-CICEKF Simulation Test

To simplify the simulation for an individual sensor suite, the coordinate frames of the IMU and the stereo vision are assumed to coincide, and the biases are omitted. Once the parameters of the quintic curve are confirmed, the position coordinates of the curve can be treated as the position ground truth. By analyzing the projections of the curve on the three standard planes, the roll, pitch, and yaw orientations of the adjacent points can be achieved by allocating the starting and ending points, which yields the ground truth of the orientation. Thus, the ideal linear acceleration, linear velocity, and angular velocity data can be differentiated from the position and orientations, which are utilized as the filter input after being corrupted with Gaussian noise.

The position and orientation curves of the generated quintic trajectory across all three axes are shown in Figure 3 and Figure 4, respectively. It should be noted that the change in the trajectory is designed in a short time span of 10 s with a 1000 Hz update rate, which means the change in simulation data is quite fast; for example, the maximum angular velocity is over 6 rad/s, and the max linear acceleration is over 11 m²/s.

${\mathit{P}}_{k|k}$ can be asymptotically stable after a complete run with the randomized initial values. By reusing the stabilized ${\mathit{P}}_{k|k}$ in other runs, the initial convergence of the filter process can be greatly boosted. ${\mathit{R}}_k$ is designed as a skew-symmetric matrix containing small random values. The coefficients are set as ${\mu }_v=0.5$ and ${\mu }_{\omega }=0.5$. The initial position and orientation are set as [0, 0, 0].

The three-dimensional position curves of the SR-CICEKF simulation are shown in Figure 5, with the circle mark as the starting point and the star markers as the ending points. The errors between the virtual measurement values and the filtered values of the position and orientation are shown in Figure 6 and Figure 7, respectively. The statistical results of RMSE, mean errors, and STD of the norm errors involving the three axes of the position and the orientation are listed in

Table 2. The norm errors between the ground truth and the SR-CICEKF simulation results.

Translation RMSE	Translation Mean Error	Translation STD	Orientation RMSE	Orientation Mean Error	Orientation STD
0.1593 m	0.0429 m	0.1535 m	0.106 rad	0.0187 rad	0.1044 rad

.

By inspecting the curves and the statistical values, it can be concluded that the SR-CICEKF quickly converges under severe data changes, while it does not need careful parameter tuning for the first estimation of ${\mathit{P}}_{k|k}$. This indicates that the filter is robust, and the observability of the fusion process is identified indirectly.

5.2. MR-CICEKF Simulation

The goal of the simulation is to estimate the relative translation and rotation between the master and slave robots. The MR-CICEKF simulation utilizes the same quintic data as the target trajectories of both the master and slave robots. The absolute relative translation of the two robots is set as [1, 1, 1], and the relative rotation is set as [0, 0, 0]. The MR-CICEKF is tested independently of the SR-CICEKF. Thus, the performance of the MR-CICEKF is not affected by the possible inaccuracy produced by the SR-CICEKF process. The configurations of ${\mathit{P}}_{k|k}^r$ and ${\mathit{R}}_k^r$ are similar to the SR-CICEKF simulation, and the simulation input is corrupted with Gaussian noise.

Figure 8 depicts the three-dimensional position curves of the MR-CICEKF simulation, containing the ground truth of the master and slave robots’ trajectories, and the filtered slave robot trajectory, which is achieved by adding the estimated ${\mathit{p}}_r$ and ${\overline{\mathit{q}}}_r$ to the master trajectory. The circle marks are the starting points, and the star markers are the ending points. The statistical values of RMSE, mean errors, and STD of the norm errors involving the three axes of the position and the orientation obtained after comparing the ground truth with the filtered slave robot trajectories are listed in

Table 3. The norm errors between the ground truth and the filtered slave robot trajectories.

Translation RMSE	Translation Mean Error	Translation STD	Orientation RMSE	Orientation Mean Error	Orientation STD
0.01872 m	5.286 × 10−6 m	0.0187 m	0.0016 rad	4.95 × 10−7 rad	0.0016 rad

.

The statistical values indicate that the final error is similar to the small white noise added. Thus, it can be concluded that the MR-CICEKF possesses good estimation accuracy, while the main error is caused by eliminating high-order terms during the state transition. This feature can significantly simplify further application since the MR-CICEKF contributes little error to the entire CICEKF process.

5.3. Dataset Test

To further analyze the performance of the CICEKF in real-world scenarios, a test was performed with the EuRoC dataset. The SR-CICEKF part is tested solely since the MR-CICEKF already showed great performance. The state-of-the-art visual SLAM method, ORB-SLAM V3 [16], was utilized as the visual black box measurement algorithm. The SR-CICEKF runs at 200 Hz, which is synchronous with the IMU update rate of the dataset. By employing EVO tools [35], the RPE features of the trajectories can be directly digitized.

The stereo vision of ORB-SLAM V3 runs at 20 Hz, and all measurement frames were treated as keyframes. The coefficients were set as ${\mu }_v=0.9$ and ${\mu }_{\omega }=0.5$. The initial covariance matrix ${\mathit{P}}_{k|k}$ was tuned following the aforementioned strategy. The SR-CICEKF, stereo ORB-SLAM V3, and its variant, along with the stereo MSCKF [17], were carried out with the room dataset of EuRoC under the same computer configuration. Figure 9 shows the comparison of the estimation trajectories of the SR-CICEKF and the ground truth. Figure 10 shows the dataset test scenarios of the algorithms separately. The RPE comparison results between each algorithm with the ground truth achieved by utilizing the EVO tools are listed in

Table 4. Norm errors of the translations between the ground truth and the results of the dataset tests.

	Translation RMSE	Translation Mean Error	Translation STD
SR-CICEKF	0.004211 m	0.003982 m	0.001371 m
Stereo ORB-SLAM V3	0.03844 m	0.03428 m	0.01739 m
Stereo ORB-SLAM V3 with the IMU	0.003236 m	0.002785 m	0.001648 m
Stereo MSCKF	0.0556 m	0.048994 m	0.02629 m

.

From the comparison, it can be concluded that by fusing IMU information and the pure visual black box measurement with the proposed SR-CICEKF algorithm, the accuracy has been greatly improved. Although the accuracy of the approach does not reach that of the optimization-based method, the fusion computational complexity is as low as $\mathit{O}\left(n\right)$. This can benefit the fusion with other visual measurement algorithms to improve the performance of trajectory estimation without affecting the computing efficiency. Furthermore, it can be combined with MR-CICEKF for the simultaneous estimation of multiple agents’ trajectories and collaborative configurations.

6. Experiments

Two experiments were designed to focus the discussion on the feasibility of the proposed method. Firstly, the Intel RealSense D435i, which contains a stereo set of cameras and an IMU, was mounted on a wheeled robot that performs locomotion. The SR-CICEKF estimated the locomotive trajectory while the ground truth was measured by the NOKOV motion capture system. Then, the D435i camera data were utilized for collaborative loco-motion estimation. The experimental scenarios (Supplementary Materials) are shown in Figure 11.

For both parts of the CICEKF, the black-box stereo visual algorithm is ORB-SLAM V3 operating at 30 Hz, while the IMU update rate is set to 200 Hz. The calibration of the cameras and IMUs was conducted using Kalibr tools [36] to obtain the intrinsic and extrinsic parameters and the fixed configuration of the integrated sensors.

During the experiment, we noticed that ORB-SLAM V3 and its IMU variant might not be stable under our experimental conditions due to the calibration accuracy and the IMU lacking sufficient excitation, since we used a slow ground vehicle as the motion platform. Therefore, to demonstrate the performance of the SR-CICEKF, the estimated trajectory was compared with that of stereo ORB-SLAM V3 and stereo MSCKF. The coefficients were set as ${\mu }_v=0.5$ and ${\mu }_{\omega }=0.5$. The comparison of the measured trajectory and the ground truth is shown in Figure 12. The RPE comparison results between each algorithm with the ground truth in a real scenario are listed in

Table 5. Norm errors of the translations between the ground truth and the experimental results.

	Translation RMSE	Translation Mean Error	Translation STD
SR-CICEKF	0.00459 m	0.006448 m	0.004528 m
Stereo ORB-SLAM V3	0.01723 m	0.01275 m	0.01159 m
Stereo MSCKF	0.01745 m	0.01292 m	0.01173 m

.

For the MR-CICEKF experiment, the two D435i cameras were calibrated with Kalibr tools to achieve a fixed relative configuration as both the ground truth and the initial guess of the filter. The relative configuration estimation of the MR-CICEKF was compared with the ground truth. The norm errors of the relative configuration estimation are listed in

Table 6. Norm errors of the relative configuration estimation.

Translation RMSE	Translation Mean Error	Translation STD	Orientation RMSE	Orientation Mean Error	Orientation STD
0.009398 m	0.0001 m	0.0094 m	0.032 rad	0.00602 rad	0.03145 rad

.

From the experimental results, it can be concluded that the proposed algorithm can not only improve the accuracy of pure visual measurement algorithms but also reliably estimate the relative configuration of multiple robots.

7. Conclusions

This study introduces a novel motion estimation approach with a loosely coupled and dual-step structure for the autonomous and collaborative localization of multiple locomotive robots. Both steps of the algorithm can run standalone, serially, or in parallel while maintaining a computational complexity of $O\left(n\right)$, which allows for convenient improvement to pure vision measurement techniques after introducing an IMU. Both the simulations and experiments demonstrate that fusing data on the velocity level without post-optimization greatly contributes to the robot trajectory estimation accuracy. Due to current experimental limitations, further research will be focused on systematically examining how the coupling coefficients affect filter performance and stability across various integrated sensors and high-speed motion units.