Let us consider a bearing sensor, with a limited field of view, moving freely in 2DOF. The sensor state x̂_v is defined by: (1) x ^ v = [ x v , y v , θ v , v x , v y , v θ ] Twhere [x_v, y_v, θ_v] represents the center position and orientation of the sensor and [v_x, v_y, v_θ] denoting linear and angular velocity.

At every step it is assumed an unknown linear and angular acceleration with zero mean and known covariance Gaussian processes, a^W and α^W, producing an impulse of linear and angular velocity: (2) n = [ V x W V y W V θ W ] = [ a x W Δ t a y W Δ t a θ W Δ t ]

The sensor motion prediction model is: (3) f v = [ x v ( k + 1 ) y v ( k + 1 ) θ v ( k + 1 ) v x ( k + 1 ) v y ( k + 1 ) v θ ( k + 1 ) ] = [ x v ( k ) + ( v x ( k ) + V x W ) Δ t y v ( k ) + ( v y ( k ) + V y W ) Δ t θ v ( k ) + ( v θ ( k ) + V θ W ) Δ t v x ( k ) + V x W v y ( k ) + V y W v θ ( k ) + V θ W ]

An Extended Kalman Filter propagates the sensor pose and velocity estimates, as well as feature estimates.

The complete state x̂ that includes the features ŷ is made of: (4) x ^ = [ x ^ v , y ^ 1 , y ^ 2 , ... y ^ n ] Twhere a feature ŷ represents a feature i defined by the 4-dimension state vector: (5) y ^ i = [ x i , y i , θ i , ρ i ] Twhich models a 2-D point located at: (6) [ x i y i ] + 1 ρ i m ( θ i )where x_i, y_i is the sensor center coordinates when the feature was first observed; and θ_i represents the azimuth (respect to the world reference W) for the directional unitary vector m(θ_i). The point depth d_i along the ray is coded by its inverse ρ_i = 1/d_i (Figure 1).

The use of an inverse depth parameterization for bearing-only SLAM can improve the linearity of the measurement equation even for small changes in the sensor position (corresponding to small changes in the parallax angle), this fact allows a Gaussian distribution to cover uncertainty in depth which spans a depth range from nearby to infinity. It is well known the relevance of a good uncertainty Gaussian representation in a scheme based in EKF [24].

Figure 2 shows a simulation for a point reconstruction from noisy bearing measurements at different parallax (upper plot), using both, the Euclidean and the Inverse Depth parameterization. The location of the vehicle is known. A Gaussian error σ_θ = 1° (degrees) is introduced in bearings. Lower plots show the evolution of the likelihood for depth and inverse depth as the parallax in the observation grows: in (a), the estimates of depth likelihood converge to a Gaussian-like shape, but the initial estimates are highly non-Gaussian, with heavy tails. In contrast, likelihoods of inverse depth (b) (abscissa in inverse meters) are nearly Gaussian, even for low parallax. Therefore it can be clearly appreciated how the uncertainty can be represented by a Gaussian using the inverse depth parameterization over whole parallax range, whereas the Euclidean representation converges to a Gaussian-like shape only to the final estimates. For the Euclidean representation, the parallax needed for the likelihood converges to a Gaussian-like shape, depending on the sensor noise, and thus the noisier the sensor the more parallax is needed for convergence.

The different locations of the sensor, along with the location of the already mapped features, are used to predict the feature angle h_i (angle describing the direction of the feature in the sensor coordinate frame). The measurement model is defined by: (7) h i = atan 2 ( 1 ρ i sin   θ i + y i − y v , 1 ρ i cos   θ i + x i − x v )

atan2 is a two-argument function that computes the arctangent of y/x given y and x, within a range of [−π, π]. At this stage it is assumed that the bearing sensor is capable of tracking and discriminating between the landmarks, in other words, the data association problem is obviated.

In implementation using real data, features search could be constrained to regions around the predicted h_i. These regions are defined by the innovation covariance matrix S_i = H_iP_k+1H′_i + R where H_i is the Jacobian of the sensor model with respect to the state, P_k+1 is the prior state covariance, and measurements z are assumed corrupted by zero mean Gaussian noise with covariance R.

As it was stated before, depth information cannot be obtained in a single measurement when bearing sensors are used. To infer the depth of a feature, the sensor must observe it repeatedly as the sensor freely moves through its environment, estimating the angle from the feature to its center. The difference between angle measurements is the feature parallax. Actually, parallax is the key that allows to estimating features depth. In the case of indoor sequences, centimeters are enough to produce parallax, on the other hand, the more distant the feature, the more the sensor has to travel to produce parallax. Therefore, in order to incorporate new features to the map, special techniques for features system-initialization are needed in order to enable the use of bearing sensors in SLAM systems.

