The SLAM algorithm with feature selection based on the observation of the entropy of the measurements was previously presented by [21]. This algorithm is considered as related to the proposal herein. This method is based on the calculation of the entropy attached to each observed feature. If the entropy is below a certain threshold value, then the observation will be computed in the correction stage of the EKF.

Considering that all variables involved in the EKF-SLAM estimation process are Gaussian random variables, the entropy value associated with a single observation can be represented as it is shown in Equation (2). (2) ∑ t = E − ln p ( ξ t | z i ) = 0.5 ln [ ( 2 π e ) n | P t | ]In Equation (2), ∑ is the entropy of the observation z. The a priori and posteriori information metric can be defined as the inverse of the entropy value, shown in Equation (2). Thus, (3) im t = − ∑ t = − 0.5 ln [ ( 2 π e ) n | P t | ] (4) im t − = − ∑ t − = − 0.5 ln [ ( 2 π e ) n | P t − | ]The information difference can be calculated as in Equation (5), where the absolute incremental information is obtained. (5) Δ i = im t − im t −Thus, when the absolute information of a feature exceeds a certain threshold (δ), that feature will be used in the correction stage of the EKF-SLAM algorithm. The algorithm of the EKF-SLAM with feature selection based on the entropy is summarized in Algorithm 2.

As Algorithm 2 shows, the calculation of the entropy associated with a single observation—and its information metric—is related to the determinant of the complete covariance matrix of the SLAM system state. Thus, the complexity of the calculation of the entropy is O(n²), where n is the dimension of the SLAM system state. Since this dimension varies, the complexity of the algorithm varies as well. Although this algorithm has the advantage of restricting the number of features to be updated, the calculation of the entropy requires the calculation of the determinant of the SLAM system state covariance matrix (P_t), which in fact increases the processing time of the EKF-SLAM algorithm. Further details on this approach can be found in [21].

The covariance ratio approach as feature selection criterion in the EKF-SLAM algorithm was formerly published by the authors in [23]. This approach is based on the evaluation of the influence of a given feature—with correct association—in the convergence of the covariance matrix of the SLAM system state. The correction of the covariance matrix of the SLAM system state can be expressed as: P t = ( I − K t H t ) P t − = P t − − K t H t P t −

Applying the determinant to both sides of the expression above, it follows: (6) | P t | = | I − K t H t | | P t − | = | P t − − K t H t P t − |

Considering that in Equation (6), P t − is pd (positive definite) and K t H t P t − is psd (positive semi-definite) matrices, the above expression leads to Equation (7). (7) | P t | = | I − K t H t | | P t − | = | P t − − K t H t P t − | ≤ | P t − |

Then, from Equations (6) and (7): (8) 0 ≤ | P t | | P t − | = | I − K t H t | ≤ 1 ;     with | P t − | ≠ 0

Equation (8) defines the covariance ratio of the SLAM algorithm. In this case, the ratio is used as a measure of the volume of the uncertainty ellipse associated with the covariance matrix of the SLAM system state [24].

Another convergence property states that, at the limit, all elements of P_t become fully correlated [24]. This last statement is equivalent to say that, (9) lim t → ∞ | P t | = 0

Thus, according to Equations (8) and (9), given a set of observed features with correct matching, the feature that causes the highest decrease of |P_t|, is the feature to which the EKF-SLAM is more sensitive to and will cause the fastest convergence of Equation (9). This latter point can be regarded as an optimization problem. Let N_t be the set of observed features at time t; let M_t ⊆ N_t be set of features with correct association. Then ∀z ∈ M_t ⊆ N_t: (10) z opt : arg z min ( | P t | ) ≡ arg z min ( | I − K t H t | )

Thus, according to Equation (10), finding the observation z that minimizes |I – K_tH_t| is equivalent to finding the observed feature that causes the highest decrease of |P_t| because | P t − | is independent of the current observation.

Considering that the EKF-SLAM implemented in this work is a sequential algorithm [3], the Jacobian of the observation model has the sparse form shown in Equation (11), where H_v,t is the Jacobian of the observation model with respect to the vehicle’s degrees of freedom and H_z,t is the Jacobian of the observation model with respect to the parameters of the observed feature. Θ₁ and Θ₂ are null matrices. The Kalman—Equation (12)—gain is also defined according to Equation (11). (11) H t = [ H v , t Θ 1 H z , t Θ 2 ] (12) K t T = [ K v , t T K Θ 1 T K z , t T K Θ 2 T ]

