Of late, there has been some interest in cooperative estimation algorithms in robotics, where the information sources are usually sensors such as cameras. When multiple robots observe surrounding environments through cameras, they can share information about salient features and correct the poses of each other. At the same time, these measurements are not independent because the source of information may have been the same, or the robots may have exchanged some data in the past and have not kept track of cross correlation terms.

In this context, there have been many papers discussing covariance intersection as a way to fuse relative measurements with individual measurements. In my example for this question, there are two robots $i$ and $j$ which are capable of performing their own position estimation at every time step. Occasionally, robot $i$ can compute the relative position to robot $j$. Say at time step $k$, $i$ generates a relative measurement to $j$ (say, $z_k^{ij}$) and using this, generates a propagated state-covariance pair for the robot $j$ as follows:

\begin{gather} \mathbf{x}_k^{j'} = \mathbf{x}_{k|k-1}^{i} + \mathbf{M}_{k}^{i,j} \mathbf{z}_{k}^{i,j} \\ \mathbf{P}_{k}^{j'} = \mathbf{H}_{k}^{i,j} \mathbf{P}_{k|k-1}^i \mathbf{H}_{k}^{{i,j}^\top} + \mathbf{M}_{k}^{i,j} \mathbf{R}_{k}^{i,j} \mathbf{M}_k^{{i,j}^\top} \end{gather}

On the other side, the robot $j$ already has a locally propagated state and covariance estimate, and once the above pair is received, it fuses this relative data with its own estimate using covariance intersection.

\begin{gather} \mathbf{P}_{k|k}^j = {\Big[\pmb{\omega}(\mathbf{P}_{k|k-1}^j)^{-1} + (1-\pmb{\omega})(\mathbf{P}_{k}^{j'})^{-1}\Big]}^{-1} \\ \mathbf{x}_{k|k}^j = \mathbf{P}_{k|k}^j \Big[\pmb{\omega}(\mathbf{P}_{k|k-1}^j)^{-1} \mathbf{x}_{k|k-1}^j + (1-\pmb{\omega})(\mathbf{P}_{k}^{j'})^{-1} \mathbf{x}_k^{j'} \Big] \end{gather}

My questions:

  1. In real systems, measurements and corrections, especially when involving communication, has a significant delay. While there has been some discussion on how to incorporate delayed measurements in the conventional KF update steps, how would a CI based Kalman filter deal with delays? Say robot $i$s state covariance pair for robot $j$ reaches robot $j$ not at time step $k$, as would have been ideal, but at time step $k+\delta$. Can a modification to the CI algorithm take that into account? Assuming that robot $j$ has access to its own measurements meanwhile.
  2. What happens to the usual update step of robot $j$ when it receives a relative measurement? Does it happen after the CI fusion step? In that case, should this also be treated as a 'delayed' measurement?
  • 1
    $\begingroup$ if something is Markov forward in time, it is probably Markov backwards in time, so it is often possible to back up to the late arrived point, update and propagate to the present. $\endgroup$ – Stanley Pawlukiewicz Feb 12 '18 at 23:44

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.