Let us consider two methods, which represent the main approaches (undelayed and delayed) for addressing the initialization problem.

For the Inverse depth (ID) undelayed method presented in [22], transition from partially to fully initialized features do not need to be explicitly tackled; this means that the feature is added to the map in its final representation since the first frame was observed. The initialization includes both the feature state initial values and the covariance assignment. The initial uncertainty region covers a huge range depth [d_min, ∞] as Gaussian because the low linearization errors, due to the inverse depth parameterization. Once initialized, the feature is processed with the standard EKF prediction-update loop.

Using the inverse depth parameterization, while the feature is observed at low parallax, the feature will be used mainly to determine the sensor orientation but the feature depth will be kept quite uncertain; if the sensor translation is able to produce a parallax big enough then the feature depth estimation will be improved.

For the ID-Undelayed method (Figure 1), a new feature ŷnew (Equation 5) is initialized, when is detected the first time k, as follows: (8) [ x i y i θ i ] = [ x v y v θ v + z θ ]where x_v, y_v, θ_v are taken directly from the current state x̂_v and z_θ is the initial bearing measurement. The initial value for ρ_i is derived heuristically to cover in its 95% acceptance region, a working space from infinity to a predefined close distance d_min expressed as inverse depth: (9) [ 1 d min , 0 ]   so :    ρ i = ρ min 2   σ ρ = ρ min 4   ρ min = 1 d min

In [22] the parameters are set as d_min = 1, ρ_i = 0.5, σ_ρ = 0.25. The new system state x̂ is conformed simply adding the new feature ŷ_i to the final of the vector state: (10) x ^ = [ x ^ v y ^ 1 y ^ 2 ]       x ^ new = [ x ^ v y ^ 1 y ^ 2 y ^ i ]  

The state covariance after feature initialization is defined by: (11) P new = J [ P ( k ) 0 0 0 R j 0 0 0 σ ρ 2 ] J ′being J the Jacobian for the initialization function.

In experiments using the undelayed initialization, it often happens that the inverse depth becomes negative after a Kalman update, due to the observation noise that predominates over the update of the depth, but there are simple solutions to solve this problem. Moreover, when an initial metric reference is used in order to recover/set the scale of the map (very relevant for robotics applications), initial fixed parameters (inverse depth and uncertainty) must be tuned in order to ensure convergence.

Figure 3 illustrates the SLAM process using the ID-Undelayed following a simple straight trajectory for 5 features, (4 of them near to the vehicle and the other more distant). In upper plots (a,b and c) the initial parameters (ρ_o = 0.01, σ_ρ = 0.005) are set in order to initialize the features at the middle of the distance between the vehicle and the more distant feature, and therefore far enough respect to the nearby features (plot a). In this case we found in almost every case a huge drift in the estimates (plot c), and it can be appreciated that initializing the features far away from the sensor fails if there are some closest landmarks. If other values (ρ_o = 0.5, σ_ρ = 0.25) are used (near to the sensor), (central plots d, e and f) then the percentage of convergence is poor (approx 50%) (plot f), due to the influence of the distant feature. In this case if the distant feature is removed from the map (not illustrated here) then a percentage of convergence of 80% is achieved. In the last series (lower plots g, h and i) we combine both initial values: ρ_o = 0.01, σ_ρ = 0.005 for the distant landmark and ρ_o = 0.5, σ_ρ = 0.25 for the all the nearby ones. In this case an effectiveness of 90% was achieved for the algorithm.

The issues mentioned above suggest us that the initial inverse depth and their associated initial uncertainty of the new features added to the map could be treated before to be added to the system state instead of use fixed initial depth and uncertainty. In [23] a delayed version of an undelayed method is proposed. In this case, initial depth and uncertainty of each feature are dynamically estimated prior to adding the new landmark in the stochastic map.

For the ID-Delayed method, a new feature ŷ_new (Equation 5) is initialized as follows:

When a feature is detected the first time k, some part of the current state x̂ and covariance matrix P together with the sensor measurement are stored, this data λ (called candidate points) is composed by: (12) λ i = [ x 1 , y 1 , θ 1 , z 1 , σ 1 x , σ 1 y , σ 1 θ ]

