Figure 6
From: Cooperative localization by dual foot-mounted inertial sensors and inter-agent ranging
Illustration of the separation constraint update for two feet in the horizontal plane. The plots show (upper left) the prior position and covariance estimates, (upper right) the transformed system with the sigma points (blue crosses) and the constraint (dashed circle), (lower left) the projected sigma points and the conditional mean and covariance, and finally (lower-right) the conditioned result in the original domain with the prior covariances indicated with thinner lines.