Algorithm 1: The cooperative localization algorithm based on State Estimation Error Compensation

1:Initialize: Assume that each robot in the system initially knows its pose with respect to a given reference coordinate frame. As Figure 1 shows, consider that at time $t_k$, the follower robot receives the pose information from the leader robot with time delay after $N$ Kalman filters at time $t_s$. 2:  State prediction and compensation: Give the one-step state prediction and covariance matrix: $$\stackrel{⌢}{X}\left(k+1,k\right)=\Phi \left(k\right)\stackrel{⌢}{X}\left(k\right)$$ $$P\left(k+1,k\right)=\Phi \left(k\right)P\left(k\right){\Phi }^T\left(k\right)+\Gamma \left(k\right)Q\left(k\right){\Gamma }^T\left(k\right)$$ 3:  Calculate the state estimation error compensation: $$M\left(k\right)=\Phi \left(k\right)\left(I-K\left(k\right)C\left(k\right)\right)M\left(k-1\right)$$ $$\Delta \stackrel{⌢}{X}\left(k+1,k\right)=M\left(k\right)\Phi \left(s\right)K\left(s\right)\left(Z\left(s\right)-C\left(s\right)\stackrel{⌢}{X}\left(s-1,s\right)\right)$$ 4:  Compute the filter gain: $$\begin{array}{l}K\left(k+1\right)\\ =P\left(k+1,k\right)C^T\left(k\right){\left[C\left(k\right)P\left(k+1,k\right)C^T\left(k\right)+R\left(k\right)\right]}^{-1}\end{array}$$ 5:  Construct the error-state propagation equation and the covariance propagation equation: $$\stackrel{⌢}{X}\left(k+1,k\right)=X^{\prime }\left(k+1,k\right)+\Delta \stackrel{⌢}{X}\left(k+1,k\right)$$ $$\stackrel{⌢}{X}\left(k+1\right)=\stackrel{⌢}{X}\left(k+1,k\right)+K\left(k+1\right)\left(Z\left(k+1\right)-C\left(k\right)\stackrel{⌢}{X}\left(k+1,k\right)\right)$$ $$P\left(k+1\right)=\left(I-K\left(k+1\right)C\left(k+1\right)\right)P\left(k+1,k\right)$$ 6:end