The values x₁, y₁ and θ₁ represent the current robot position; σ₁^x, σ₁^y and σ₁^θ represent their associated variances taken from the state covariance matrix P_k and z₁ is the first bearing measurement to the landmark. In subsequent instants k, the feature is tracked until a minimum parallax threshold α_min is reached. Figure 4 shows that a few degrees of parallax are enough to reduce the uncertainty in the estimation.

The parallax α is estimated using:

The base-line b.

For each candidate point λ_i, every time that a new bearing measurement z is available, the parallax angle α can be estimated as (Figure 4): (13) α   =   π −   ( β   +   γ )

The angle β is determined by the directional unitary vector h₁ and the vector b₁ defines the base-line b in the direction of the sensor trajectory.

The angle γ is determined in a similar way as β but using the directional unitary vector h₂ and the vector b₂ defining the base line in the opposite direction of the sensor trajectory by: (14) β = cos − 1 ( h 1 ⋅ b 1 ‖ h 1 ‖ ‖ b 1 ‖ )          γ = cos − 1 ( h 2 ⋅ b 2 ‖ h 2 ‖ ‖ b 2 ‖ )where (h₁ · b₁) is the dot product between h₁ and b₁. The directional vector h₁, expressed in the absolute frame W, points from the sensor location to the direction when the landmark was observed for the first time, and is computed using the data stored in λ_i denoting the bearing z_i. The directional vector h₂ expressed in the absolute frame W is computed in a similar way as h₁ but using the current sensor position x̂_v and the current measurement z_i. (15) h 1 = [ cos   ( θ 1 + z 1 ) sin   ( θ 1 + z 1 ) ]      h 2 = [ cos   ( θ v + z i ) sin   ( θ v + z i ) ]

b₁ is the vector representing the robot base-line between the robot center position x₁, y₁ stored in λ_i where the point was first detected and the current sensor center (x_v, y_v). b₂ is equal to b₁ but pointing to the opposite direction. The base-line b is the module of b₂ or b₁: (16) b = ‖ b 1 ‖ = ‖ b 2 ‖      b 1 = [ ( x v − x 1 ) , ( y v − y 1 ) ]    b 2 = [ ( x 1 − x v ) , ( y 1 − y v ) ]

If α > α_min then λ_i is initialized as a new feature ŷ_i. The threshold α_min can be established depending on the accuracy of the bearing sensor. Depth uncertainty is reasonably well minimized when α = 10°.

For a new feature ŷ_i, values of x_i, y_i, θ_i are defined in the same way as Equation 8. For the delayed approach the dynamical estimation of ρ_i is derived from: (17) ρ i = sin   α b   sin   β

The variance σ_ρ for the inverse depth ρ is calculated now from the initialization process, instead of a variance predefined heuristically as it was made in the undelayed method, therefore the covariance for the new feature ŷ_new is derived from the error diagonal covariance matrix R_i measurement and the state covariance matrix P. (18) R i = diag [ σ 1 x , σ 1 y , σ 1 θ , σ z 2 , σ z 2 ]

For reasons of simplicity R_i is defined as a diagonal matrix (cross-covariances are not taken into account) and is now conformed by the error variance of the standard deviation of the bearing sensor σ_z (one for each bearing estimation z₁ and z) and the variances stored in λ_i (σ₁^x, σ₁^y and σ₁^θ). Note that the value of σ_z is constant and is not stored previously in Equation 12.

The new state covariance matrix, after initialization, is: (19) P new = J [ P k 0 0 R j ] J ′

Note that unlike the ID-Undelayed method there is not an implicit initial uncertainty in depth σ_ρ (Equation 11). In the ID-Delayed method the complete covariance for the new feature is fully estimated by the initialization process.

In an Undelayed approach, when a feature is added to the map the first time that it has been observed, its depth is modeled with a huge uncertainty. In that sense, this new feature does not provide any depth information. However, at this stage the benefit of the Undelayed approach is that features provide information about the sensor orientation from the beginning.

On the other hand, it can be useful to wait until the sensor movement produces some degrees of parallax, (gathering depth information) in order to improve robustness, especially when an initial metric reference is used for recovering scale. Moreover, when cameras are used in real cluttered environments, the delay can be used for efficiently reject weak features, thus initializing only the best features as new landmarks to the map.

When a feature is detected for the first time k, it is initialized immediately in the map as a new landmark ŷ_L(i) which is composed by the 3-dimension state vector: (20) y ^ L ( i ) = [ x i , y i , θ i ] Twhere (21) [ x i y i θ i ] = [ x v y v θ v + z θ ]

