This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).
This paper presents a state of the art of the estimation algorithms dealing with OutofSequence (OOS) measurements for nonlinearly modeled systems. The state of the art includes a critical analysis of the algorithm properties that takes into account the applicability of these techniques to autonomous mobile robot navigation based on the fusion of the measurements provided, delayed and OOS, by multiple sensors. Besides, it shows a representative example of the use of one of the most computationally efficient approaches in the localization module of the control software of a real robot (which has nonlinear dynamics, and linear and nonlinear sensors) and compares its performance against other approaches. The simulated results obtained with the selected OOS algorithm shows the computational requirements that each sensor of the robot imposes to it. The real experiments show how the inclusion of the selected OOS algorithm in the control software lets the robot successfully navigate in spite of receiving many OOS measurements. Finally, the comparison highlights that not only is the selected OOS algorithm among the best performing ones of the comparison, but it also has the lowest computational and memory cost.
Autonomous mobile robots require to be localized, either in a local or global frame, in order to successfully perform different navigation tasks. With that purpose, they are usually equipped with (1) multiple sensors that provide redundant or complementary information about the robot location and (2) a localization module that is responsible for estimating online the robot location fusing the information provided by the sensors. The calculated location estimates are used in other modules of the robot control software that are in charge of deciding how the robot should act next. This makes the control signals applied to the robot actuators highly dependent on the location estimates. Therefore, it is extremely important to estimate the robot location correctly and efficiently.
In order to achieve both objectives, the location module can implement any of the sequential estimators that can deal with the uncertainty and characteristics associated to the robot dynamics and sensors, such as the Kalman, Information or Particle Filter (KF, IF, PF [
However, the measurements often arrive delayed to the localization module, due to multiple factors such as the physical distribution of the sensors in some robotic problems, the communication network used to send information from the sensing modules to the localization one, and/or the time used to preprocess the raw measurements and extract the useful information that is sent to the localization module. The most difficult scenario appears when the delays and the sequence of the arrival of information to the localization module are not fixed, constituting the OutOfSequence Problem (OOSP) [
In order to deal with the measurement arrival delays, the localization module can basically implement four different solutions. The first, which consists on discarding the delayed measurements, is the easiest solution because it only requires to check the time stamp of the measurements. However, it is only useful for systems with spurious delayed measurements, because rejecting measurements increases the uncertainty of the robot location, affecting the stability and reducing the reliability of the robot control system [
The estimation community has developed many OOS algorithms during the last decade in order to fulfill the requirements of an increasing number of sensor networks and tracking networked systems. Among the big number of possibilities, the choice of OOS algorithm to implement in the localization module of the robot depends on (1) the types of dynamic and sensorial models associated to the robot, and on (2) the nature of the out of sequence problems that the localization module faces. For instance, those autonomous robots whose dynamic and sensorial behaviors are modeled as linear systems with additive Gaussian noise can implement any of the big quantity of OOS Kalman or Information Filter variants (see [
This paper is organized as follows. Section 2 presents the analysis of the characteristics of the existing OOS filters for nonlinear systems and studies the tracking nonlinear modeled mobile object problems that have been tested with the analyzed OOS filters and that are close to the problem of estimating the location of an autonomous mobile robot with controlling purposes. In order to illustrate how to apply one of these techniques within the control software of a real robot, Section 3 analyzes the dynamic and sensors models of one of our robots, explains the general and efficient algorithm that has been adapted to use in the localization module of the robot control software, analyzes the performance of this algorithm under simulated and real data, and compares its performance against some other applicable OOS techniques. Finally, some conclusions are presented in Section 4.
In this section we analyze the different OOS filters for nonlinear systems that haven been developed during the last years as well as the autonomous robotic and tracking examples that are found in the literature and that already consider the OOSP.
The algorithms analyzed in this section are those that can estimate the location
One important type of system that fulfills those requirements, and which can be used to model different types of autonomous mobile robots, is the group of
In order to estimate the value of
The basic formulations of the LKF/LIF, EKF/EIF, UKF, EnKF, and PF cannot deal with the OOSP. Therefore, new OOS versions of these filters have been developed in the last decade. We briefly describe them in the following paragraphs, referring to those OOS algorithms that appear in the same paper as others by the paper number and its habitual nomenclature in the OOS literature (for instance, [
 [
 [
 [
 The OOS SIR PFs ([
 The OOS MPFs ([
The main characteristics of the OOS nonlinear filters are summarized in
An analysis of the table shows that the OOS nonlinear algorithms that can be used for systems with linear and/or nonlinear systems with measurements delayed more than 1step lag are: the OOS EIF [
To the best of the authors’ knowledge, there is not any published localization module that forms part of a control system of an autonomous mobile robots that deals with the OOSP using an OOS algorithm, instead of throwing the delayed measurements (easiest and more frequent approach), delaying the operations [
However, the majority of the OOS nonlinear filters analyzed in the previous section are used to track the location of mobile objects. Based on the information contained in the robot location
From the point of view of a tracking system both problems are equally interesting, although the second is more difficult due to (1) the nonlinearities in the transition model and to (2) the discontinuity between 0 and 2
Based on the previous analysis, when the robot dynamics are modeled with nonlinear expressions in the localization module of the control system of an autonomous mobile robot, we suggest to select those algorithms that have an inherent complete nonlinear support ([
When the robot dynamics can be modeled with linear expressions, any of the techniques with linear and/or nonlinear dynamic model support can be applied. However, special care should be taken with the OOS EKFs that by default do not consider that those sensors with strong nonlinearities can require to have the information recalculated when measurements with older time stamps arrive later than the ones associated to them that have been already assimilated.
This section illustrates the benefits of using one of these OOS techniques by implementing an adapted version of [
In this section we present the dynamic and sensorial models associated to the localization module of the autonomous mobile robot that we will use in our experiments. Besides, the constants and covariances of the models are presented in Section 3.3.1, and its Jacobians in the
The robot, represented in
The location
The dynamics of our robot can be modeled with different types of expressions [
The covariance matrix
The robot is equipped with an electronic compass that provides information (
The covariance matrix
The robot is also equipped with two incremental encoders that can be used to obtain information about the displacement of each motorized wheel. This displacement, represented as
Finally, the robot is also equipped with a belt of 8 ultrasonic sensors numbered from s = 3 to s = 10 (as s = 1 and s = 2 are already associated to the compass and the encoders), distributed around the upper robot platform as
The distance between a
The distance between a
The distance between a
In order to relate the robot pose
In short, the expressions used to model the robot behavior have the following characteristics:
The dynamic/transition model is nonlinear. Besides, the angular location
The compass model is also nonlinear due to the
The two encoders grouped together in
The behavior of the sensor that combines the measurement of each ultrasonic sensor
That is, we are going to estimate the location of a robot using a nonlinear transition model, and linear and nonlinear measurement models.
In this section, we present an adapted version of EIFAsyn, the OOS EIF that consists of sets of EKF predictions, EIF updates and projections between the state and information space [
The two main steps of this version of EIFAsyn, prediction (carry out to make the filter estimate the state of the next time step) and measurement update (performed when any measurement is arrived), are presented in
The EIFAsyn prediction step (
The EIFAsyn update step (
The recalculation of the information of sensor
Therefore it is necessary to study the behavior of the nonlinear sensors before deciding to exclude them from the recalculation set. In our problem, we have to analyze the behavior of the:
The validity of both choices has been tested in multiple simulations. Before presenting their results in the following section, it is worth noting that the necessity of recalculating the sensorial information associated to some nonlinear sensors is a requirement of EIFAsyn and of the OOS EKF (which can not be used for our problem due to the additional requirement of the linearity in the transition model), due to the direct relationship that exists between the EKF and EIF [
Finally, note that the additional step associated to the correction of the angle does not change the behavior of the algorithm significantly. Moreover, it is only a convenient modification. However, the step associated to the validation step, which is useful to reject those measurements that are not valid due to the malfunction of the sensors or corrupted during the communication through the network, has a bigger impact on the results of the algorithm. Moreover, as the validity of the measurements is only tested once using the location estimate in the test, the validity of the measurements that are in the limit can depend on the order of arrival of the measurements. However, as the analysis of the validation for the asynchronous version of IFAsyn shows [
In this section we present the results obtained by EIFAsyn for the localization of our autonomous mobile robot with simulated and real data. The simulated experiments are set up to see which of the nonlinear sensors require the recalculation of its measurements information when there are delayed data and what happens if the OOS data is not used. The real experiment shows if the localization module based on the OOSEIF works properly when it is part of the control software of the actual robot.
In order to analyze what happens if the delayed measurements are not used and which nonlinear sensors require the recalculation of its information when there are delayed measurements, we run multiple simulations to generate all the sensors measurements every 0.1 s and run EIFAsyn, for the same generated data, considering 5 different scenarios: (1) using all the measurements without delay; (2) using all the measurements with delays in the compass data without recalculating the sonar information; (3) using all the measurements with delays in the sonar data recalculating the compass information; (4) using all the measurements with delays in the sonar data without recalculating the compass information; and (5) not using the delayed measurements of the compass and sonars. We use constant delays of 1 s in all the cases,
As the nonlinearity of the compass is due to the angular indetermination [0, 2
The results of EIFAsyn for the different setups and the values obtained during one of the simulations are summarized in the graphics presented in
The first column shows the behavior of the sensors and validation tests representing all the measurements received by EIFAsyn grouped by sensor (color) and status:
At a first glance, the state and covariance graphics of the first, second and third row look really similar. That is, EIFAsyn obtains similar results when the data is nondelayed, when the compass data is delayed and the filter does not recalculate the nondelayed sonar information, and when the sonar data is delayed and the filter recalculates the nondelayed compass information. The sensor behavioral graphics look different because they encode both the existence of the delays and the results of the validation. If we compare only their validation status, we can observe that the same data is validated in the experiment without delayed data (first row) and with delayed sonar measurements recalculating the compass information (third row), making the state and covariances under both setups equal. However, the validation status of the experiment with delayed compass data (second row) and without delayed measurement (first row) is slightly different, because the order of arrival of the information can slightly affect the validation step [
The results presented in the fourth row of
When we generate the simulated data 100 times with the same setup, we observe the same type of erroneous results, sooner or later, 46 times. That is, while moving around the indetermination, the compass information requires to be recalculated almost in half of the simulations. The discrepancies associated to the nonrecalculation of the sonar information are minimal and negligible in all the tests. Therefore, and in order to avoid the loss of track of the filter when it is embedded in the control architecture of a real robot, we treat the compass and sonar respectively as sensors with strong and soft nonlinearities,
Finally, the results presented in the fifth row of
In order to illustrate the behavior of EIFAsyn within the localization module of the control software of a real robot, we place the robot in the same hall with the same known objects and two unknown ones, and let the control software generate the control signals (within the range [0,15] cm/s) based on the robot location estimated by EIFAsyn and the obstacles locations estimated using the occupancy grid and Bayesian filter in [
The results of the experiment are presented in the sensorial behavior, state and covariance graphics of
The sparsity of the sonar information makes the OOS filter of the localization module of the control software estimate the robot localization using almost all the compass and encoders information, and only a few measurements provided by the sonars. The small amount of (
In this section we compare, using the robotic problem study in this paper, the behavior of EIFAsyn against the performance and efficiency of some of the general Nstep lag OOS algorithms suggested in Section 2.3. The nonlinearity in the dynamic model of the robot reduces the possible comparison to the OOS EnKF in [
In order to carry out the comparison, we have disabled the validation step of EIFAsyn because the other algorithms do not include it and we want all the filters to be able to use the same information. Note that the same validation step can not be included in the other filters because it is not straightforward to obtain in those filters the predicted mean and covariance values used to validate the new measurements in EIFAsyn. Additionally, we compare EIFAsyn with two versions of each of the selected algorithms, because:
The OOS PF [
The OOS EnKF [
The two variants of the OOS PF [
On one hand,
On the other,
Finally, the performance comparison of the filters in the fifth scenario (fourth row) show that for the robotic problem we need to incorporate the compass and sonar data, because when any of the filters work only with the encoders measurements, the location mean values diverge and the location variances significantly grow.
In order to measure the computational load of the 5 filters, we measure for the 4 scenarios presented in the previous section the time required by each of the filters, which are implemented in C++, when they are run over a Intel Core2 Duo at 2.53 GHz with Windows XP. This time is measured in milliseconds with the help of the C++ function GetTickCounts(), which retrieves the number of milliseconds that have elapsed since the PC was started. The resolution of the function, and therefore of our measurements, are in the range of 10 to 18 milliseconds.
The obtained results are presented in
The mean time columns show that for each filter, the computational load of the last scenario (where the filters are only predicting and assimilating nondelayed encoder measurements) is smaller than the computational load of the first (where the filters assimilate all the available measurements without delays) and that the first and fifth are lower than the second and third (where the filters assimilate delayed measurements). The similar values within the second and third scenario in all the filters show that each filter is similarly computationally penalized when the delays occur in the compass or in the sonar measurements.
The normalized mean time columns show that for the same scenario EIFAsyn has a significantly lower computational load than the other filters. Besides, the computational load of the modified versions of the selected filters are lower than the computational load of their standard counterparts. In the PF case, the bigger computational load of the standard version is due to the necessity of computing the mean and covariance values of the intermediate states when any delayed measurements arrives. In the EnKF case, the bigger computational load of the standard version is due to the computational difference of fusing the information of 10 or 4 ensembles.
In short, the filters can be sorted according to the computational time, from the best to the worst as: EIFAsyn, PF (MOD), PF (STD), EnKF (MOD), EnKF (STD). Finally, it is worth noting that the linear relationship that exists in the selected PF and EnKF between the number of particles and the computational load of the algorithms will make these PFs/EnKFs with only 200/50 particles (which are are not enough to make any of the filters work properly) still have a computational load one order of magnitude bigger than EIFAsyn. Therefore, the low computational load of the EIFAsyn, in spite of its necessity to recalculate the information associated to the sensors with strong nonlinearities, is a plus that can not be underestimated.
In order to compare the memory requirements of the 5 filters, we theoretical count the number of floats that are necessary to store the minimal information required to make them assimilate any measurement that is delayed at maximum
The results of this count are presented in the second column of
The results of the count for our problem (
Taking into account the performance comparison, we conclude that while the OOS EnKF variants are invalid for our problem, both the OOS PF variants and EIFAsyn are similarly applicable. Besides, between the two valid possibilities, the memory and computational requirements of EIFAsyn are significantly smaller, making it an ideal choice to be incorporated in the complex control architecture of a robotic system like ours.
This paper presents a thoughtful analysis of the different possibilities of OOS algorithms that exist for estimating the location of nonlinearly modeled robots, taking into account the properties of the existing algorithms and of the robotic problem. Therefore, this analysis can be used to determine which are the best algorithms for different types of nonlinearly modeled OOS estimation problems, as well as to know which problems similar to the autonomous mobile robot location estimation one have already been tackled with which algorithms.
Besides, this paper also shows how one of the most generic and efficient OOS algorithms, the EIFAsyn presented in [
Finally, this paper also compares the performance, computational cost and memory requirements of EIFAsyn with two of the other techniques that have been selected using the characteristics determined in our thoughtful analysis. The results of the comparison show that the selected algorithm EIFAsyn is the best choice for the localization problem regarding its memory and computational cost and one of the best options regarding its performance.
This work has been supported by the Spanish Grants DPI200914552C0201 and CAM S0505/DPI 0391.
In the following equations, we show the Jacobians,
For
Relevant additional aspects to implement the OOS PFs and EnKFs.
Calculating mean values of angular variables  x  x  Performing the operation in the cartesian space associated to the polar coordinates of the angle [ 
Resampling step  x  Carrying it out optionally, based on the number of effective particles [  
The existence of a sonar prediction depends on the state orientation  x  Assigning a 0 weight to those particles from which the object can not be observed, unless no observation can be predicted from any particle  
x  Assimilating the sonar measurement only if an object can be observed from all the particles of the ensemble 
OutOfSequence Problem. (
Robot. (
Sensor Models. (
Prediction and Measurement Update Steps of the adapted EIFAsyn. (
Experiments Setup. (a) Map Objects; (b) Simulated Experiment; (c) Real Experiment.
Simulated Experiments.
Real Experiments. (
OOS Approaches Comparison.
OOS Filters for Gaussian NonLinear Systems.
[ 
LKF  [ 
Retrodiction  Lzd  Lzd  1 steplag  
[ 
EKF  [ 
Retrodiction  L  L/NL  N steplag  
[ 
EKF  [ 
Retrodiction  L  L/NL  N steplag  
[ 
EKF  [ 
Retrodiction  L  L/NL  N steplag  
[ 
EKF  [ 
Forward propagation  L  L/NL  N steplag  
[ 
EIF  [ 
Forward propagation  L/NL  L/NL  N steplag  Optional recalculation 
[ 
UKF  [ 
Retrodiction  L  L/NL  1 steplag  
[ 
EnKF  [ 
Linear Interpolation  L/NL  L/NL  N steplag  
[ 
PF  SIR 

L  L/NL  N steplag  
[ 
PF  SIR 

L  L/NL  N steplag  MCMC smoothing 
[ 
PF  SIR  Linear Interpolation  L/NL  L/NL  N steplag  
[ 
PF  UPF 

L  L/NL  N steplag  
[ 
PF  SIR 

L/NL  L/NL  N steplag  
[ 
PF  MPF  Fixedpoint EK Smoother  L/NL  L/NL  N steplag  Check diversity 
[ 
PF  MPF  Fixedpoint UK Smoother  L/NL  L/NL  N steplag  Check diversity 
[ 
PF  MPF  Fixedpoint Particle Smoother  L/NL  L/NL  N steplag  Check diversity 
[ 
PF  MPF 

L  L/NL  N steplag  
[ 
PF  [ 
[ 
L/NL  L/NL  N steplag  Check diversity 
Constants and Variances of the Models.
Real Robot Data  (1)^{2}  (2 
(2 
(0.5)^{2}  (0.5)^{2}  (1.5)^{2}  
Simulation Data  0.1  24.5  12 
(1)^{2}  (0.25 
(2 
(0.5)^{2}  (0.5)^{2}  (1.5)^{2} 
EIFAsyn Data  (2)^{2}  (3 
(3 
(1)^{2}  (1)^{2}  (2)^{2} 
Computational Cost Comparison.
 

Scenario 1  1,880  241,010  230,840  602,340  404,860  1  128  123  320  215 
Scenario 2  2,010  291,280  283,670  618,840  419,090  1  150  141  307  208 
Scenario 3  2,200  294,480  284,940  618,360  415,080  1  134  130  281  189 
Scenario 5  320  181,060  170,500  388,950  242,810  1  565  533  1,215  759 
Memory Comparison.
EIFAsyn 

620 
PF (STD & MOD)  102; 000  
EnKF (STD & MOD)  250; 000 