A Global Asymptotic Convergent Observer for SLAM

This paper examines the global convergence problem of SLAM algorithms, an issue that faces topological obstructions. This is because the state-space of attitude dynamics is defined on a non-contractible manifold: the special orthogonal group of order three SO(3). Therefore, this paper presents a novel, gradient-based hybrid observer to overcome these topological obstacles. The Lyapunov stability theorem is used to prove the globally asymptotic convergence of the proposed algorithm. Finally, comparative analyses of two simulations were conducted to evaluate the performance of the proposed scheme and to demonstrate the superiority of the proposed hybrid observer to a smooth observer.


I. INTRODUCTION
S IMULTANEOUS Localization and Mapping (SLAM) is a well-known highly nonlinear problem which many previous studies have examined [1]. This estimation problem has an extensive variety of applications, ranging from unmanned aerial vehicles (UAV) to underwater robotics. Likewise, the co-dependence of environmental mapping and pose estimation makes the problem of significant theoretical interest. In the SLAM problem, a mobile robot tries to construct a map of an environment while simultaneously estimating its pose (i.e., attitude and position) [2]. Different types of estimation techniques have been applied to the SLAM problem, including Kalman-type filters [3] and geometric nonlinear observers [4].
As mentioned above, the Kalman filter and its variants are the estimation algorithms that have most frequently been employed to solve SLAM [5]. Nevertheless, Kalman-type filters suffer from some serious shortcomings; for example, their performance depends on the prior information regarding noise statistics and initial values; consistency is also an issue [6]. Several previous studies have addressed these limitations [7]. For example, [8] introduced a new unscented Kalmantype filter (UKF), called observability-constrained (OC)-UKF, to address two key restrictions of UKF: the computational cost in high-dimension systems and the inconsistency problem. A right invariant extended Kalman filter (RI-EKF) based on a new Lie group structure has also been presented by [9]  been presented in [10] to resolve the problems related to stability and tracking accuracy. In this strategy, an adaptive factor was included to calculate the process noise covariance matrix, and a dynamic model of the robot was utilized to predict the locations of the robot and of landmarks. The inconsistency of EKF-SLAM has also been investigated by [11]; here, the filter Jacobians are determined utilizing the first-ever accessible estimates for each state to preserve the dimensions of the observable subspace. [12] used a combination of EKF and a particle filter to address the SLAM problem. In this method, the particle filter determines the position of the mobile robot, and the EKF estimates the position of the environment. The performance of the UKF-SLAM was further developed by [13], who rendered an adaptive random search maximization scheme to adapt the scaling parameter. To further improve the performance of the standard UKF-SLAM and reduce its dependency on prior knowledge, a robust SLAM has also been developed based on H ∞ square root UKF [14].
One recently adopted technique for solving the SLAM problem is the use of geometric nonlinear observers. In these strategies, observers are directly designed in matrix Lie groups, including SE 1+n (3) and SLAM n (3). For instance, [15] used a gradient-based observer in the underlying Lie group; the innovation term was derived from the descent direction of an error function. Utilizing group speed and landmark measurements, a geometric nonlinear observer has been introduced that evolves directly on the matrix Lie group SE 1+n (3) [16]. Furthermore, [17] developed a geometric nonlinear observer directly on the manifold of the Lie group SLAM n (3). This proposed observer could guarantee predefined performance parameters and remove the unspecified bias in velocity measurements by using measurements of inertial measurement unit (IMU), group velocity, and landmarks. In a continuation of previous work, the authors have developed the observer by diminishing the boundaries of the error function to ensure faster convergence on the origin [18]. The SLAM manifold has been introduced to develop the matrix Lie group SLAM n (3) for the SLAM problem in [19]. Consequently, a global asymptotic stable observer has also been derived on the suggested manifold to solve SLAM in dynamic environments.
Alongside the SLAM problem, Visual SLAM (VSLAM) has also received significant attention. VSLAM is a specific case of SLAM in which a monocular camera provides the measurements. Van Goor et al. proposed a new Lie group called V SLAM n (3) and derived an almost globally asymptotically stable observer on V SLAM n (3) [20]. The introduced observer utilized decoupled gain matrices for each landmark and employed a new cost function to calculate innovations in the robot's pose. In addition, [21] continues the authors' prior work; here, a gradient-based observer with almost global stability has been designed in the V SLAM n (3) Lie group. Van Goor's et al. [20] work has been further developed in [22] with the introduction of equivariant group actions. The suggested nonlinear equivariant observer's almost semi-global convergence is its most important feature.
Although the observers described above have a number of advantages, they also share a significant shortcoming. To the best of the authors' knowledge, most state-of-the-art observers ensure almost global stability [23]. This is because the special orthogonal group of order three SO(3) is a non-contractible manifold [24]. Hence, there exists a set with Lebesgue measure zero from which the estimation error cannot converge to zero. Hybrid systems have therefore been used to overcome this topological obstruction and to derive observers with global stability on SO(3) [25], SE(3) [26], and SE 2 (3) [27]. For example, two hybrid observers were introduced in [28]; the first observer uses fixed gains, while the second uses variable gains by solving a continuous Riccati equation. Wang et al. [29] expands on the authors' previous work; here, the same strategy has been used to develop two hybrid observers. In contrast to previous observers, these observers do not need information about the gravity vector and can overcome the difficulty in estimation under intermittent landmark measurements.
In light of the shortcomings of previous solutions, the present paper addresses the problem of designing an observer with global stability for SLAM. Here, a new, gradient-based hybrid observer is introduced on the SE 1+n (3) manifold to solve the SLAM problem. The present article is divided into five sections, including the introduction. Section 2 provides the preliminary mathematical notation, SLAM kinematics and measurements equations, and the basic background on hybrid systems. The proposed hybrid observer is described in section 3. Section 4 illustrates the results of simulations in which the proposed observer is compared to a smooth observer. Finally, section 6 summarizes the paper and provides some concluding remarks.