x_v, y_v, θ_v are taken directly from the current state x̂_v and z_θ is the initial bearing measurement. ŷ_L(i) defines a directional vector, expressed in the absolute frame W, which represents the direction of the landmark from the sensor, when it was observed for the first time. The covariance matrix P is updated in the same manner as Equation 19 but using the proper Jacobian J.

Parallel to the system state (represented by the state vector x̂), a state vector x̂_can is used for estimating (via an extra linear Kalman Filter) the feature depth of each landmark ŷ_L. The state x̂_can is not directly correlated with the map.

Every time a new feature ŷ_L(i) is initialized in x̂, the state x̂_can is augmented as: (22) x ^ can = [ λ 1 λ 2 ]       x ^ can _ new = [ λ 1 λ 2 λ i ]  where λ_i is a 3-dimension vector composed by: (23) λ i = [ α i , Δ α i , ρ i ] ′

For each λ_i, α_i is the estimated parallax, Δα_i is the rate of change in parallax and ρ_i is the estimated inverse depth.

The covariance matrix of x̂_can, P_can, is augmented simply by: (24) P can _ new = [ P can 0 0 R c ]

The three initial values of λ_i are set to zero, and the initial values of R_c have been heuristically determined as: R_c = diag(.01, .01, 1).

While the sensor moves through its environment, it can observe repeatedly a landmark ŷ_L(i), at each iteration generating a new angle measurement z. All these new measurements are successively added to the linear Kalman Filter (responsible for estimating x̂_can) in order to infer the landmark depth. For each new measurement z_i of a feature ŷ_L(i) an iteration of the filter is executed.

The state transition model for each λ_i is: (25)   x ^ can _ i ( k + 1 ) = [ α i ( k + 1 ) Δ α i ( k + 1 ) ρ i ( k + 1 ) ] = [ α i ( k ) + Δ α i ( k ) Δ α i ( k ) ρ i ( k ) ]

A process noise w_k ∼ N(0,Q_k) is considered. In experiments: Q_k = diag(8e⁻⁷, 10e⁻⁹) have been used.

The measurement prediction model is directly obtained from the state. On the other hand, the measurements z_can used to update the filter are a function of: (i) the feature ŷ_L(i), (ii) the sensor state x̂_v and (iii) the current measurement z_i. (26)   z can = [ z α z ρ ] = f z ( y ^ L ( i ) , x ^ v , z i )

z_α and z_ρ are estimated in the same manner as Equation 13 and Equation 17 respectively. Only note that in Equation 16, x₁ and y₁ are taken from ŷ_L(i), so x₁ = x_i and y₁ = y_i.

The implicit uncertainties in the estimation of the function f_z are used to compose the error measurement covariance matrix R_can: (27) R can = ∇ f z ( P t ) ∇ f z ′where Δf_z is the Jacobian of f_z with respect to z_can. P_t is formed by: (28) P t = [ P x ^ v P x ^ v y ^ L ( i ) 0 P y ^ L ( i ) x ^ v P y ^ L ( i ) 0 0 0 σ z 2 ]

All the components of P_t, except σ_z (the error variance of the bearing sensor) are taken directly from the covariance matrix P of the system state x̂. P_x̂v is the submatrix of P corresponding to the covariance of the sensor state x̂_v. P_ŷL(i) is the covariance of the feature ŷ_L(i). P_x̂vŷL(i) and P_ŷL(i)x̂v are the correlations between x̂_v and ŷ_L(i).

R_can is used in the Kalman update equations for estimating the innovation covariance matrix S_i.

Features expressed in the form of ŷ_L(i) are very useful to estimate the sensor orientation θ_v. In [25] a visual compass is proposed based in this fact. Besides, the depth of features are needed for estimating the sensor location [x_v,y_v]. For near features ŷ_L(i), a small sensor translation is enough to produce some parallax and thus to infer depth.

The state x̂_can encloses the parallax α_i and inverse depth ρ_i estimations for each feature ŷ_L(i). Figure 6 shows the evolution of parameters α_i (upper plot) and d = 1/ρ_i (lower plot) and its uncertainty for the feature estimated by the linear Kalman filter, (left plot) feature at a distance of d = 1,000 units and (right plot) for a distance of d = 50 units. The boundary uncertainties at 3σ are indicated in blue color. The filtered values are depicted in red color. Also note in green color the raw measurements z_can (taken from Equation 26). In these graphics it can be clearly appreciated how the estimation of depth d is directly influenced by the parallax; for the near feature, about 100 steps are needed to producing parallax and thus d converges rapidly to its real value. Also note that the uncertainty is rapidly minimized. On the other hand, the distant feature produces small parallax. For a low parallax the sensor noise predominates and therefore produces very fluctuant raw measurements. Even so, after several steps, the filter estimates the depth reasonable well with a high related uncertainty.

