Ensemble Kalman filter for multisensor fusion with multistep delayed measurements

For a target tracking problem, such as tracking of a mobile robot or an unmanned vehicle, multiple sensors are required to achieve accurate estimated position of the target. Practically, measurements from sensors arrive out of sequence, e.g., delayed data due to the processing of images. We call these measurements Out of Sequence Measurements (OOSMs). Many researches propose solutions to OOSMs using an Extended Kalman filter (EKF) or particle filter (PF) as a basic algorithm. Our previous research proposes an algorithm that applies Ensemble Kalman filter (EnKF) to handle the OOSM problem. We store ensembles of the state particles during the filtering process and make use of the information about those ensembles later. By calculating a cross covariance between ensembles from different points of time, we can directly update the current estimated state with delayed measurements. Moreover, by using EnKF, we can simply apply the method to systems with strong nonlinear models without finding any Jacobian or backward transition matrix. However, our previous algorithm only preforms well for one-step lag measurements. In order to handle multistep lag measurement, in this paper, we propose an algorithm with an additional backward updating step. We illustrate the results of simulations comparing with Rauch-Tung-Striebel (RTS) smoothing filter and the conventional algorithms proposed by [1] and [2] which apply EKF and particle filter techniques, respectively. The proposed algorithm shows commendable results compared to others.