Thus, the Jacobian of the observation is only calculated on the Jacobian entries that correspond to the vehicle and to the feature with correct association [1,2]. By using Equations (11) and (12), the determinant of |I – K_tH_t| can be calculated as: (13) | I − K t H t | = = | [ I v I Θ 1 I z I Θ 2 ] − [ K v , t K Θ 1 K z , t K Θ 2 ] [ H v , t Θ 1 H z , t Θ 2 ] | = | I v − K v , t H v , t Θ − K v , t H z , t Θ − K Θ 1 H v , t I Θ 1 − K Θ 1 H z , t Θ − k z , t H z , t Θ I z − K z , t H z , t Θ − K Θ 2 H v , t Θ − K Θ 2 H z , t I Θ 2 | = | I v − K v , t H v , t − K v , t H z , t − K z , t H v , t I z − K z , t H z , t |

In Equation (13)I is the identity matrix, I_v, I_Θ1, I_z and I_Θ2 are identity block matrices with the dimensions of K_v,tH_v,t, K_Θ1 Θ₁, K_z,tH_z,t and K_Θ2 Θ₂ respectively. If we consider that the vehicle has three degrees of freedom—two related to the position and one to the orientation—and the feature is determined by two parameters, then the final calculation of Equation (13) is a 5 × 5 matrix.

The correction stage of the EKF-SLAM algorithm with the feature selection based on the covariance ratio is presented in Algorithm 3.

In Algorithm 3, sentences (1) − (2) are the declaration of the domain that is going to be used in the correction stage; sentence (3) determines—if possible—the maximum number of features that will be used for correcting the SLAM. If the number of features in M_t is smaller than LIM, then the complete set of features in M_t will be used in the correction loop. Sentences (4) − (9) show the for-loop of the correction stage. Given M_t, the algorithm searches for a first z^opt. When it is found, the correction takes place—(6) to (8)—and this features is removed from M_t. In the second iteration of the for-loop, the z^opt is searched inside the new M_t and both the actual predicted system state and covariance are the last corrected system state and covariance matrix as shown in sentence (10). This situation ensures that sequentiality of the EKF-SLAM is not lost.

In this approach, instead of selecting the features according to the determinant of Equation (13), the eigenvalues associated with it will be used.

By inspection, it is possible to see that if an eigenvalue of Equation (13) tends to zero faster than the others, then that eigenvalue will dominate the convergence of |P_t|—see Equation (9). Thus, the eigenvalues approaches presented herein give a better description of the behavior of the set of eigenvalues associated with (I − K_tH_t) in Equation (13), because they consider the behavior of all eigenvalues.

Let us calculate the eigenvalues of Equation (13)—Eig(I − K_tH_t). Applying the definition of eigenvalues we have that: (14) | ( I − K t H t ) − λ I | = | A 1 , 1 Θ A 1 , 3 Θ A 2 , 1 A 2 , 2 A 2 , 3 Θ A 3 , 1 Θ A 3 , 3 Θ A 4 , 1 Θ A 4 , 3 A 4 , 4 |with, A 1 , 1 = I v − K v , t H v , t − λ I v A 1 , 3 = − K v , t H z , t A 2 , 1 = − K Θ 1 H v , t A 2 , 2 = I Θ 1 − λ I Θ 1 A 2 , 3 = − K Θ 1 H z , t A 3 , 1 = − K z , t H z , t A 3 , 3 = I z − K z , t H z , t − λ I z A 4 , 1 = − K Θ 2 H v , t A 4 , 3 = − K Θ 2 H z , t A 4 , 4 = I Θ 2 − λ I Θ 2 Θ   is a null matrix with the appropriate dimensions

By inspection of Equation (14) and considering that Θ is a null matrix with the appropriate dimension, it is possible to see that, (15) | ( I − K t H t ) − λ I | = ( 1 − λ ) r | I v − K v , t H v , t − λ I v − K v , t H z , t − K z , t H v , t I z − K z , t H z , t − λ I z |