A. Notation
This paper denotes the sets of real, nonnegative real, and natural numbers by R, R ≥0 , and N, respectively. R n represents n-dimensional Euclidean space, where {e i } 1≤i≤n ⊂ R n is the canonical basis of R n . x = x, x denotes the twonorm of a vector where x, y := x T y is the inner products of vectors x, y ∈ R n and x A := min y∈A x − y . The trace, determinant, and transpose of a matrix A ∈ R n×n are denoted by tr(A), det(A), and A T , respectively. Moreover,  (3), p ∈ R 3 , η ∈ R 3×n } represents the matrix Lie group. Throughout this paper, the following identities are used frequently.
The inverse of X is determined as , and the Lie algebra associated with the SE 1+n (3) is given by The gradient of a differentiable smooth function m : Where dm is the differential of m and ., . X is a Riemannian metric on SE 1+n (3) such that (3) is defined as Ad X V := X VX −1 ; this takes a tangent vector of one element and transforms it to a tangent vector of another element. The Rodrigues formula ℜ : R × S 2 → SO(3) parametrizes a rotation matrix R ∈ SO(3) using a specific angle θ ∈ R around a fixed axis y ∈ S 2 ; this formula is expressed as follows.

B. SLAM Kinematics
The kinematic equations define the motion of a rigid body, and a family of n landmarks are given as follows.
Here, ω ∈ R 3 and v ∈ R 3 are the angular rate and linear velocity of the rigid body expressed in the body-fixed frame B, respectively. ξ i ∈ R 3 is the linear speed of the i-th landmark expressed in B. The kinematic equations (4)-(6) can be rephrased using the following compact form.
In this paper, it is assumed that the landmarks are stationary (i.e., ξ i = 0) and that the linear and angular velocities of the rigid body are available for measurement. It is also assumed that the angular and linear velocity measurements include an unknown constant bias, that is Furthermore, it is also assumed that the robot can perceive both range γ = η i − p and bearing  = R T (η i − p)/γ relative to landmarks. Accordingly, the following compact equation is the result of a combination of the range and bearing measurements.
C. Hybrid System Frameworks The present paper uses the following framework of hybrid systems H that was first introduced by [30].
In this framework, f : R n × R m → R n is the flow map that defines the continuous dynamics of H, and g : R n × R m → R n is the jump map that specifies the behavior of H during jumps. The flow set C ⊂ R n × R m indicates where continuous evolution is allowed to flow, and the jump set D ⊂ R n ×R m demonstrates where the system is permitted to jump.
A hybrid arc consists of a hybrid time domain dom x and a function x : dom x → R n , which is also called a solution to H.
Lemma [31]: The closed set A ⊂ R n is locally and exponentially stable for H, if (α 1 > α 2 , s 1 , s 2 , n) ∈ R ≥0 exists and there is a continuously differentiable function V : dom V → R on an open set containing the closure of C that satisfies the following equation.
where B := {x ∈ R n : x ≤ 1} is the closed unit ball. The set A is said to be globally exponentially stable if s 1 = ∞, and A is said to be globally asymptotically stable if s 1 = ∞, s 2 = 0.