A minimum parallax threshold α_min is used for updating a feature ŷ_L(i) as ŷ_i. Distant features will not produce parallax and therefore will remain expressed as ŷ_L(i), but contributing to the estimation of sensor orientation θ_v.

On the other hand, if α_i > α_min then: (29) y ^ L ( i ) = [ x i , y i , θ i ] T → y ^ i = [ x i , y i , θ i , ρ i ] Twhere ρ_i is taken directly from x̂_can. The covariance matrix P is transformed by the corresponding Jacobian: (30) P new = J [ P 0 0 q σ ρ i 2 ] J ′being σ_ρ^y the variance of the inverse depth estimation for ŷ_L(i) and taken from P_can. The constant q is used to increase the initial uncertainty of ρ_i in order to improve filter consistency. In experiments q is set to 100. (31) J = [ I 0 0 0 0 ∂ y ^ i ∂ y ^ L ( i ) 0 1 0 0 I 0 ]

When a feature ŷ_L(i) is updated as ŷ_i, then its corresponding values will be removed from the linear Kalman filter responsible for estimating the state x̂_can.

At any time, the map can include both kind of features ŷ_L(i) and ŷ_i. Thus each kind of feature has its own measurement prediction model:

- For features ŷ_i measurement Equation 7 is used. A value of 2 to 6 times the real error of the bearing sensor is considered for the measurement process.

Both kinds of measurement prediction models must be used together, if several measurements z_i are made at the same time for both kinds of features. For example, consider that the bearing sensor takes measurements of the features ŷ_L(2) and ŷ₃ simultaneously, then: (33) v = [ z 2 z 3 ] − [ h L ( 2 ) h 3 ]      R = [ c σ z 2 0 0 σ z 2 ] (34) ∇ H = [ ∂ h L ( 2 ) ∂ x ^ v 0 ∂ h L ( 2 ) ∂ y ^ L ( 2 ) 0 0 ∂ h 3 ∂ x ^ v 0 0 ∂ h 3 ∂ y ^ 3 0 ]where v is the innovation, R is the error covariance matrix and ∇H is the Jacobian measurement model.

Figure 7 shows the evolution in the initialization process of two features maps with the concurrent initialization: a distant one (600 units of depth) and a near one (50 units of depth). The only information given a priori to the system was the scale reference (the three points in yellow) which was introduced with an associated uncertainty close to zero in the covariance matrix R. Taking into account that there is not an additional sensory input (e.g., odometry), at every step an unknown linear and angular acceleration is introduced with zero mean and known-covariance Gaussian processes (Section 3.1). In this case, a^W_x = 4 m/s², a^W_y = 4 m/s² and a^W_θ = 2 rad/s² were used. The only input sensor of the system is a noisy sensor capable of measuring the bearing of features, with a limited field of view of 110° (emulating a 2-DOF camera). A standard deviation of σ = 1° is considered for the sensor readings.

At the begin of the sequence (plot a) both features has been initialized as a directional vector ŷ_L(i), defining a ray (in cyan color). Note that three feature points (in yellow color) have been previously added to the map as a priori known landmarks in order to define/set the scale of the world. Around step 100 (plot b), the small displacement of the sensor to the right produces enough parallax to estimating the depth of the near feature. Observe that the near feature ŷ_L(i) has been transformed to a ŷ_i feature (blue color). Also note that some uncertainty (especially in depth) remains at the moment of the transformation. By the last step, at 250 iterations, (plot c) the uncertainty in the near feature has been full minimized, on the other hand, the movement of the sensor has not produced enough parallax and therefore, the distant feature remains in the form of ŷ_L(i) but still contributing to the estimation of the sensor orientation.

In order to show the performance of the Concurrent method proposed in this article, a comparative study between the ID-Undelayed method [22] and the ID-Delayed [23] method is presented.

Figure 8 illustrates the environment setup used in the study. For all the tests, the bearing sensor is moved over a semi-cycled U-like shape trajectory, since our main goal is to observe the effect of the initialization process of new features in the estimation of both map and sensor location, instead of the closing loop problem. About 100 landmarks (in green) simulate the environment of the sensor. Figure 8 also shows both the map and sensor trajectory estimates after a single run of 2,000 steps of the Concurrent method. The features map and their uncertainties are indicated in blue. Note the evolution of uncertainty in both sensor and features location. Also note the typical (in SLAM systems) drift in both trajectory and map estimations as the sensor moves far away from its initial location.