Thus, the only eigenvalues of (I − K_tH_t) affected by the current feature z_i are the eigenvalues of [ I v − K v , t H v , t − K v , t H z , t − K z , t H v , t I z − K z , t H z , t ]. The rest of the eigenvalues equal one—see Equation (15). Considering that the pose of the robot has three degrees of freedom—two associated with the position and one with the orientation—and the feature has 2 parameters that define it, then the calculation of the eigenvalues of (I − K_tH_t)—which is an n × n matrix—is reduced to the calculation of a 5 × 5 matrix.

In this section, two eigenvalues approaches are presented for selecting features. The first approach consists on choosing the features according with the sum of eigenvalues of Equation (15). Thus, from the set M_t of features with appropriate association, only the feature with the minimum sum of its eigenvalues will be selected.

The other approach is to select the features based on the lowest value of the highest eigenvalue. Equation (16) shows the selection criterion based on the sum of eigenvalues whereas Equation (17) shows the selection criterion based on the value of the highest eigenvalue associated with a feature. (16) ∀ z i ∈ M t , z opt ≡ arg z min ( | P t | ) ≡ arg z min ( ∑ Eig ( [ I v − K v , t H v , t − K v , t H z , t − K z , t H v , t I z − K z , t H z , t ] ) ) (17) ∀ z i ∈ M t , z opt ≡ arg z min ( | P t | ) ≡ arg z min ( MAX   Eig ( [ I v − K v , t H v , t − K v , t H z , t − K z , t H v , t I z − K z , t H z , t ] ) )

Thus, in Equation (16), the feature selected has the minimum sum of eigenvalues; in Equation (17), the feature selected is the one which has the smallest maximum eigenvalue. The last is based on that if the higher eigenvalue decreases, also decrease (or remain equal) the rest of the eigenvalues. Thus, this method allows a selection of features based on the behavior of the eigenvalues. Further information about the sum of eigenvalues method can be found in [20].

Algorithm 4 shows the general structure of the selection procedure. Sentence (5) can be chose according to Equations (16) or (17).

Up to now, the feature selection approaches presented are based on the covariance matrix of the SLAM system state: P t = ( I − K t H t ) P t −. Due to the fact of possible lost of positive definiteness of P_t during the numerical computation, the Joseph’s form of the covariance matrix of the SLAM system state within an EKF-SLAM is widely used by the scientific community [17]. The Joseph’s form is shown in Equation (18). (18) P t = ( I − K t H t ) P t − ( I − K t H t ) T + K t R t K t T

In Equation (18)R_t is the covariance matrix of the observation. As it can be noted, the expression above corresponds to an n × n matrix, where n is the order of the SLAM system state.

In order to reduce the computational cost by applying any selection criterion previously presented with Equation (18) instead of Equation (1) some calculations are needed.

