Delayed fusion of relative state measurements by extending stochastic cloning via direct Kalman filtering

In this work, a real-time localization problem is formulated by incorporating exteroceptive sensors as sources of both relative state measurement and delayed information within a direct Kalman filtering approach. The problem of delayed incorporation of relative state measurements is addressed by extending the stochastic cloning framework, whereby an augmented state vector is defined by copies of the current and delayed states. The formulation is developed within a tightly coupled vision-aided inertial navigation system suitable for GPS-denied environments. To enhance performance, a previously described approximate observation model is replaced here with an exact one. The proposed formulation is tested and demonstrated with simulation experiments conducted on a small RUAV flying in a virtual environment, showing the mitigation of adverse effects on the estimator performance caused by delays and the nature of relative state measurements.