The average NEES (normalised estimation error squared [26]) over N Monte Carlo runs of the filter was used in order to evaluate the consistency of the methods, as it is proposed in [27]. The NEES is estimated as follows: (35) ε k = [ x k − x ^ k | k ] T P k | k − 1 [ x k − x ^ k | k ]where x_k is the true state and the average NEES is computed as: (36) ε ¯ k = 1 N ∑ i = 1 N ε k ( i )

Four different tests were realized to comparing the methods under diverse conditions. Table 2 shows the values for the linear and angular acceleration (a^W_x, a^W_y and a^W_θ in 4m/s²) and the time between “frames” (Δt in seconds) used for each test. In this case, a higher Δt implies bigger displacements of the robot from frame to frame and therefore more linearization errors due to large changes in parallax.

In experiments ρ_ini = 0.05 and σ_ρ = 0.025 were used for the ID-Undelayed method and α_min = 10° were used for both ID-Delayed and Concurrent methods. The NEES was estimated over the 3-dimensional robot pose. The average NEES for each method was estimated using N=20 Monte Carlo runs. In simulations the sensor was moved 3 meters every Δt = 1 second for the straight sections and 4.5 meters every Δt = 1 seconds for the curve sections of the trajectory.

MATLAB code was run using a 1.73 GHz Pentium M laptop. Table 3 shows the execution time of the three methods for conditions Δt = 1/30 and Δt = 1/120. As it would be expected, for Δt = 1/120 the execution time was around four times longer than Δt = 1/30, for the same trajectory. Execution time of the Concurrent method was estimated using a diagnostic version of the algorithm, marked with an asterisk (*) in Table 3. This version uses a 5n Kalman filter (instead of 3n) which estimates two extra diagnostic parameters. This means that the real execution time of the Concurrent method should be somewhat faster.

For some runs of the algorithms, filter divergence could occur. Table 3 also shows the failed attempts, this means the number of times that filter divergence appeared before a method reach N = 20 positives runs (with convergence), for each test. The average NEES was only estimated using positive runs.

Figure 9 shows the evolution of the average NEES (ε̄_k) for tests (a), (b), (c), (d) for each method. Note that test (b) and (d) need four times more steps than test (a) and (c) in order to complete the same trajectory due to their particular value of Δt.

As it would be expected, the whole methods become optimistic after a certain time [27]. Nevertheless, it can be observed for tests (a), (b) and (c) that ε̄_k achieved for Concurrent method (red) tends to be lower than ε̄ _k achieved for both ID-Undelayed (blue) and ID-Delayed (green) methods. This behavior is more evident in test (a) and (c), where lower noise is injected into the sensor motion model. The difference between (a) and (c) is the time between “frames” (Δt) used for each test. The above results suggest, that if adequate noise is injected, then Concurrent method seems to be less sensitive to large jumps in parallax (and linearization errors), and therefore could be suitable for application where a high “frame-rate” is not available. Regarding to the ID-Undelayed method, it can be observed the effect of the parameter Δt over the magnitude of ε̄_k, although its form is somewhat similar for all tests. In test (a) and (c) where Δt is higher (1/30), ε̄_k reaches around 600 units, while in test (b) and (d) where Δt is lower (1/120) ε̄_k reaches around 200 units. Theoretically the ε̄_k for the ID-Undelayed method would be more favorable as Δt→0. At this point, it is important to remember that implementation of Concurrent method implies the estimation of an extra linear Kalman filter of dimension 3n, where n is the number of features ŷ_L(i) in the state x̂. On the other hand, the Concurrent method is lees sensitive to the parameter Δt. At least for this experimental setup, the average NEES for the Concurrent method, when Δt = 1/30 (94 s of execution time), is even lower than the ε̄_k for the ID-Undelayed, when Δt = 1/120 (302 s of execution time). The ID-Delayed method represents the most efficient alternative in computational cost with medium performance in average NEES terms. However the ID-Delayed method shows to be the least robust method in terms of convergence (Table 3). The problem of convergence for the ID-Delayed method is notorious in tests (a) and (c) where Δt is higher (1/30), these results indicate a frame-rate dependency of the method. On the other hand, for the realized tests, the Concurrent method shows the best results in terms of convergence.