III. PROPOSED HYBRID OBSERVER
This section describes the proposed observer. As mentioned above, the two main techniques that have been utilized to solve the SLAM problem are geometric nonlinear observers and Kalman-type filters. The drawbacks of these methods have also been discussed in the previous section. Most state-ofthe-art observers are almost globally stable due to the noncontractibility of the state-space of attitude kinematics (i.e., SO (3)). Consequently, hybrid systems have been used to tackle this topological challenge and to obtain globally stable results [32]. Therefore, the present paper builds on the observer developed by [16] and describes a hybrid observer for solving the SLAM problem. Consider the following smooth real-value function: Where A := n i=1 k i r i r i T and k i ∈ R ≥0 are positive constants. Utilizing the Riemannian metric on SE 1+n (3) and the identities provided in the Appendix A, one can show that Therefore, the gradient of U with respect to X is calculated with the following equation.
Throughout this paper,X denotes the estimated value of the state X . Therefore,X =X X −1 is the estimation error withR = RR T ,p = p −Rp, andη = η −Rη. Hence, the following identities can be easily calculated using (7) and (1). (For details, see [16].) The dynamics of the proposed hybrid observer are defined as follows.
Theorem: Consider the proposed hybrid observer (16) with any θ ∈ R >0 and ℓ ∈ S 2 for the SLAM kinematics (7). The state estimation errorX and bias estimation error Vb = V b −Vb converge to I n×n and 0, respectively; therefore, the following set is globally asymptotically stable. (17) Proof: According to Lemma, Theorem is proven in two steps.
The Lyapunov function candidate is defined as follows.
The time derivative of V is calculated as follows.V After simplifying (21) and utilizing the Cauchy-Schwarz inequality for matrix [33], the resulting equation is as follows.
It can thus be deduced thatR,p,η, and Vb are globally bounded. This implies thatV is also globally bounded. Barbalat's lemma reveals that lim t→+∞V = 0; therefore,X = I and Vb = 0. (For details, see [16].) Step 2: In this step, the last condition of (11) is proven. Since the switching variable q generates jumps, it is essential to assay the variation in V (X , Vb) to ensure that the Lyapunov function is reduced across jumps. The variation in V along jumps is given by the following equation.
From (16), one can show that Finally, it follows from Lemma that the set A is globally asymptotically stable.

IV. SIMULATION RESULTS
This section presents the numerical simulations used to evaluate the performance of the proposed observer. The proposed hybrid observer is contrasted with the smooth observer described in [16]. The stability of the proposed algorithm is verified under various initial conditions. In both experiments, it is assumed that the robot can measure range and bearing to four landmarks located at    Figures (1-4) illustrate the results of this experiment. Figure ( Figure (4). These figures demonstrate that the proposed hybrid observer has lower estimation errors than the smooth observer. Furthermore, the convergence rate of the proposed observer is faster than that of the smooth observer.

B. Second Experiment
As discussed above, in this experiment, a different simulation scenario is used to further assess the performance of the proposed observer. The robot is simulated to move along a eight-shape trajectory at a fixed height of 4 m. The robot's initial attitude and position were set to R(0) = R(0, e 1 ) and p(0) = [0 0 4] T , respectively. In this experiment, angular velocity was assumed to be ω = [0 0 ± 0.4] T rad/sec, and linear velocity was assumed to be v = [2 0 0] T m/sec. Both observers were initialized atp(0) = [0 0 0] T , R(0) = R(π/3, e 1 ), andη = 0.4 * η. The simulation results of the second experiment are shown in Figures (5-8). Figure  (5) shows the 3D trajectories of the robot's position tracking and estimated landmark locations, as well as the true robot trajectory and actual landmark locations. The errors in the estimation of attitude, robot path, landmark locations, and biases are illustrated in Figures (6,7). Figure (8) shows the evolution of the Lyapunov functions. These figures reveal that the proposed hybrid observer performs better than the smooth observer on both trajectory tracking and reducing the effect of noise.

V. CONCLUSION
The present paper has investigated the problem of global convergence in SLAM observers. Most state-of-the-art SLAM techniques can guarantee almost global convergence due to the non-contractibility of the state-space of attitude. Accordingly, this paper has introduced a gradient-based hybrid observer to    overcome topological obstructions and achieve global convergence. The proposed algorithm was demonstrated to be globally asymptotically convergent. Finally, the proposed hybrid observer was compared to a smooth observer, demonstrating the superior performance of the proposed algorithm.
APPENDIX A SOME USEFUL IDENTITIES This paper uses the following identities related to the orthogonal projection and matrix inner product.