Thus, considering that ( I − K − t H t ) P t − ( I − K t H t ) T and K t R t K t T in Equation (18) are psd the two following conditions hold. (19) { | P t | ≥ | ( I − K t H t ) P t − ( I − K t H t ) − | = | I − K t H t | 2 | P t | | P t | ≥ | K t R t K t T |

In Equation (19), | P t − | is a constant value because P t − is independent of the current feature; in addition, in Equation (19) |I − K_tH_t|² ≤ |I − K_tH_t| ≤ 1 according to Equation (8).

The calculation of |I − K_tH_t| applies as was shown in Equation (13). On the other hand, considering that K t R t K t T = P t − H t T Ψ t − 1 R t Ψ t − T H t P t − T, with Ψ t = H t P t − H t T + R t, then, it follows that | K t R t K t T | = | P t − | 2 | H t T Ψ t − 1 R t Ψ t − T H t |. Replacing M t = Ψ t − 1 R t Ψ t − T, where M has the dimensions of R_t −2 × 2 matrix, then, | K t R t K t T | = | P t − | 2 | H t T M t H t |

Considering also that the EKF-SLAM used in this work is a sequential EKF, the above expression can be written as it is shown in Equation (20). (20) H t T M t H t = = [ H v , t T Θ 1 T H z , t T Θ 2 T ] T M t [ H v , t Θ 1 H z , t Θ 2 ] = [ H v , t T M t H v , t Θ H v , t T M t H z , t Θ Θ Θ Θ Θ H z , t T M t H v , t Θ H z , t T M t H z , t Θ Θ Θ Θ Θ ]

In Equation (20), Θ means a null block matrix with the appropriate dimensions. By inspection of Equation (20) is possible to see that | P t − 2 | | H t T M t H t | = | K t R t K t T | = 0 which leads to the following expressions. (21) | P t − | | I − K t H t | 2 ≤ | P t | ≤ | P t − | | I − K t H t | 2 ≤ | P t | | P t − | ≤ 1 ;     with | P t − | > 0

Thus, as it can be seen in Equation (21), smaller the determinant of |I – K_tH_t|, smaller the value that |P_t| could adopt. Re-writing Equation (21) follows that, (22) 1 ≤ | P t − | | P t | ≤ 1 | I − K t H t | 2

Equation (22) implies that the smaller |I − K_tH_t| the bigger the inverse of the covariance ratio presented in Equation (8). Concluding, for a covariance matrix of the EKF-SLAM system state corrected according to the Joseph’s form, the feature selection criterion is the same as the one presented in Equation (10): z^opt : arg_zmin(|P_t|) ≡ arg_zmin(|I − K_tH_t|). From this last statement is possible to see that the feature selection approaches presented in Sections 4.2 and 4.3 apply in the same way when the Joseph’s form of the covariance correction is used.

The features’ covariance approach is based on the following assumption. Let us suppose that at a time instant t, the mobile robot extracts five features from the environment (⧣N_t = 5 in Algorithm 1). From the set of five features extracted, only two of them have appropriate association with the predicted features from the SLAM system state (⧣M_t = 2 < ⧣N_t in Algorithm 1). Figure 1 shows this situation. Each of these two features has associated a covariance matrix (R₁ and R₂ respectively) which intrinsically depends on the feature extraction procedure. Also, R₁ and R₂ are positive definite matrices. The features’ covariance approach consists in choosing the feature with the smallest covariance matrix with respect to the covariance matrices of the rest of features with appropriate association. Thus, for example, in Figure 1, if R₁ is smaller than R₂—(R₂ − R₁) is positive semi-definite, then Feature 1 will be used within the correction stage of the EKF-SLAM instead of Feature 2. Although it seems intuitive to choose R₁—because of its smaller covariance, the following theorem is the corresponding mathematical justification of the features’ covariance approach criterion.

Theorem—Let R₁ and R₂ be two symmetric positive definite covariance matrices associated with two features from the environment with correct association—as it is shown in Figure 1. Also, let R₂ ≽ R₁ (≽ stands for positive semi-definite, therefore, R₂ − R₁ ≽ 0), then, (23) | P t R 1 | ≤ | P t R 2 | . −

The theorem above establishes that the feature with associated covariance matrix R₁ will cause the highest decrement of the uncertainty volume of the covariance matrix of the EKF-SLAM, |P_t|, when compared with R₂—see Equation (9).

Proof—By hypothesis, we have that: (24) R 1   ≼   R 2

Considering that H t P t − H t T in Equation (1) is positive semi-definite, then the above relation does not change if we add H t P t − H t T on both members of Equation (24). (25) H t P t − H t T + R 1   ≼   H t P t − H t T + R 2

Given that R₁ and R₂ are positive definite (by hypothesis), then the matrix H t P t − H t T + R i, with i = 1, 2, is also positive definite. Therefore, its inverse exists. Then, (26) ( H t P t − H t T + R 1 ) − 1 ≽ ( H t P t − H t T + R 2 ) − 1

Equation (26) shows that, after applying the inverse at both sides of Equation (25) the positive definition between matrices changes [25]. Rewriting Equation (26) and pre-multiplying by P t − H t T and post-multiplying by H t P t − respectively—observe that P t − H t T = ( H t P t − ) T—we have that, (27) P t − H t T ( ( H t P t − H t T + R 1 ) − 1 − ( H t P t − H t T + R 1 + R 2 ) − 1 ) H t P t −   ≽   0 P t − H t T ( H t P t − H t T + R 1 ) − 1 H t P t − − P t − H t T ( H t P t − H t T + R 2 ) − 1 H t P t −   ≽   0

Observe that the matrices definition between Equations (26) and (27) is not lost (see [25]). Finally, adding and subtracting P t − in Equation (27) we have the following result. (28) ( P t − − P t − H t T ( H t P t − H t T + R 2 ) − 1 H t P t − ) − ( P t − − P t − H t T ( H t P t − H t T + R 1 ) − 1 H t P t − )   ≽   0

Thus, according to Equations (1, 28) implies that, P t R 2 − P t R 1   ≽   0where P t R i is the covariance matrix of the SLAM algorithm correction stage if the feature with covariance matrix R_i were used (i = 1, 2).

Considering that both P t R 2 and P t R 1 are positive definite and P t R 2 − P t R 1   ≽   0, then, according to [25], | P t R 2 | ≥ | P t R 1 |. Therefore, having into account Equation (9), we can conclude that the feature that has the covariance matrix R₁ associated with it will cause the highest decrement in the determinant of the covariance matrix of the SLAM system state correction stage. Then, that feature is the most meaningful from the convergence perspective of the SLAM algorithm.

The Algorithm 5 presents the Features’ Covariance approach for selecting the most meaningful features. The code line (6) shows the implementation of the selection criterion based on the covariance matrix of the features with correct association. In the case that a z^opt is not found, then the correction of the EKF-SLAM is performed based on the detected features with correct association. Thus, i.e., if LIM = 2 and ⧣M_t ≥ 2 in the Algorithm 5 and no z^opt exists, then the correction is performed with any two features from M_t—code line (2) and (3).

Figure 4 shows two maps representation of the environment. Figure 4(a) shows the map reconstruction when there is no restriction on the number of features to be used during the update stage of the EKF-SLAM; on the other hand, Figure 4(b) shows the map reconstruction when the sequential EKF-SLAM updating stage is restricted to the two first detected features; Figure 4(c) shows a zoom of Figure 4(a) where it can be seen that each feature has associated a covariance ellipse. The features of the environment are represented by blue triangles; the path traveled by the robot is a solid black line and the path estimated by the SLAM is a solid magenta line.

Figure 5 shows the map reconstruction of the environment based on the information provided by EKF-SLAM algorithm with entropy feature selection criterion shown in Algorithm 2. Figure 5(a) shows the map reconstruction when gate of the information difference was set to δ = 0.2; on the other hand, Figure 5(b) shows the map reconstruction for a gate of the information difference of δ = 0.4 (see the Algorithm 2). As it can be seen, the map reconstruction in Figure 5(b) is less precise than the one shown in Figure 5(a).

The EKF-SLAM results based on the covariance ratio approach shown in Algorithm 3 are shown in Figure 6. In Figure 6(a), LIM—the maximum number of features to be selected—is set to LIM = 5. The last means that only the five most significant features from the covariance ratio approach point of view will be selected for the correction stage of the EKF-SLAM. Figure 6(b) shows the case when LIM = 2.

As it can be seen, the map constructed by the EKF-SLAM with the feature selection based on the covariance ratio approach (see Figure 6) is more similar to the one shown in Figure 4(a) than the map constructed by the EKF-SLAM with the entropy feature selection criterion (see Figure 5).

Figure 7 shows the map reconstruction based on the sum of eigenvalues feature selection criterion (shown in Algorithm 4). Figure 7(a) shows the case when LIM = 5 (the five most significant features are used in the correction stage of the EKF-SLAM); on the hand, in Figure 7(b), LIM = 2.

As it can be seen, Figure 7(a) is very similar to Figure 4(a).

Figure 8 shows the map reconstruction based on the EKF-SLAM with the maximum eigenvalue feature selection criterion presented en Section 4.3. Figure 8(a) shows the case when LIM = 5 and Figure 8(b) shows the case for LIM = 2.

Finally, Figure 9 shows the map reconstruction based on the EKF-SLAM with the features’ covariance approach. Figure 9(a) shows the case when LIM = 5 and Figure 9(b) shows the case for LIM = 2. As it can be seen, the results shown in Figure 9 are very similar to the ones shown in Figure 6.

For the purpose of showing the performance of each SLAM algorithm, Table 1 shows the mean square error (MSE) between the pre-established path and the estimated path by the SLAM algorithms. The MSE associated with each algorithm was calculated point-to-point according to the data stored from the mobile robot pose SLAM estimation and the predefined path. In addition, Figure 10 shows the error evolution between the estimated path and the predefined one. As it can be seen, the full sequential EKF-SLAM shows the smallest error at all time whereas the EKF-SLAM with the feature selection criterion based on the covariance ratio shows the closest evolution with respect to the full sequential EKF-SLAM. Furthermore, the EKF-SLAM with the feature selection method based on the entropy approach shows the worst path between all the executions. Figure 11 shows a zoom of Figure 10.

As it is shown in Table 1, when increasing the number of features to be used within the correction stage, decreases the MSE associated with the path estimated by the SLAM. Among the five feature selection approaches presented in this work, the feature selection criterion based on the covariance ratio approach has shown the best performance with both: LIM = 5 and LIM = 2. The entropy selection approach has shown to be the worst criterion given the experiment shown in Figure 5. Furthermore, the covariance ratio approach and both eigenvalues approaches have shown a better MSE when LIM = 2 than the sequential EKF-SLAM with the correction of the two first detected features (see Figure 4(b)). In addition, at the end of the experiments shown in Figures 4–9, the size of the map was of 50 point-based features for the full sequential EKF-SLAM—with no feature selection; for the EKF-SLAM with the entropy feature selection approach restricted to the two most significant features, the SLAM’s map was of 150 features—mainly because of both: the bad association given by the Mahalanobis distance criterion used in this work [3] and the increasing processing time; the map obtained by the EKF-SLAM with the covariance ratio feature criterion approach restricted to two features was of 58 point-features whereas the map obtained by the EKF-SLAM with sum of eigenvalues and the maximum eigenvalue feature selection approaches restricted to two features was of 68 and 65 point-based features respectively. For the features’ covariance approach, the number of features detected was of 60. As it can be seen, the EKF-SLAM with the covariance ratio feature selection approach and the features’ covariance approach show the minimum map reconstruction when compared with the other methods with feature selection restriction.

The implementation results shown in Figures 6–8 were carried out using the Joseph’s covariance matrix instead of the classical SLAM covariance matrix correction shown in Equation (1). The results of using the covariance matrix correction shown in Equation (1) have not shown significant map reconstruction differences with respect to the Joseph’s approach. Hence, the graphical results were not included herein.

For the purpose of showing the evolution of the features’ estimation, Figures 12–17 show the evolution of the covariance of five features—with two parameters per feature—at different stages of the navigation of the experiments shown in Figures 4–9. Figure 12 shows the covariance evolution of the features when estimated by the full sequential EKF-SLAM without restrictions to the correction stage; Figure 13 shows the evolution of the same set of features when estimated by the EKF-SLAM with the entropy feature selection approach; Figure 14 shows the evolution of the features when using the EKF-SLAM with the covariance ratio feature selection approach implemented; Figures 15 and 16 show the evolution of such set of features when estimated by the EKF-SLAM with the sum of eigenvalues and the maximum eigenvalue selection approaches respectively. Figure 17 shows the feature covariance evolution for the features’ covariance approach. In Figures 13–17, the magenta feature and the green feature are not used in the correction stage. The last means that, although those features are added to the SLAM system state, they were not considered as most significant features at the moment of the correction of the SLAM algorithm. As it can be seen, there is no difference between the convergence of non-significant features compared with significant ones.

With the aim of showing the processing resources used by each EKF-SLAM with feature selection criterion algorithm, Figure 18 shows the accumulated processing time associated with them. Figure 18 represents the amount of time that each algorithm of Table 1 remained processing data. As expected, the algorithms with a feature selection criterion have shown a lower accumulated processing time than the ones with non restrictions in the correction stage of the EKF-SLAM. Furthermore, the EKF-SLAM with the feature selection criterion based on the entropy (Section 4.1) has a bigger amount of processing time when compared with the others algorithms—with the exception of the sequential EKF-SLAM with non feature selection criterion. The increment on its accumulated processing time is due to the determinant of the covariance matrix of the SLAM system state, as stated in the information calculation shown in Equations (3) and (4). Thus, as increases the number of elements on the SLAM system state, increases the computational cost of calculating the determinant of the covariance matrix associated with it.

The improvement of the restrictions within the sequential EKF-SLAM is linear with O(n²).