A Geometric Nonlinear Stochastic Filter for Simultaneous Localization and Mapping
PPersonal use of this material is permitted. Permission from the author(s) and/or copyright holder(s), must be obtained for all other uses. Please contact us and provide details ifyou believe this document breaches copyrights.
A Geometric Nonlinear Stochastic Filter forSimultaneous Localization and Mapping
Hashim A. Hashim ∗ Member, IEEE
Abstract —Simultaneous Localization and Mapping (SLAM) isone of the key robotics tasks as it tackles simultaneous mappingof the unknown environment defined by multiple landmarkpositions and localization of the unknown pose ( i.e ., attitude andposition) of the robot in three-dimensional (3D) space. The trueSLAM problem is modeled on the Lie group of
SLAM n (3) , andits true dynamics rely on angular and translational velocities.This paper proposes a novel geometric nonlinear stochastic esti-mator algorithm for SLAM on SLAM n (3) that precisely mimicsthe nonlinear motion dynamics of the true SLAM problem.Unlike existing solutions, the proposed stochastic filter takes intoaccount unknown constant bias and noise attached to the velocitymeasurements. The proposed nonlinear stochastic estimator onmanifold is guaranteed to produce good results provided withthe measurements of angular velocities, translational velocities,landmarks, and inertial measurement unit (IMU). Simulation andexperimental results reflect the ability of the proposed filter tosuccessfully estimate the six-degrees-of-freedom (6 DoF) robot’spose and landmark positions. Index Terms —Simultaneous Localization and Mapping, non-linear stochastic observer, stochastic differential equations, poseestimator, position, attitude, Brownian motion process, inertialmeasurement unit, SLAM.
I. I
NTRODUCTION R OBOTICS applications are experiencing a surge in de-mand for navigation solutions suitable for partially orcompletely unknown robot pose in three-dimensional (3D)space ( i.e ., attitude and position) within an unknown envi-ronment. Robot’s pose is comprised of two elements: robot’sorientation, also known as attitude, and robot’s position. Es-timating map of the environment given robot’s pose consti-tutes a mapping problem popular within computer scienceand robotics communities [1]. On the other hand, recoveringrobot’s pose within a known environment is referred to aspose estimation problem long-established and well-detailedamong robotics and control community [2,3]. When neitherthe robot’s pose nor the map of the environment are known,the problem is termed Simultaneous Localization and Map-ping (SLAM). SLAM concurrently maps the environment andlocalizes the robot with respect to the map. Unreliability of ab-solute positioning systems, such as global positioning systems,in occluded environments makes SLAM indispensable for anumber of applications, such as terrain mapping, multipurposehousehold robots, mine exploration, locating missing terrestrialobjects, reef monitoring, surveillance, and others. Thus, for
This work was supported in part by Thompson Rivers University Internalresearch fund ∗ Corresponding author, H. A. Hashim is with the Department of Engineer-ing and Applied Science, Thompson Rivers University, Kamloops, BritishColumbia, Canada, V2C-0C8, e-mail: [email protected] over a decade, SLAM and SLAM-related applications havebeen a fundamental and widely-explored problem [4–11].The SLAM problem is traditionally addressed employingthe measurements available in the body-frame of a movingrobot. Due to the fact that measurements are contaminated withuncertain elements, SLAM estimation requires a robust filter.The problem of SLAM estimation is conventionally tackledusing Gaussian filters or nonlinear deterministic filters. Overten years ago, several Gaussian filters for SLAM tailoredspecifically to the task of estimating the robot state andthe surrounding landmarks were proposed. Gaussian solu-tions include MonoSLAM using real-time single camera [10],FastSLAM using scalable approach [12], incremental SLAM[13], unscented Kalman filter (UKF) [14], particle filter [11],invariant EKF [15], in addition to others. These solutionsaccount for uncertainties and rely on probabilistic framework.SLAM problem presents a number of open challenges, namelyconsistency [16], computational cost and solution complexity[17], as well as landmarks in motion. Other significant chal-lenges that hinder SLAM estimation process are as follows: 1)complexity of simultaneous localization and mapping furthercomplicated by 3D motion of the robot, 2) duality of theproblem that requires simultaneous pose and map estimation,and most importantly 3) high nonlinearity of the SLAMproblem. To address nonlinearity, it is important to note thattrue motion dynamics of SLAM are composed of robot’s posedynamics and landmark dynamics. Firstly, the highly nonlinearpose dynamics of a robot traveling in 3D space are modeled onthe Lie group of the special Euclidean group SE (3) . Secondly,robot’s attitude is an essential part of the landmark dynamics,and therefore the attitude is described according to the SpecialOrthogonal Group SO (3) . Thereby, the key to successfullySLAM estimation lies in utilizing filter design that capturesthe true nonlinear structure of the problem.Novel nonlinear attitude and pose filters evolved on SO (3) [18–21] and on SE (3) [2,3,22] enabled the development ofnonlinear filters for SLAM. The fact that nonlinear attitudeand pose filters mimic the true attitude and pose dynamics,served as a motivation for adopting the Lie group of SE (3) in application to the SLAM problem [23]. A dual nonlinearfilter comprised of a nonlinear filter for robot’s pose estimationand a Kalman filter for landmark observation was proposed[24]. However, nonlinear nature of the true SLAM problemwas not yet completely captured by the work in [24]. As aresult, nonlinear filters for SLAM that use measurements oflandmarks and group velocity vectors directly were developed[7,8,25]. The work in [7,8,25,26] considered unknown constantbias inherent in the group velocity vector measurements.To this end, two major challenges must be considered during To cite this article:
H. A. Hashim ”A Geometric Nonlinear Stochastic Filter for Simultaneous Localization and Mapping,” Aerospace Science andTechnology, vol. PP, no. PP, pp. PP, 2021. doi: 10.1016/j.ast.2021.106569 a r X i v : . [ ee ss . S Y ] F e b the design process of a nonlinear filter for SLAM: 1) theSLAM problem is modeled on the Lie group of SLAM n (3) which is highly nonlinear; and 2) the true SLAM kinemat-ics rely on a group of velocities, namely angular velocity,translational velocity, and velocities of landmarks expressedrelative to the body-frame. As such, successful estimationcan be attained by designing a nonlinear filter that relieson the previously mentioned group of velocities which arenormally corrupted with unknown noise as well as unknownconstant bias components. Moreover, noise components aredistinguished by random behavior, and it is well recognizedthat noise can negatively impact the output performance[18,22,27]. To the best of the author knowledge, SLAMestimation problem has been neither addressed nor solved instochastic sense. As a result, it is important to take into accountany noise and/or bias components present in the measurementprocess. Having this in mind and given the following set ofavailable measurements: group velocity vector, n landmarksand an inertial measurement unit (IMU), this paper introducesa novel nonlinear stochastic filter for SLAM that has thestructure of nonlinear deterministic filters adapting it to thestochastic sense. The main contributions of this paper are listedbelow:1) A geometric nonlinear stochastic filter for SLAM de-veloped directly on the Lie group of SLAM n (3) whichexactly follows the nonlinear structure of the true SLAMproblem is proposed.2) The proposed nonlinear stochastic filter accounts forunknown constant bias and random noise attached tothe group velocity measurements, unlike [7,8].3) The closed loop error signals of the Lyapunov candi-date function are shown to be semi-globally uniformlyultimately bounded (SGUUB) in mean square.4) The proposed stochastic filter involves gain mapping thattakes into account cross coupling between the innovationof pose and landmarks.The rest of the paper is structured in the following man-ner: Section II contains an overview of the preliminaries aswell as introduces mathematical notation, the Lie group of SO (3) , SE (3) , and SLAM n (3) . Section III describes theSLAM problem, true motion kinematics and formulates theSLAM problem in a stochastic sense. Section IV outlines acommon structure of nonlinear deterministic filter for SLAMon SLAM n (3) and then proposes a nonlinear stochastic filterdesign on SLAM n (3) . Section V shows the effectiveness ofthe proposed stochastic filter. Finally, Section VI concludesthe work.II. M ATH N OTATION AND
SLAM n (3) P RELIMINARIES
Throughout this paper two frames of reference are used: {I} is a fixed inertial frame and {B} is a moving body-frameof a robot. R , R + , and R p × q denote sets of real numbers,nonnegative real numbers, and a real space of dimension p -by- q , respectively. I n represents an identity matrix withdimension n , n represents a vector comprised of zeros, and (cid:107) y (cid:107) = (cid:112) y (cid:62) y represents Euclidean norm for y ∈ R n . P {·} , E [ · ] , and exp ( · ) denote probability, an expected value, and an exponential of a component, respectively. C n stands fora set of functions characterized by continuous n th partialderivatives. K ∞ represents a set of functions whose elementsare continuous and strictly increasing. The Special OrthogonalGroup SO (3) is expressed as SO (3) = (cid:8) R ∈ R × (cid:12)(cid:12) RR (cid:62) = R (cid:62) R = I , det ( R ) = +1 (cid:9) with det ( · ) referring to a determinant of a matrix, and R ∈ SO (3) being rigid-body’s orientation described in {B} ,also known as attitude. The Special Euclidean Group SE (3) is represented by SE (3) = (cid:26) T = (cid:20) R P (cid:62) (cid:21) ∈ R × (cid:12)(cid:12)(cid:12)(cid:12) R ∈ SO (3) , P ∈ R (cid:27) with P ∈ R being rigid-body’s position and R ∈ SO (3) its orientation. T is used to express the rigid-body’s posein 3D space and it is often referred to as a homogeneoustransformation matrix: T = (cid:20) R P (cid:62) (cid:21) ∈ SE (3) (1)where denotes a zero column vector. so (3) represents theLie-algebra associated with SO (3) with so (3) = (cid:110) [ h ] × ∈ R × (cid:12)(cid:12) [ h ] (cid:62)× = − [ h ] × , h ∈ R (cid:111) (2)such that [ h ] × stands for a skew symmetric matrix. The relatedmap of (2) [ · ] × : R → so (3) is [ h ] × = − h h h − h − h h ∈ so (3) , h = h h h and [ y ] × h = y × h where × represents a cross product for h, y ∈ R . se (3) is the Lie-algebra associated with SE (3) defined as se (3) = (cid:26) [ U ] ∧ ∈ R × (cid:12)(cid:12)(cid:12)(cid:12) ∃ Ω , V ∈ R : [ U ] ∧ = (cid:20) [Ω] × V (cid:62) (cid:21) (cid:27) with [ · ] ∧ being a wedge operator. The related wedge map [ · ] ∧ : R → se (3) is defined by [ U ] ∧ = (cid:20) [Ω] × V (cid:62) (cid:21) ∈ se (3) , U = (cid:20) Ω V (cid:21) ∈ R (3)The inverse mapping of [ · ] × is given by vex : so (3) → R such that vex (cid:0) [ h ] × (cid:1) = h, ∀ h ∈ R (4)Consider P a to be an anti-symmetric projection on so (3) P a ( H ) = 12 (cid:0) H − H (cid:62) (cid:1) ∈ so (3) , ∀ H ∈ R × (5)Let Υ ( · ) denote a composition mapping of Υ = vex ◦ P a where Υ ( H ) = vex ( P a ( H )) ∈ R , ∀ H ∈ R × (6)Define (cid:107) R (cid:107) I as a normalized Euclidean distance of R ∈ SO (3) with (cid:107) R (cid:107) I = 14 Tr { I − R } ∈ [0 , (7) Define ◦ M and M as submanifolds of R ◦ M = (cid:110) ◦ x = (cid:2) x (cid:62) (cid:3) (cid:62) ∈ R (cid:12)(cid:12)(cid:12) x ∈ R (cid:111) M = (cid:110) x = (cid:2) x (cid:62) (cid:3) (cid:62) ∈ R (cid:12)(cid:12)(cid:12) x ∈ R (cid:111) Let
SLAM n (3) = SE (3) × M n be a Lie group SLAM n (3) = (cid:110) X = ( T , p) (cid:12)(cid:12)(cid:12) T ∈ SE (3) , p ∈ M n (cid:111) (8)with p = [p , p , . . . , p n ] ∈ M n and M n = M × M × · · · ×M . slam n (3) = se (3) × ◦ M n denotes a tangent space at theidentity element of X = ( T , p) ∈ SLAM n (3) represented as slam n (3) = (cid:26) Y = (cid:16) [ U ] ∧ , ◦ v (cid:17) (cid:12)(cid:12)(cid:12)(cid:12) [ U ] ∧ ∈ se (3) , ◦ v ∈ ◦ M n (cid:27) (9)with ◦ v = (cid:104) ◦ v , ◦ v , . . . , ◦ v n (cid:105) ∈ ◦ M n and ◦ M n = ◦ M × ◦ M ×· · · × ◦ M . The following identities will be utilized in filterderivations: [ Ra ] × = R [ a ] × R (cid:62) , a ∈ R , R ∈ SO (3) (10) [ b × a ] × = ab (cid:62) − ba (cid:62) , a, b ∈ R (11) [ a ] × = − || a || I + aa (cid:62) , a ∈ R (12) M [ a ] × + [ a ] × M =Tr { M } [ a ] × − [ M a ] × ,a ∈ R , M ∈ R × (13) Tr (cid:8) [ a ] × M (cid:9) =0 , a ∈ R , M = M (cid:62) ∈ R × (14) Tr (cid:8) M [ a ] × (cid:9) =Tr (cid:8) P a ( M ) [ a ] × (cid:9) = − vex ( P a ( M )) (cid:62) a,a ∈ R , M ∈ R × (15)III. SLAM F ORMULATION IN S TOCHASTIC S ENSE
The rigid-body’s (vehicle’s) attitude R ∈ SO (3) , a vitalpart of the robot’s pose T ∈ SE (3) , is expressed in the body-frame R ∈ {B} , while its translation P ∈ R is expressed inthe inertial-frame P ∈ {I} . Let the map include n landmarkswhere p i denotes location of the i th landmark defined in theinertial-frame p i ∈ {I} for all i = 1 , , . . . , n . SLAM problemconsiders the following two elements to be completely un-known: 1) pose of the moving robot, and 2) landmarks withinthe environment p = [p , p , . . . , p n ] ∈ M n . Accordingly,SLAM estimation problem given a set of measurements in-corporates two tasks executed concurrently: 1) estimation ofthe robot’s pose with respect to the environment landmarks,and 2) estimation of landmark positions within the map. Figure1 illustrates the SLAM estimation problem. A. SLAM Kinematics and Measurements
Let X = ( T , p) ∈ SLAM n (3) denote the true con-figuration of the SLAM problem with T ∈ SE (3) as in(1) and p = [p , p , . . . , p n ] ∈ M n . Notice that X isunknown. A group of measurements is available in {B} andcan be employed for SLAM estimation, namely 1) body-frame measurements associated with attitude determination, 2)landmark measurements, and 3) group velocity measurements.Assume that there are n R body-frame vectors suitable for attitude determination and available for measurement definedby [18,19] ◦ a j = T − ◦ r j + ◦ b aj + ◦ n aj ∈ ◦ M , j = 1 , , . . . , n R or equivalently a j = R (cid:62) r j + b aj + n aj ∈ R (16)where r j denotes known inertial-frame vector, b aj denotesunknown constant bias, and n aj stands for unknown randomnoise of the j th measurement. Note that the inverse of T is T − = (cid:20) R (cid:62) − R (cid:62) P (cid:62) (cid:21) ∈ SE (3) . The measurements in(16) exemplify a low cost IMU. It is a common practice tonormalize r j and a j in (16) as follows υ rj = r j (cid:107) r j (cid:107) , υ aj = a j (cid:107) a j (cid:107) (17)The normalized values in (17) will be part of the subsequentestimation. Consider combining the normalized vectors intotwo distinct sets as follows (cid:40) υ r = (cid:2) υ r , υ r , . . . , υ rn R (cid:3) ∈ {I} υ a = (cid:2) υ a , υ a , . . . , υ an R (cid:3) ∈ {B} (18) Remark 1.
Rigid-body’s attitude can be established providedthat at least three non-collinear vectors in {B} along withtheir observations in {I} are obtainable at each time sample.In case of n R = 2 , the third measurement in {B} and itsobservation in {I} is to be calculated via the cross product υ a = υ a × υ a and υ r = υ r × υ r , respectively ensuring thatthe two sets in (17) are with rank 3. Assume that n landmarks are available for measurement inthe body-frame via, for example, low-cost inertial vision units.The i th measurement is as follows [2,22]: y i = T − p i + ◦ b yi + ◦ n yi ∈ M , ∀ i = 1 , , . . . , n or equivalently y i = R (cid:62) (p i − P ) + b yi + n yi ∈ R (19)with R , P , and p i representing the true attitude and positionof the robot, and landmark position, respectively, while b yi and n yi stand for unknown constant bias and random noise,respectively, for all y i , b yi , n yi ∈ {B} . Assumption 1.
A minimum of three landmarks availablefor measurement is necessary to define a plane y =[ y , y , . . . , y n ] ∈ M n . Consider Y = (cid:16) [ U ] ∧ , ◦ v (cid:17) ∈ slam n (3) to be the truegroup velocity which is bounded and continuous with ◦ v = (cid:104) ◦ v , ◦ v , . . . , ◦ v n (cid:105) ∈ ◦ M n . Note that Y is given through sen-sor measurements. Hence, the true motion dynamics of thevehicle’s pose and n -landmarks are (cid:40) ˙ T = T [ U ] ∧ ˙p i = R v i , ∀ i = 1 , , . . . , n (20) Landmark 2 x I y I Z I Inertial-frame { I } Landmark n Landmark 1 Z Body-frame { B } B y B x B p p y y y n Available (measurements): - Angular velocity ∈ { B } - Translational velocity ∈ { B } - Landmark ∈ { B } - IMU ∈ { B } - Vector Observation of IMU ∈ { I } Unknown: - Rigid-body Orientation R ∈ { B } - Rigid-body Position P ∈ { I } - Landmark Position ∈ { I } p i SLAM p n Fig. 1. SLAM estimation problem.
The dynamics in (20) can be expressed as ˙ R = R [Ω] × ˙ P = RV ˙p i = R v i , ∀ i = 1 , , . . . , n with U = (cid:2) Ω (cid:62) , V (cid:62) (cid:3) (cid:62) ∈ R referring to the group velocityvector of the rigid-body where Ω ∈ R is the true angularvelocity and V ∈ R is the true translational velocity. v i ∈ R defines the i th linear velocity of the landmark in the moving-frame for all Ω , V, v i ∈ {B} . The measurements of angularand translational velocity are defined as (cid:40) Ω m = Ω + b Ω + n Ω ∈ R V m = V + b V + n V ∈ R (21)where b Ω and b V denote unknown constant bias, and n Ω and n V denote unknown random noise. Define the group ofvelocity measurements, bias, and noise as U m = (cid:2) Ω (cid:62) m , V (cid:62) m (cid:3) (cid:62) , b U = (cid:2) b (cid:62) Ω , b (cid:62) V (cid:3) (cid:62) , and n U = (cid:2) n (cid:62) Ω , n (cid:62) V (cid:3) (cid:62) , respectively, forall U m , b U , n U ∈ R . This work concerns exclusively fixedlandmark environments, thereby ˙p i = and v i = ∀ i = 1 , , . . . , n . B. SLAM Kinematics in Stochastic Sense
Recall the expression of group velocity measurements in (21).Since derivative of a Gaussian process results a Gaussianprocess, the SLAM dynamics in (20) can be rewritten withrespect to Brownian motion process vector dβ U /dt ∈ R [28,29]. Assume { n U , t ≥ t } to be a vector representationof the independent Brownian motion process n U = Q U dβ U dt ∈ R (22)where Q U ∈ R × denotes an unknown nonzero nonnegativetime-variant diagonal matrix whose elements are bounded. Therelated covariance of the noise n U can be expressed as Q U = Q U Q (cid:62) U . The following properties characterize the Brownianmotion process [22,29–32]: P { β U (0) = 0 } = 1 , E [ dβ U /dt ] = 0 , E [ β U ] = 0 In view of (20), (21), and (22), SLAM dynamics could berepresented by a stochastic differential equation (cid:40) d T = T [ U m − b U ] ∧ dt − T [ Q U dβ U ] ∧ d p i = R v i dt, ∀ i = 1 , , . . . , n (23)Or equivalently dR = R [Ω m − b Ω ] × dt − R [ Q Ω dβ Ω ] × dP = R ( V m − b V ) dt − R Q V dβ V d p i = R v i dt, ∀ i = 1 , , . . . , n where U = U m − b U − n U is considered. Given unknown bias b U and unknown time-variant covariance matrix Q U , with theaim of achieving adaptive stabilization, define σ as the upperbound of Q U σ = max (cid:110) Q , , Q V (1 , (cid:111) max (cid:110) Q , , Q V (2 , (cid:111) max (cid:110) Q , , Q V (3 , (cid:111) ∈ R (24)with max {·} being maximum value of the correspondingelements. Assumption 2. (Uniform boundedness of b U and σ ) Consider b U and σ to belong to a known compact set Λ U with b U , σ ∈ Λ U ⊂ R , such that b U and σ are upper bounded by a constant Π where || Λ U || ≤ Λ < ∞ .C. Error Criteria Define the pose estimate as ˆ T = (cid:20) ˆ R ˆ P (cid:62) (cid:21) ∈ SE (3) with ˆ R being the estimate of R , and ˆ P being the estimate of P in (1). Define ˆ y i = ˆ T − ˆp i where ˆp i is the i th landmarkestimate of p i . Let the pose error (true relative to estimated) be ˜ T = ˆ T T − = (cid:20) ˆ R ˆ P (cid:62) (cid:21) (cid:20) R (cid:62) − R (cid:62) P (cid:62) (cid:21) = (cid:20) ˜ R ˜ P (cid:62) (cid:21) (25)with ˜ R = ˆ RR (cid:62) being the error in orientation and ˜ P = ˆ P − ˜ RP being the error in position of the rigid-body. Pose estimationaims to asymptotically drive ˜ T → I in order to achieve thisgoal ˜ R → I and ˜ P → . Let the landmark position error(true relative to estimated) be ◦ e i = ˆp i − ˜ T p i , ∀ i = 1 , , . . . , n (26)such that ◦ e i = (cid:2) e (cid:62) i , (cid:3) (cid:62) ∈ ◦ M and ˆp i = (cid:2) ˆp (cid:62) i , (cid:3) (cid:62) ∈ M . Notethat ◦ e i = ˆp i − ˆ T T − p i , and therefore from (19) one has ◦ e i = ˆp i − ˆ T y i , ∀ i = 1 , , . . . , n (27)which leads to ◦ e i = (cid:20) ˆp i (cid:21) − (cid:20) ˆ R ˆ P (cid:62) (cid:21) (cid:20) R (cid:62) (p i − P )1 (cid:21) = (cid:20) ˜p i − ˜ P (cid:21) ∈ ◦ M (28)with ˜p i = ˆp i − ˜ R p i and ˜ P = ˆ P − ˜ RP . Considering the factthat the last row of the matrix in (28) is a zero, define thestochastic differential equation of the error above as de i = F i dt + G i Q U dβ U , ∀ i = 1 , , . . . , n (29)where the stochastic dynamics in (29) are to be obtained in thestochastic filter. Taking in consideration the group velocity in(21) with ˆ b U = (cid:104) ˆ b (cid:62) Ω , ˆ b (cid:62) V (cid:105) (cid:62) being the unknown bias estimateof b U , define the bias error as (cid:40) ˜ b Ω = b Ω − ˆ b Ω ˜ b V = b V − ˆ b V (30)where ˜ b U = b U − ˆ b U = (cid:104) ˜ b (cid:62) Ω , ˜ b (cid:62) V (cid:105) (cid:62) ∈ R . Also, consider ˆ σ tobe the estimate of σ in (24). Define the error between ˆ σ and σ as follows ˜ σ = σ − ˆ σ (31)The subsequent Definitions and Lemmas are applicable in thederivation process of the nonlinear stochastic estimator forSLAM. Definition 1.
Let U s be a non-attractive forward invariantunstable subset of SO (3) U s = (cid:110) ˜ R (0) ∈ SO (3) (cid:12)(cid:12)(cid:12) Tr { ˜ R (0) } = − (cid:111) (32) The only three possible scenarios for ˜ R (0) ∈ U s are: ˜ R (0) = diag(1 , − , − , ˜ R (0) = diag( − , , − , and ˜ R (0) = diag( − , − , . Lemma 1.
Define ˜ R ∈ SO (3) , M = M (cid:62) ∈ R × such that rank { M } = 3 and Tr { M } = 3 . Define ˘ M = Tr { M } I − M with λ = λ ( ˘ M ) being the minimum singular value of ˘ M .Thereby, the following holds: || ˜ RM || I ≤ λ || vex (cid:16) P a ( ˜ RM ) (cid:17) || { ˜ RM M − } (33) Proof. See Lemma 1 [19].
Definition 2.
Consider the stochastic differential systemin (29) , and let V ( e , . . . , e n ) be a twice differentiablefunction V ( e , . . . , e n ) ∈ C . The differential operator L V ( e , . . . , e n ) is expressed as below L V ( e , . . . , e n ) = n (cid:88) i =1 (cid:18) V (cid:62) e i F i + 12 Tr (cid:8) G i Q U G (cid:62) i V e i e i (cid:9)(cid:19) such that V e i = ∂ V /∂e i , and V e i e i = ∂ V /∂e i ∀ i =1 , , . . . , n . Definition 3. [18,22,33] Consider the stochastic differentialsystem in (29) with trajectory e i being SGUUB if for a givencompact set Σ ∈ R and any e i ( t ) , there exists a positiveconstant κ > , and a time constant τ = τ ( κ, e i ( t )) with E [ (cid:107) e i ( t ) (cid:107) ] < κ, ∀ t > t + τ . Lemma 2. [31] Consider the stochastic dynamics in (29) to be assigned with a potential function V ∈ C where V : R → R + . Suppose there exist class K ∞ functions ¯ α ( · ) and ¯ α ( · ) , constants η > and η ≥ such that ¯ α ( e , . . . , e n ) ≤ V ( e , . . . , e n ) ≤ ¯ α ( e , . . . , e n ) (34) L V ( e , . . . , e n ) = n (cid:88) i =1 (cid:18) V (cid:62) e i F i + 12 Tr (cid:8) G i Q U G (cid:62) i V e i e i (cid:9)(cid:19) ≤ − η V ( e , . . . , e n ) + η (35) then for e i ∈ R , there is almost a unique strong solutionon [0 , ∞ ) for the stochastic dynamics in (29) . Moreover, thesolution e i is bounded in probability where E [ V ( e , . . . , e n )] ≤ V ( e (0) , . . . , e n (0)) exp ( − η t ) + η η (36) In addition, if the inequality in (36) is met, e i in (29) is SGUUBin the mean square. The existence of a unique solution and proof of Lemma 2can be found in [31].
Lemma 3. (Young’s inequality) Let a ∈ R n and b ∈ R n .Define c > and c > such that ( c −
1) ( c −
1) = 1 ,and (cid:37) > as a small constant. Consequently, the followingholds: a (cid:62) b ≤ (1 /c ) (cid:37) c (cid:107) a (cid:107) c + (1 /c ) (cid:37) − c (cid:107) b (cid:107) c (37)Prior to moving forward, it is important to recall that thetrue SLAM dynamics in (20) 1) are nonlinear and 2) areposed on the Lie group of SLAM n (3) = SE (3) × M n where X = ( T , p) ∈ SLAM n (3) . Additionally, the tangent spaceof SLAM n (3) is slam n (3) = se (3) × ◦ M n such that and Y = (cid:16) [ U ] ∧ , ◦ v (cid:17) ∈ slam n (3) . With the aim of proposinga robust stochastic filter able to produce good results, the proposed filter design should imitate the true nonlinearityof the SLAM problem and should be modeled on the Liegroup of SLAM n (3) with the tangent space slam n (3) . Com-plying with the above-mentioned requirements, the structureof the stochastic filter is ˆ X = (cid:16) ˆ T , ˆp (cid:17) ∈ SLAM n (3) and ˆ Y = (cid:18) [ ˆ U ] ∧ , ◦ ˆv (cid:19) ∈ slam n (3) with ˆ T ∈ SE (3) and ˆp = (cid:2) ˆp , . . . , ˆp n (cid:3) ∈ M n being pose estimates and landmark posi-tions, respectively, and ˆ U ∈ se (3) and ◦ ˆv = (cid:20) ◦ ˆv , . . . , ◦ ˆv n (cid:21) ∈ ◦ M n being velocities to be designed in the following Section. Itis worth noting that ◦ ˆv i = (cid:2) ˆv (cid:62) i , (cid:3) ∈ ◦ M and ˆp i = (cid:2) ˆp (cid:62) i , (cid:3) (cid:62) ∈M for all i = 1 , , . . . , n and ˆv i , ˆp i ∈ R .IV. N ONLINEAR S TOCHASTIC F ILTER D ESIGN
The SLAM nonlinear stochastic filter design is proposedin this Section. With the aim of defining the concept ofthe nonlinear SLAM filtering and paving the way for thenovel nonlinear stochastic filter solution presented in thesecond subsection, the first subsection introduces a nonlineardeterministic filter that operates based only on the surroundinglandmark measurements which is similar in the structure to[7,8]. In contrast to the deterministic filter, the novel nonlinearstochastic SLAM filter relies on measurements collected bya low-cost IMU and measurements of the landmarks. Thefirst simple filter will provide a benchmark for the proposedstochastic solution.
A. Nonlinear Deterministic Filter Design without IMU
Consider the nonlinear filter design for SLAM: ˙ˆ T = ˆ T (cid:104) U m − ˆ b U − W U (cid:105) ∧ (38) ˙ˆ p i = − k p e i , i = 1 , , . . . , n (39) ˙ˆ b U = − n (cid:88) i =1 Γ α i (cid:20) [ y i ] × ˆ R (cid:62) ˆ R (cid:62) (cid:21) e i (40) W U = − n (cid:88) i =1 k w α i (cid:20) [ y i ] × ˆ R (cid:62) ˆ R (cid:62) (cid:21) e i (41)with k w , k p , Γ , and α i being positive constants, e i being asgiven in (27) for all i = 1 , , · · · , n , W U = (cid:2) W (cid:62) Ω , W (cid:62) V (cid:3) (cid:62) ∈ R being a correction factor, and ˆ b U = (cid:104) ˆ b (cid:62) Ω , ˆ b (cid:62) V (cid:105) (cid:62) ∈ R beingthe estimate of b U . Theorem 1.
Consider the true motion of SLAM dynamicsto be ˙ X = (cid:16) ˙ T , ˙p (cid:17) as in (20) , the output to be landmarkmeasurements ( y i = T − p i ) for all i = 1 , , . . . , n andthe velocity measurements in (20) to be attached only withconstant bias where U m = U + b U and n U = 0 . LetAssumption 1 hold true and the deterministic filter be as in (38) , (39) , (40) , and (41) combined with the measurements of U m and y i . Set the design parameters k w , k p , Γ , and α i as positive scalars for all i = 1 , , . . . , n . Also, consider the set S = (cid:8) ( e , e , . . . , e n ) ∈ R × R × · · · × R (cid:12)(cid:12) e i = ∀ i = 1 , , . . . n } (42) Then 1) the error e i in (26) is exponentially regulated to the set S , 2) ˜ T remains bounded and 3) given constants R c ∈ SO (3) and P c ∈ R one has ˜ R → R c and ˜ P → P c as t → ∞ .Proof: Since ˙ T − = − T − ˙ T T − , one obtains the errordynamics of ˜ T defined in (25) as follows ˙˜ T = ˙ˆ T T − + ˆ T ˙ T − = ˆ T (cid:104) U + ˜ b U − W U (cid:105) ∧ T − − ˆ T [ U ] ∧ T − = ˆ T (cid:104) ˜ b U − W U (cid:105) ∧ ˆ T − ˜ T (43)Thereby, the error dynamics of ◦ e i in (26) are ◦ ˙ e i = ◦ ˙ˆp i − ˙˜ T p i − ˜ T ˙p i = ◦ ˙ˆp i − ˆ T (cid:104) ˜ b U − W U (cid:105) ∧ ˆ T − ˜ T p i (44)From (43), one finds ˆ T (cid:104) ˜ b U (cid:105) ∧ ˆ T − = (cid:20) ˆ R ˆ P (cid:62) (cid:21) (cid:34) (cid:104) ˜ b Ω (cid:105) × ˜ b V (cid:35) (cid:20) ˆ R (cid:62) − ˆ R (cid:62) ˆ P (cid:62) (cid:21) = (cid:34) ˆ R (cid:104) ˜ b Ω (cid:105) × ˆ R (cid:62) ˆ R ˜ b V − ˆ R (cid:104) ˜ b Ω (cid:105) × ˆ R (cid:62) ˆ P (cid:62) (cid:35) = (cid:34) ˆ R ˜ b Ω ˆ R ˜ b V + (cid:104) ˆ P (cid:105) × ˆ R ˜ b Ω (cid:35) ∧ ∈ se (3) (45)where (cid:104) R ˜ b Ω (cid:105) × = R (cid:104) ˜ b Ω (cid:105) × R (cid:62) as defined in (10). Recallingthe definition of wedge operator in (3), one finds that (45)becomes ˆ T (cid:104) ˜ b U (cid:105) ∧ ˆ T − = (cid:34)(cid:34) ˆ R × (cid:104) ˆ P (cid:105) × ˆ R ˆ R (cid:35) ˜ b U (cid:35) ∧ (46)According to (46) and (44), one has ˆ T (cid:104) ˜ b U (cid:105) ∧ ˆ T − ˜ T p i = (cid:34)(cid:34) ˆ R × (cid:104) ˆ P (cid:105) × ˆ R ˆ R (cid:35) ˜ b U (cid:35) ∧ (cid:20) ˆ Ry i + ˆ P (cid:21) = (cid:34) − (cid:104) ˆ Ry i (cid:105) × ˆ R ˜ b Ω + ˆ R ˜ b V (cid:35) = (cid:20) − ˆ R [ y i ] × ˆ R (cid:62) (cid:62) (cid:21) ˜ b U (47)In view of (44) and (47), one can rewrite (44) as ◦ ˙ e i = ◦ ˙ˆp i − (cid:20) − ˆ R [ y i ] × ˆ R (cid:62) (cid:62) (cid:21) (cid:16) ˜ b U − W U (cid:17) (48)The last row in (48) are zeros, thereby, one obtains ˙ e i = ˙ˆp i − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:16) ˜ b U − W U (cid:17) (49) Consider the candidate Lyapunov function V = V (cid:16) e , . . . , e n , ˜ b U (cid:17) V = n (cid:88) i =1 α i e (cid:62) i e i + 12˜ b (cid:62) U Γ − ˜ b U (50)The time derivative of (50) is ˙ V = n (cid:88) i =1 α i e (cid:62) i ˙ e i − ˜ b (cid:62) U Γ − ˙ˆ b U = n (cid:88) i =1 α i e (cid:62) i ˙ˆp i − n (cid:88) i =1 α i e (cid:62) i (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:16) ˜ b U − W U (cid:17) − ˜ b (cid:62) U Γ − ˙ˆ b U (51)Replacing W U , ˙ˆ b U and ˙ˆp i with their expressions in (39), (40)and (41), respectively, one obtains ˙ V = − n (cid:88) i =1 k p α i (cid:107) e i (cid:107) − k w (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) n (cid:88) i =1 e i α i (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) − k w (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) n (cid:88) i =1 [ y i ] × ˆ R (cid:62) e i α i (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) (52)Based on (52) the time derivative of V is negative definitewhere ˙ V equals to zero at e i = . The result in (52)affirms that e i is exponentially regulated to the set S givenin (42). Based on Barbalat Lemma, ˙ V is negative, continuousand approaches the origin implying that ˜ T , ˜ b U , and ¨ e i staybounded. Also, the expression in (28) demonstrates that if e i → , then ˜p i − ˜ P → , and accordingly by (26) one has ˆp i − ˜ T p i → . As such, ˜ T is upper bounded with ˜ R → R c and ˜ P → P c as t → ∞ which completes the proof. B. Nonlinear Stochastic Filter Design with IMU
The nonlinear deterministic filter design in Subsection IV-Aallows e i = ˜p i − ˜ P → exponentially where ˜ P = ˆ P − ˜ RP and ˜p i = ˆp i − ˜ R p i . However, ˜ R → R c and ˜ P → P c as t → ∞ such that R c ∈ SO (3) and P c ∈ R are constants.Hence, for the case when R (0) and P (0) are not preciselyknown, the error in ˜ R , ˜ P , and ˜p i will become remarkablylarge, resulting in highly inaccurate pose and landmark esti-mates. Additionally, the nonlinear deterministic filter considersthe group velocity vector measurements associated with theSLAM dynamics in (23) to be noise free n U = 0 . Failing toincorporate the impact of noise may significantly underminethe effectiveness of the estimation process and destabilize theoverall closed loop dynamics. This behavior is exemplified bythe previously proposed solutions, for example [7,8]. Remark 2.
Define R c ∈ SO (3) and P c ∈ R as constants.It has been definitively proven that SLAM problem is notobservable [34], therefore, the best achievable solution is for ˜ R → R c , ˜ P → P c , and ˆp i → ˆ P + ˜ R p i − ˜ RP as t → ∞ . Based on the above discussion, the objective of this sub-section is to propose a nonlinear stochastic filter design forSLAM that is able to produce good performance throughvelocity, IMU, and landmark measurements regardless the initial value of pose and landmarks were accurately knownor not. Considering the body-frame measurements and theassociated normalization in (16) and (17), define M = M (cid:62) = n R (cid:88) j =1 s j υ rj (cid:0) υ rj (cid:1) (cid:62) , ∀ j = 1 , , . . . n R (53)with s j ≥ being a constant gain associated with theconfidence level of the j th sensor measurements. Notice that M in (53) is symmetric. Based on Remark 1, the availability ofa minimum two non-collinear body-frame measurements alongwith their inertial-frame observations is assumed ( n R ≥ )which can be satisfied by a low-cost IMU module. For n R = 2 ,the third measurement and its observations are calculatedusing cross product υ a = υ a × υ a and υ r = υ r × υ r .As such, rank ( M ) = 3 . Defining the eigenvalues of M as λ ( M ) = { λ , λ , λ } , one has λ , λ , λ > . Let ˘ M = Tr { M } I − M , given that rank ( M ) = 3 . Hence, rank( ˘ M ) = 3 as well allowing to conclude that ( [35] page.553):1) ˘ M is positive-definite.2) ˘ M has the following eigenvalues: λ ( ˘ M ) = { λ + λ , λ + λ , λ + λ } with λ ( ˘ M ) > being theminimum eigenvalue.In all of the following discussions it is assumed that rank ( M ) = 3 . Additionally, for j = 1 , , . . . , n R it is selectedthat (cid:80) n R j =1 s j = 3 signifying that Tr { M } = 3 .With the aim of proposing a stochastic filter design relianton a set of measurements, let us reintroduce the necessaryvariables in vectorial terms. From (16) and (17), as the truenormalized value of the j th body-frame vector is υ aj = R (cid:62) υ rj ,let ˆ υ aj = ˆ R (cid:62) υ rj , ∀ j = 1 , , . . . n R (54)Define the pose error analogously to (25) where ˜ R = ˆ RR (cid:62) .Based on the identities in (10) and (11), one has ˆ R n R (cid:88) j =1 s j υ aj × υ aj × = ˆ R n R (cid:88) j =1 s j (cid:16) υ aj (cid:0) ˆ υ aj (cid:1) (cid:62) − ˆ υ aj (cid:0) υ aj (cid:1) (cid:62) (cid:17) ˆ R (cid:62) = 12 ˆ RR (cid:62) M − M R ˆ R (cid:62) = P a ( ˜ RM ) This means Υ ( ˜ RM ) = vex ( P a ( ˜ RM )) = ˆ R n R (cid:88) j =1 (cid:16) s j υ aj × υ aj (cid:17) (55)Accordingly, ˜ RM is equivalent to ˜ RM = ˆ R n R (cid:88) j =1 (cid:16) s j υ aj (cid:0) υ rj (cid:1) (cid:62) (cid:17) (56) Recall that Tr { M } = 3 . From the definition in (7), one has E ˜ R = || ˜ RM || I = 14 Tr (cid:110) ( I − ˜ R ) M (cid:111) = 14 Tr I − ˆ R n R (cid:88) j =1 (cid:16) s j υ aj (cid:0) υ rj (cid:1) (cid:62) (cid:17) = 14 n R (cid:88) j =1 (cid:16) − s j (cid:0) ˆ υ aj (cid:1) (cid:62) υ aj (cid:17) (57)Also, note that − (cid:13)(cid:13)(cid:13) ˜ R (cid:13)(cid:13)(cid:13) I = 1 −
14 Tr (cid:110) I − ˜ R (cid:111) = 1 −
34 + 14 Tr { ˜ R } = 14 (cid:16) { ˜ R } (cid:17) (58)One may rewrite the above results (58) as − || ˜ R || I = 14 (cid:16) { ˜ RM M − } (cid:17) (59)In view of (53), (56) and (59), one obtains π ( ˜ R, M ) = Tr (cid:110) ˜ RM M − (cid:111) = Tr n R (cid:88) j =1 s j υ aj (cid:0) υ rj (cid:1) (cid:62) n R (cid:88) j =1 s j ˆ υ aj (cid:0) υ rj (cid:1) (cid:62) − (60)To this end, in the filter design it is considered that ˘ M =Tr { M } I − M , E ˜ R = || ˜ RM || I , π ( ˜ R, M ) , Υ ( ˜ RM ) , and e i are given relative to vector measurements as in (53), (57), (60),(55), and (27), respectively, for all i = 1 , , · · · , n . Considerthe following nonlinear stochastic filter: ˙ˆ T = ˆ T (cid:104) U m − ˆ b U − W U (cid:105) ∧ (61) ˙ˆ p i = − k (cid:37) e i + ˆ R [ y i ] × W Ω , i = 1 , , . . . , n (62) ˙ˆ b U = n (cid:88) i =1 Γ α i (cid:20) α i τ b ˆ R (cid:62) − [ y i ] × ˆ R (cid:62) × − ˆ R (cid:62) (cid:21) (cid:20) Υ ( ˜ RM ) (cid:107) e i (cid:107) e i (cid:21) − k b Γˆ b U (63) ˙ˆ σ = Γ σ τ σ diag (cid:16) ˆ R (cid:62) Υ ( ˜ RM ) (cid:17) ˆ R (cid:62) Υ ( ˜ RM ) − k σ Γ σ ˆ σ (64) W U = n (cid:88) i =1 α i (cid:34) α i (cid:16) k τ w I + E ˜ R +2 E ˜ R +1 diag (ˆ σ ) (cid:17) ˆ R (cid:62) Υ ( ˜ RM ) − k ˆ R (cid:62) (cid:107) e i (cid:107) e i (cid:35) (65)where τ b = ( E ˜ R + 1) exp ( E ˜ R ) , τ σ = ( E ˜ R + 2) exp ( E ˜ R ) , τ w = λ ( ˘ M ) (cid:16) π ( ˜ R, M ) (cid:17) , W U = (cid:2) W (cid:62) Ω , W (cid:62) V (cid:3) (cid:62) ∈ R is acorrection factor, and ˆ b U = (cid:104) ˆ b (cid:62) Ω , ˆ b (cid:62) V (cid:105) (cid:62) ∈ R is the estimate of b U . k , k , k , Γ σ , Γ = (cid:20) Γ × × Γ (cid:21) , and α i are positiveconstants. Theorem 2.
Consider combining the stochastic SLAM dynam-ics ˙ X = (cid:16) ˙ T , ˙p (cid:17) in (23) with landmark measurements (output y i = T − p i ) for all i = 1 , , . . . , n , inertial measurementunits υ aj = R (cid:62) υ rj for all j = 1 , , . . . n R , and velocitymeasurements ( U m = U + b U + n U ) where n U (cid:54) = 0 .Let Assumptions 1 and 2 hold, and let the filter design beas in (61) , (62) , (63) , (64) , and (65) . Consider the designparameters k > / , k b , k σ , k , k , Γ , Γ σ , and α i to bepositive constants and (cid:37) to be sufficiently small. Consider thefollowing set: S = (cid:110)(cid:16) ˜ R, e , e , . . . , e n (cid:17) ∈ SO (3) × R × R × · · · × R (cid:12)(cid:12)(cid:12) ˜ R = I , e i = ∀ i = 1 , , . . . n } (66) Then, 1) all the closed loop error signals are SGUUB in meansquare, and 2) the error (cid:16) ˜ R, e , e , . . . , e n (cid:17) converges to theclose neighborhood of S in probability for ˜ R (0) / ∈ U s .Proof: Due to the fact that ˙ˆ T in (61) is identical to (38),and in view of the pose error dynamics in (43) one has de i = (cid:16) ˙ˆp i − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:16) ˜ b U − W U (cid:17)(cid:17) dt − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) Q U dβ U = F i dt + G i Q U dβ U (67)where F i = ˙ˆp i − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:16) ˜ b U − W U (cid:17) and G i = − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) . Also, the attitude error dynamics are ˙˜ R = ˙ˆ RR (cid:62) + ˆ R ˙ R (cid:62) = ˆ R (cid:104) ˜ b Ω − W Ω (cid:105) × R (cid:62) + ˆ R (cid:20) Q Ω dβ Ω dt (cid:21) × R (cid:62) d ˜ R = (cid:104) ˆ R (˜ b Ω − W Ω ) (cid:105) × ˜ Rdt + (cid:104) ˆ R Q Ω dβ Ω (cid:105) × ˜ R (68)Recall the definition in (7) where E ˜ R = || ˜ RM || I = Tr (cid:110) ( I − ˜ R ) M (cid:111) . Thereby, after considering the identity in(15) one finds dE ˜ R = −
14 Tr (cid:26)(cid:104) ˆ R (˜ b Ω − W Ω ) (cid:105) × ˜ RM (cid:27) dt −
14 Tr (cid:26)(cid:104) ˆ R Q Ω dβ Ω (cid:105) × ˜ RM (cid:27) = −
14 Tr (cid:26) ˜ RM P a (cid:18)(cid:104) ˆ R (˜ b Ω − W Ω ) (cid:105) × (cid:19)(cid:27) dt −
14 Tr (cid:26) ˜ RM P a (cid:18)(cid:104) ˆ R Q Ω dβ Ω (cid:105) × (cid:19)(cid:27) = 12 vex (cid:16) P a ( ˜ RM ) (cid:17) (cid:62) ˆ R (˜ b Ω − W Ω ) dt + 12 vex (cid:16) P a ( ˜ RM ) (cid:17) (cid:62) ˆ R Q Ω dβ Ω = f dt + g Q Ω dβ Ω (69)where f = vex (cid:16) P a ( ˜ RM ) (cid:17) (cid:62) ˆ R (˜ b Ω − W Ω ) and g = vex (cid:16) P a ( ˜ RM ) (cid:17) (cid:62) ˆ R . It should be noted that ˙ M = 0 × according to the definition in (53). Since E ˜ R = || ˜ RM || I is greater than zero for all || ˜ RM || I (cid:54) = 0 orequivalently ˜ R (cid:54) = I and E ˜ R = 0 only at ˜ R = I , consider the V = n (cid:88) i =1 α i (cid:107) e i (cid:107) + E ˜ R exp ( E ˜ R ) + 12˜ b (cid:62) U Γ − ˜ b U + 12 ˜ σ (cid:62) Γ − σ ˜ σ (70)The first and the second partial derivatives of (70) with respect to E ˜ R and e i are (cid:40) V E ˜ R = ∂ V /∂E ˜ R = ( E ˜ R + 1) exp ( E ˜ R ) V E ˜ R E ˜ R = ∂ V /∂E R = ( E ˜ R + 2) exp ( E ˜ R ) (71) (cid:40) V e i = ∂ V /∂e i = (cid:107) e i (cid:107) e i V e i e i = ∂ V /∂e i = (cid:107) e i (cid:107) I + 2 e i e (cid:62) i (72)From (71) and (72), the differential operator L V in Definition 2 can be expressed as L V = n (cid:88) i =1 (cid:18) V (cid:62) e i F i + 12 Tr (cid:8) G i Q U G (cid:62) i V e i e i (cid:9)(cid:19) + V (cid:62) E ˜ R f + 12 Tr (cid:8) g Q g (cid:62) V E ˜ R E ˜ R (cid:9) − ˜ b (cid:62) U Γ − ˙ˆ b U − ˜ σ (cid:62) Γ − σ ˙ˆ σ = n (cid:88) i =1 α i (cid:107) e i (cid:107) e (cid:62) i (cid:16) ˙ˆp i − (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:16) ˜ b U − W U (cid:17)(cid:17) + 12 Tr (cid:40) n (cid:88) i =1 α i (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) Q U (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:62) (cid:16) (cid:107) e i (cid:107) I + 2 e i e (cid:62) i (cid:17)(cid:41) + 12 τ b (cid:2) Υ ( ˜ RM ) (cid:62) ˆ R (cid:62) (cid:3) (cid:16) ˜ b U − W U (cid:17) + 18 τ σ Υ ( ˜ RM ) (cid:62) ˆ R Q ˆ R (cid:62) Υ ( ˜ RM ) − ˜ b (cid:62) U Γ − ˙ˆ b U − ˜ σ Γ − σ ˙ˆ σ (73)where τ b = ( E ˜ R + 1) exp ( E ˜ R ) and τ σ = ( E ˜ R + 2) exp ( E ˜ R ) with both τ b and τ σ being positive for all t ≥ . One can easilyfind that Tr (cid:110) (cid:107) e i (cid:107) I + 2 e i e (cid:62) i (cid:111) ≤ Tr (cid:110) (cid:107) e i (cid:107) I (cid:111) ≤ (cid:107) e i (cid:107) Based on the result above and by the virtue of Young’s inequality in Lemma 3, one finds
92 Tr (cid:40) n (cid:88) i =1 α i (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) Q U (cid:2) − ˆ R [ y i ] × ˆ R (cid:3) (cid:62) (cid:41) (cid:107) e i (cid:107) ≤≤ n (cid:88) i =1 α i (cid:37) (cid:107) e i (cid:107) + n (cid:88) i =1 (cid:37) α i Tr (cid:110) ˆ R (cid:16) [ y i ] (cid:62)× diag ( σ ) [ y i ] × + diag ( σ ) (cid:17) ˆ R (cid:62) (cid:111) (74)Note that σ is the upper bound of Q U . Due to the fact that ˆ R ∈ SO (3) which is orthogonal, for any M ∈ R one has Tr (cid:110) ˆ RM ˆ R (cid:62) (cid:111) = Tr { M } . Also, let y i be the upper bound of y i . As such, define c = n (cid:88) i =1 (cid:37) α i Tr (cid:8) − [ y i ] × diag ( σ ) [ y i ] × + diag ( σ ) (cid:9) (75)From (74) and (75), one may express (73) in inequality form as L V ≤ n (cid:88) i =1 α i (cid:107) e i (cid:107) e (cid:62) i ˙ˆp i + n (cid:88) i =1 α i (cid:20) Υ ( ˜ RM ) (cid:107) e i (cid:107) e i (cid:21) (cid:62) (cid:34) α i τ b ˆ R × ˆ R [ y i ] × − ˆ R (cid:35) (cid:16) ˜ b U − W U (cid:17) + 18 τ σ Υ (cid:16) ˜ RM (cid:17) (cid:62) ˆ R (cid:104) ˆ R (cid:62) Υ (cid:16) ˜ RM (cid:17)(cid:105) D σ + n (cid:88) i =1 α i (cid:37) (cid:107) e i (cid:107) + c − ˜ b (cid:62) U Γ − b ˙ˆ b U − ˜ σ (cid:62) Γ − σ ˙ˆ σ (76) Directly substituting W U , ˙ˆ b U , ˙ˆ σ and ˙ˆp i with the definitions in (62), (63), (64), and (65), respectively, one shows L V ≤ − n (cid:88) i =1 k α i − / (cid:37)α i (cid:107) e i (cid:107) − k (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) n (cid:88) i =1 (cid:107) e i (cid:107) α i ˆ Re i (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) − k τ b τ w (cid:13)(cid:13)(cid:13) Υ (cid:16) ˜ RM (cid:17)(cid:13)(cid:13)(cid:13) + k b ˜ b (cid:62) U ˆ b U + k σ ˜ σ (cid:62) ˆ σ + c (77)As a result of (33) in Lemma 1, one has τ w || Υ (cid:16) ˜ RM (cid:17) || = 2 τ w (cid:13)(cid:13)(cid:13) Υ (cid:16) ˜ RM (cid:17)(cid:13)(cid:13)(cid:13) ≥ (cid:13)(cid:13)(cid:13) ˜ RM (cid:13)(cid:13)(cid:13) I (78)where E ˜ R = || ˜ RM || I and τ w = λ ( ˘ M ) (cid:16) π ( ˜ R, M ) (cid:17) = λ ( ˘ M ) (cid:16) (cid:110) ˜ RM M − (cid:111)(cid:17) . Also k b ˜ b (cid:62) U b U ≤ k b (cid:107) b U (cid:107) + k b (cid:13)(cid:13)(cid:13) ˜ b U (cid:13)(cid:13)(cid:13) k σ ˜ σ (cid:62) σ ≤ k σ (cid:107) σ (cid:107) + k σ (cid:107) ˜ σ (cid:107) According to the above result and (78), one can rewrite (77) as L V ≤ − n (cid:88) i =1 k α i − / (cid:37)α i (cid:107) e i (cid:107) − k (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) n (cid:88) i =1 (cid:107) e i (cid:107) α i ˆ Re i (cid:13)(cid:13)(cid:13)(cid:13)(cid:13) − k E ˜ R + 1) exp ( E ˜ R ) E ˜ R − k b (cid:13)(cid:13)(cid:13) ˜ b U (cid:13)(cid:13)(cid:13) − k σ (cid:107) ˜ σ (cid:107) + k b (cid:107) b U (cid:107) + k σ (cid:107) σ (cid:107) + c (79)Define τ i = k α i − / (cid:37)α i , ∀ i = 1 , , . . . , nη = k b (cid:107) b U (cid:107) + k σ (cid:107) σ (cid:107) + c Also, let H = τ I · · · × ... . . . ... × · · · τ n I n × × n k × × × k b Γ 0 × × × k σ Γ σ ˜ Y = α I · · · × ... . . . ... × · · · α n I n × × n × × × Γ − × × × Γ − σ / (cid:107) e (cid:107) √ α e ... (cid:107) e n (cid:107) √ α n e n (cid:112) E ˜ R exp ( E ˜ R )˜ b U ˜ σ Therefore, the differential operator in (79) becomes L V ≤ − λ ( H ) V + η (80)where λ ( · ) represents the minimum eigenvalue of a matrix.Based on (80), one finds d ( E [ V ]) dt = E [ L V ] ≤ − λ ( H ) E [ V ] + η (81)Let c = E [ V ( t )] ; hence d ( E [ V ]) dt ≤ for λ ( H ) > η c . Assuch, V ≤ c is an invariant set and for E [ V (0)] ≤ c there is E [ V ( t )] ≤ c ∀ t > . In view of Lemma 2, the inequality in (81) holds for V (0) ≤ c and for all t > such that ≤ E [ V ( t )] ≤ V (0) exp ( − λ ( H ) t ) + η λ ( H ) , ∀ t ≥ (82)Hence, E [ V ( t )] is eventually bounded by η /λ ( H ) in turnimplying ˜ Y is SGUUB in the mean square. Therefore, theresult in (79) guarantees that e i as well as ˜ R are regulatedto the neighborhood of the set S defined in (66) for all i =1 , , . . . , n and ˜ R (0) / ∈ U s . In addition, ˜ P → P c as t → ∞ .This completes the proof.Algorithm 1 details the implementation stages of the non-linear stochastic filter for SLAM defined in in (61), (62), (63), Algorithm 1
Implementation steps of the nonlinear stochasticfilter for SLAM
Initialization : Set ˆ R (0) ∈ SO (3) and ˆ P (0) ∈ R . Instead, establish ˆ R (0) ∈ SO (3) using any method of attitude determina-tion, see [36] Set ˆp i (0) ∈ R for all i = 1 , , . . . , n Set ˆ b Ω (0) , , ˆ b V (0) , ˆ σ (0) ∈ R Select k w , k w , k p , Γ , Γ σ , (cid:37) , k b , k σ , and α i as positiveconstants while (1) do for j = 1 : n R Measurements and observations as in (16) υ rj = r j (cid:107) r j (cid:107) , υ aj = a j (cid:107) a j (cid:107) as in (17) ˆ υ aj = ˆ R (cid:62) υ rj as in (54) end for M = (cid:80) n R j =1 s j υ rj (cid:0) υ rj (cid:1) (cid:62) as in (53) with ˘ M =Tr { M } I − M Υ = ˆ R (cid:80) n R j =1 (cid:0) s j ˆ υ aj × υ aj (cid:1) as in (55) π = Tr (cid:26)(cid:16)(cid:80) n R j =1 s j υ aj (cid:0) υ rj (cid:1) (cid:62) (cid:17) (cid:16)(cid:80) n R j =1 s j ˆ υ aj (cid:0) υ rj (cid:1) (cid:62) (cid:17) − (cid:27) as in (60) for i = 1 : n e i = ˆp i − ˆ Ry i − ˆ P as in (27) end for W Ω = (cid:16) k τ w I + E ˜ R +2 E ˜ R +1 diag (ˆ σ ) (cid:17) ˆ R (cid:62) Υ , with τ R = λ ( ˘ M ) × (1 + π ) W V = − (cid:80) ni =1 k α i (cid:107) e i (cid:107) ˆ R (cid:62) e i ˙ˆ R = ˆ R (cid:104) Ω m − ˆ b Ω − W Ω (cid:105) × ˙ˆ P = ˆ R (cid:16) V m − ˆ b V − W V (cid:17) for i = 1 : n ˙ˆ p i = − k (cid:37) e i + ˆ R [ y i ] × W Ω end for ˙ˆ b Ω = Γ τ b ˆ R (cid:62) Υ − (cid:80) ni =1 Γ α i (cid:107) e i (cid:107) [ y i ] × ˆ R (cid:62) e i − k b Γ ˆ b Ω ˙ˆ b V = − (cid:80) ni =1 Γ α i (cid:107) e i (cid:107) ˆ R (cid:62) e i − k b Γ ˆ b V ˙ˆ σ = Γ σ τ σ diag (cid:16) ˆ R (cid:62) Υ (cid:17) ˆ R (cid:62) Υ − k σ Γ σ ˆ σ end while (64), and (65). V. N UMERICAL R ESULTS
A. Simulation
This section demonstrates the robustness of the proposedstochastic estimator for SLAM on
SLAM n (3) Lie group.Consider the true angular and translational velocities of thevehicle in 3D space to be
Ω = [0 , , . (cid:62) (rad / sec) and V = [2 . , , (cid:62) (m / sec) , respectively. Also, let its true initialpose be R (0) = I , P (0) = [0 , , (cid:62) Let four landmarks be fixed with respect to {I} within theunknown environment and positioned at p = [6 , , (cid:62) , p =[ − , , (cid:62) , p = [0 , , (cid:62) , and p = [0 , − , (cid:62) . Let angularand translational velocities be corrupted by unknown constant bias b U = (cid:2) b (cid:62) Ω , b (cid:62) V (cid:3) (cid:62) with b Ω = [0 . , − . , − . (cid:62) (rad / sec) and b V = [0 . , . , − . (cid:62) (m / sec) , respectively. Ad-ditionally, assume that the group velocity vector is al-tered by unknown noise n U = (cid:2) n (cid:62) Ω , n (cid:62) V (cid:3) (cid:62) with n Ω = N (0 , .
2) (rad / sec) and n V = N (0 , .
2) (m / sec) . It shouldbe noted that abbreviation n Ω = N (0 , . indicates thatthe random noise vector n Ω is normally distributed arounda zero mean with a standard deviation of . . Considertwo non-collinear inertial-frame observations equal to r =[ − , , . (cid:62) and r = [0 , , . (cid:62) where the associated body-frame measurements are defined as in (16). As was indicatedby Remarks 1, the third observation and measurement canbe calculated using a cross product of the two availableobservations. To account for large error in initialization, theinitial estimate of attitude and position are set as ˆ R (0) = . − . . . , ˆ P (0) = [0 , , (cid:62) The four landmark estimates are initiated at positions: ˆp (0) =ˆp (0) = ˆp (0) = ˆp (0) = [0 , , (cid:62) . The design parametersare selected as α i = 0 . , Γ = 3 I , Γ = 10000 I , Γ σ = 10 , k = 10 , k = 10 , k = 10 , k σ = 0 . , and (cid:37) = 0 . while theinitial values of bias and covariance estimates are ˆ b U (0) = and ˆ σ (0) = , respectively, for all i = 1 , , , . Also, select k b = 10 − as a very small constant.Figure 3 highlights the contrast between the true andmeasured values of angular and translational velocities. Theevolution of estimate trajectories output by the proposedSLAM nonlinear stochastic filter is depicted in Figure 3. Asdemonstrated by Figure 3, despite large initialization error, therobot’s position converged smoothly and continuously fromthe zero point of origin to the true trajectory of travel arrivingat the desired terminal point. Analogously, landmark estimates,initiated at the origin, rapidly diverged to their true locations.The asymptotic convergence of the error trajectories of e i achieved by the nonlinear filter for SLAM with IMU(stochastic) and without IMU (deterministic) is demonstratedin Figure 4 for all i = 1 , , , . Consider the error definedas || ˜ R || I = Tr { I − ˜ R } where ˜ R = ˆ RR (cid:62) , ˜ P = ˆ P − ˜ RP ,and ˜p i = ˆp i − ˜ R p i . It is apparent that e i = ˜p i − ˜ P doesnot necessarily result in || ˜ R || I → , ˜ P → , and ˜p i → .When designing a SLAM filter, convergence of ˜ R , ˜ P , and ˜p i to a constant does not constitute the ultimate goal. Thetrue objective is to drive || ˜ R || I → , || P − ˆ P || → , and || p i − ˆp i || → . As such, Figure 5 benchmarks the outputperformance of the proposed stochastic estimator for SLAMwith IMU highlighting its superiority over the deterministicsolution without IMU. Actually, IMU facilitates achieving ˜ R → I which in turn leads to || ˜ R || I → as t → ∞ significantly reducing error values of || P − ˆ P || and || p i − ˆp i || .This indeed is true as ˜ P = ˆ P − ˜ RP , and ˜p i = ˆp i − ˜ R p i causing ˜ R to strongly influence the values of ˜ P and ˜p i .Figure 5 reveals the robustness of the proposed stochasticestimator for SLAM using IMU. Figure 5 illustrating its strongconvergence as well as tracking capabilities. In contrast, ascan be clearly seen in Figure 5, the deterministic nonlinearestimator without IMU shows unreasonable performance in Fig. 2. Angular and translational velocities: measured plotted in coloredsolid-line vs true plotted in black center-line. agreement with [7,8]. It should be noted that presence of theresidual error is unavoidable for || P − ˆ P || and || p i − ˆp i || for theproposed stochastic filter illustrated by Figure 6. Nonetheless,the nonlinear stochastic filter proposed in Subsection IV-Boutperforms the nonlinear deterministic filter presented inSubsection IV-A in terms of the convergence rate of || ˜ R || I and || P − ˆ P || by a wide margin. B. Experimental Validation
To further validate the proposed nonlinear stochastic estima-tor for SLAM, the algorithm has been tested on a real-worldEuRoc dataset [37]. The data set includes 1) the true orienta-tion and position trajectory of the unmanned aerial vehicle, 2)IMU data, and 3) stereo images. Due to the fact that the datasetdoes not include landmark information, four landmarks fixedwith respect to {I} have been positioned at p = [3 , , (cid:62) , p = [ − , , (cid:62) , p = [0 , , (cid:62) , and p = [0 , − , (cid:62) .The four landmark estimates are initiated at the followingpositions: ˆp (0) = ˆp (0) = ˆp (0) = ˆp (0) = [0 , , (cid:62) .In spite of the large initialization error, Figure 7 demonstratessmooth and continuous convergence of the robot’s positionfrom the origin to the true trajectory successfully arrivingto the desired destination. Likewise, Figure 7 shows theconvergence of the estimated landmarks from the origin totrue locations. VI. C ONCLUSION
To truly capture the nonlinear structure of the motion dy-namics of Simultaneous Localization and Mapping (SLAM),a nonlinear stochastic filter for SLAM on the Lie group of
SLAM n (3) is proposed. The proposed stochastic filter takes Fig. 3. Output performance of the proposed nonlinear stochastic filter forSLAM described in Subsection IV-B and detailed in Algorithm 1 plottedagainst the true robot’s position and landmark locations in 3D space. The truerobot trajectory is plotted in black solid-line with the black circle markingits terminal point. The black circles also mark the true fixed landmarks.Estimation of the robot’s position is plotted as a blue center-line initiatingat the origin and converging to its final location marked with a blue star (cid:63) . Landmark estimation trajectories depicted as red dashed-lines initiate at (0 , , and diverge to their final positions marked with red stars (cid:63) . Fig. 4. Error trajectories utilized in the Lyapunov function candidate.The proposed nonlinear stochastic estimator for SLAM with IMU outlinedin Subsection IV-B is depicted in blue against the deterministic nonlinearestimator for SLAM presented in Subsection IV-A depicted in red. into account the unknown constant bias and random noise Fig. 5. Output performance of || ˆ RR (cid:62) || I , || P − ˆ P || and || p i − ˆp i || forall i = 1 , , , . All colors other than red represent the proposed nonlinearstochastic filter based on velocity, landmark, and IMU measurements, whilered represents the nonlinear filter based only on velocity and landmarkmeasurements. Det and Stoch abbreviate deterministic and stochastic filters,respectively. -4 Fig. 6. Steady-state values of || ˆ RR (cid:62) || I , || P − ˆ P || and || p i − ˆp i || for all i = 1 , , , of the proposed nonlinear stochastic filter for SLAM. corrupting the velocity measurements. The proposed filterdirectly incorporates angular and translational velocity, land-mark, and IMU measurements. The closed loop error signalshave been shown to be semi-globally uniformly ultimatelybounded (SGUUB) in mean square. Numerical results con-clusively prove filter’s ability to localize the unknown robot’spose and simultaneously map the unknown environment.A CKNOWLEDGMENT
The authors would like to thank
Maria Shaposhnikova forproofreading the article.
Fig. 7. Experimental results using dataset Vicon Room 2 01. R EFERENCES[1] S. Thrun et al. , “Robotic mapping: A survey,”
Exploring artificialintelligence in the new millennium , vol. 1, no. 1-35, p. 1, 2002.[2] H. A. Hashim, L. J. Brown, and K. McIsaac, “Nonlinear pose filterson the special euclidean group SE(3) with guaranteed transient andsteady-state performance,”
IEEE Transactions on Systems, Man, andCybernetics: Systems , vol. PP, no. PP, pp. 1–14, 2019.[3] D. E. Zlotnik and J. R. Forbes, “Higher order nonlinear complementaryfiltering on lie groups,”
IEEE Transactions on Automatic Control ,vol. 64, no. 5, pp. 1772–1783, 2018.[4] J. Guo, Y. He, X. Qi, G. Wu, Y. Hu, B. Li, and J. Zhang, “Real-time measurement and estimation of the 3d geometry and motionparameters for spatially unknown moving targets,”
Aerospace Scienceand Technology , vol. 97, p. 105619, 2020.[5] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-ping: part i,”
IEEE robotics & automation magazine , vol. 13, no. 2, pp.99–110, 2006.[6] V. Sazdovski, A. Kitanov, and I. Petrovic, “Implicit observation modelfor vision aided inertial navigation of aerial vehicles using single cameravector observations,”
Aerospace science and technology , vol. 40, pp. 33–46, 2015.[7] H. A. Hashim, “Guaranteed performance nonlinear observer for simulta-neous localization and mapping,”
IEEE Control Systems Letters , vol. 5,no. 1, pp. 91–96, 2021.[8] D. E. Zlotnik and J. R. Forbes, “Gradient-based observer for simultane-ous localization and mapping,”
IEEE Transactions on Automatic Control ,vol. 63, no. 12, pp. 4338–4344, 2018.[9] M. Li and B. Xu, “Autonomous orbit and attitude determination for earthsatellites using images of regular-shaped ground objects,”
AerospaceScience and Technology , vol. 80, pp. 192–202, 2018.[10] M. J. Milford and G. F. Wyeth, “Mapping a suburb with a singlecamera using a biologically inspired slam system,”
IEEE Transactionson Robotics , vol. 24, no. 5, pp. 1038–1053, 2008.[11] R. Sim, P. Elinas, and J. J. Little, “A study of the rao-blackwellisedparticle filter for efficient and accurate vision-based slam,”
InternationalJournal of Computer Vision , vol. 74, no. 3, pp. 303–318, 2007.[12] E. Eade and T. Drummond, “Scalable monocular slam,” in , vol. 1. IEEE, 2006, pp. 469–476.[13] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert,“isam2: Incremental smoothing and mapping with fluid relinearizationand incremental variable reordering,” in . IEEE, 2011, pp. 3281–3288.[14] G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “A quadratic-complexity observability-constrained unscented kalman filter for slam,”
IEEE Transactions on Robotics , vol. 29, no. 5, pp. 1226–1243, 2013.[15] M. Barczyk, S. Bonnabel, J.-E. Deschaud, and F. Goulette, “Experi-mental implementation of an invariant extended kalman filter-based scanmatching slam,” in . IEEE, 2014,pp. 4121–4126. [16] G. Dissanayake, S. Huang, Z. Wang, and R. Ranasinghe, “A review ofrecent developments in simultaneous localization and mapping,” in .IEEE, 2011, pp. 477–482.[17] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira,I. Reid, and J. J. Leonard, “Past, present, and future of simultaneouslocalization and mapping: Toward the robust-perception age,” IEEETransactions on robotics , vol. 32, no. 6, pp. 1309–1332, 2016.[18] H. A. Hashim, L. J. Brown, and K. McIsaac, “Nonlinear stochasticattitude filters on the special orthogonal group 3: Ito and stratonovich,”
IEEE Transactions on Systems, Man, and Cybernetics: Systems , vol. 49,no. 9, pp. 1853–1865, 2019.[19] H. A. Hashim, “Systematic convergence of nonlinear stochastic estima-tors on the special orthogonal group SO(3),”
International Journal ofRobust and Nonlinear Control , vol. 30, no. 10, pp. 3848–3870, 2020.[20] K. J. Jensen, “Generalized nonlinear complementary attitude filter,”
Journal of Guidance, Control, and Dynamics , vol. 34, no. 5, pp. 1588–1593, 2011.[21] M. Zamani, J. Trumpf, and R. Mahony, “Minimum-energy filtering forattitude estimation,”
IEEE Transactions on Automatic Control , vol. 58,no. 11, pp. 2917–2921, 2013.[22] H. A. Hashim and F. L. Lewis, “Nonlinear stochastic estimators onthe special euclidean group SE(3) using uncertain imu and visionmeasurements,”
IEEE Transactions on Systems, Man, and Cybernetics:Systems , vol. PP, no. PP, pp. 1–14, 2020.[23] H. Strasdat, “Local accuracy and global consistency for efficient visualslam,” Ph.D. dissertation, Department of Computing, Imperial CollegeLondon, 2012.[24] T. A. Johansen and E. Brekke, “Globally exponentially stable kalmanfiltering for slam with ahrs,” in . IEEE, 2016, pp. 909–916.[25] H. A. Hashim and A. E. E. Eltoukhy, “Landmark and imu data fusion:Systematic convergence geometric nonlinear observer for slam andvelocity bias,”
IEEE Transactions on Intelligent Transportation Systems ,vol. PP, no. PP, pp. 1–10, 2020.[26] ——, “Nonlinear filter for simultaneous localization and mapping on amatrix lie group using imu and feature measurements,”
IEEE Transac-tions on Systems, Man, and Cybernetics: Systems , vol. PP, no. PP, pp.1–12, 2021.[27] V. Stojanovic, S. He, and B. Zhang, “State and parameter joint estimationof linear stochastic systems in presence of faults and non-gaussiannoises,”
International Journal of Robust and Nonlinear Control , vol. 30,no. 16, pp. 6683–6700, 2020.[28] R. Khasminskii,
Stochastic stability of differential equations . Rockville,MD: S & N International, 1980.[29] A. H. Jazwinski,
Stochastic processes and filtering theory . CourierCorporation, 2007.[30] K. Ito and K. M. Rao,
Lectures on stochastic processes . Tata instituteof fundamental research, 1984, vol. 24.[31] H. Deng, M. Krstic, and R. J. Williams, “Stabilization of stochasticnonlinear systems driven by noise of unknown covariance,”
IEEETransactions on Automatic Control , vol. 46, no. 8, pp. 1237–1253, 2001.[32] S. Tong, Y. Li, Y. Li, and Y. Liu, “Observer-based adaptive fuzzybackstepping control for a class of stochastic nonlinear strict-feedbacksystems,”
IEEE Transactions on Systems, Man, and Cybernetics, Part B(Cybernetics) , vol. 41, no. 6, pp. 1693–1704, 2011.[33] H.-B. Ji and H.-S. Xi, “Adaptive output-feedback tracking of stochasticnonlinear systems,”
IEEE Transactions on Automatic Control , vol. 51,no. 2, pp. 355–360, 2006.[34] K. W. Lee, W. S. Wijesoma, and J. I. Guzman, “On the observabilityand observability analysis of slam,” in . IEEE, 2006, pp. 3569–3574.[35] F. Bullo and A. D. Lewis,
Geometric control of mechanical systems:modeling, analysis, and design for simple mechanical control systems .Springer Science & Business Media, 2004, vol. 49.[36] H. A. Hashim, “Attitude determination and estimation using vector ob-servations: Review, challenges and comparative results,” arXiv preprintarXiv:2001.03787 , 2020.[37] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,”
The International Journal of Robotics Research , vol. 35, no. 10, pp.1157–1163, 2016.
AUTHOR INFORMATION