Extended Kalman filter (EKF) is often employed in determining the position of mobile robot and landmarks in simultaneous localization and mapping (SLAM). Nonetheless, there are some disadvantages of using EKF, namely, the requirement of Gaussian distribution for the state and noises, as well as the fact that it requires the smallest possible initial state covariance. This has led researchers to find alternative ways to mitigate the aforementioned shortcomings. Therefore, this study is conducted to propose an alternative technique by implementing H∞ filter in SLAM instead of EKF. In implementing H∞ filter in SLAM, the parameters of the filter especially γ need to be properly defined to prevent finite escape time problem. Hence, this study proposes a sufficient condition for the estimation purposes. Two distinct cases of initial state covariance are analysed considering an indoor environment to ensure the best solution for SLAM problem exists along with considerations of process and measurement noises statistical behaviour. If the prescribed conditions are not satisfied, then the estimation would exhibit unbounded uncertainties and consequently results in erroneous inference about the robot and landmarks estimation. The simulation results have shown the reliability and consistency as suggested by the theoretical analysis and our previous findings.
1. Introduction
The existence of uncertainties in various forms has caused numerous applications to function not as intended. Until today, their existence is almost inevitable; nonetheless, some devices have been proposed to mitigate such a problem. Although a number of the devices were proven to have succeeded in filtering the noise, the solution still demands further improvements. Owing to the existence of this problem, a robotics application known as simultaneous localization and mapping (SLAM) has experienced difficulties in increasing the efficiency of its estimation. SLAM provides a condition where a mobile robot is assigned to observe an unknown environment and incrementally constructs a map showing the environment that it has recognized. The mobile robot then attempts to localize itself on the constructed map recursively until its task is achieved. SLAM has been applied in a wide range of applications such as mining, underwater, and space exploration [1–5] through various techniques, for example, 3D visualization [5, 6], multiple robot navigation [7–9], vision-based strategies [10, 11], learning strategies through artificial intelligence [9, 12, 13], and intermittent observation issues [14–16]. Figure 1 depicts a simple notion of SLAM.
Simultaneous localization and mapping: mobile robot makes relative measurements with some uncertainties.
A considerable approach that currently seems to be able to tolerate uncertainties in SLAM relies on probabilistic technique. This technique which is based on Bayesian approach is more preferred as compared to the behaviour-based SLAM and mathematical-based SLAM owing to their complexity as well as high computational cost [4]. The probability-based approach takes into account these limitations reasonably well as it does not require extensive mathematical computation nor the demand of high reliability sensors for the position estimation. The extended Kalman filter (EKF) is an example of one of the many probabilistic techniques available. EKF’s application in SLAM gained popularity since the early 2000s and still receives high attention among researchers [17–22]. Early development of EKF-based SLAM was proposed by Gamini Dissanayake et al. [17, 19] notably due to its ease of application as well as lower computational cost as compared to other probabilistic approaches. Unfortunately, SLAM demands further considerations for the environmental conditions. An assumption of Gaussian noise has restricted EKF performance as the main player thus allowing space for a more robust approach such as the particle filter (PF) [23, 24]. Nonetheless, PF has its drawbacks owing to its complexity as well as higher computational cost. Therefore, in this circumstance, H∞ filter approach for SLAM is proposed to mitigate the aforementioned issues as this particular technique is more robust as compared to the EKF as far as the non-Gaussian noise is concerned and it has a much lower computational cost as compared to the PF. A brief description of the filter is discussed in [25].
In this paper, H∞ filter-based SLAM performance is further analysed to aid previous work conducted by [2, 26–28]. One of the earliest applications of this technique was reported by [2]. It has been proven that the H∞ filter is an alternative solution for SLAM problem in an underwater application. The filter performance has been compared to PF and EKF to identify the performance between each technique. Even though PF has shown better results on the estimation, H∞ filter is suggested to be the best solution especially when computational cost and non-Gaussian noise environments are taken into consideration. Further investigations were made on the filter convergence properties as reported by [26, 27, 29].
Despite what H∞ filter could offer to SLAM, unlike EKF, H∞ filter solution can unboundedly increase and exhibit finite escape time problem as reported by [26, 30]. Therefore, in order to apply H∞ filter efficiently to SLAM, the designer must carefully design the filter parameters to achieve the desired and expected performance. Hence, further inspection regarding the filter characteristics has been proposed. Bolzern and Maroni [31] discovered that H∞ filter must also satisfy P0=R-1 to achieve better estimation. A study for both filtering and prediction stages has been proposed and it was found that, under feasibility and sufficient conditions, the filter is able to achieve a stable result. Furthermore, [27] proposed the covariance inflation [32, 33] and γ-switching strategy as an additional technique to prevent the occurrence of finite escape time. Experimental results supported their analysis and demonstrated that the methods may alternatively avoid finite escape time.
Motivated by the preceding works, further analyses of the H∞ filter-based SLAM are proposed to gain more insight into the sufficient conditions for estimation purposes. Based on the preliminary results obtained through theoretical analysis and simulations, it suggests that if some conditions are satisfied, the H∞ filter provides a better estimation while at the same time refraining the existence of finite escape timein the estimation. The results obtained are also in good agreement with the previous results. Nevertheless, it is noteworthy to mention that there is also some trade-off between γ and the design parameters especially between the initial state covariance, process, and measurement noises distributions. Besides, as there are many types of environment available, two conditions of different initial state covariance are examined to understand its effect on SLAM with consideration for the process and measurement noises distributions. The analyses are shown to provide the filter characteristics in different situations of environmental conditions.
This paper is organized as follows. Section 2 describes the problem formulation about H∞ filter-based SLAM, followed by Section 3 which examines the convergency of H∞ filter for SLAM under some conditions. Next, some simulation results which consist of two cases of environmental conditions are demonstrated to evaluate our proposal in Section 4. Finally, Section 5 concludes this paper.
2. Problem Statement
The H∞ filter-based SLAM is introduced and the model used throughout the study is explained in this section. The state is formulated as a joint state vector of the robot position and the location of the landmarks observed in the environment. The state vector Xk∈R3+2m at time k is represented as
(1)Xk=XrXm,
where Xr=θkxkykT is the position of the mobile robot that is represented by robot heading angle θk and coordinates of the centre of the mobile robot with respect to the global coordinate frame (xk,yk). While Xm∈R2m, m=1,2,…,N is the location of each respective landmark of xi, yi(i=1,2,…,l) that exist in the environment. The landmarks are modelled as point landmarks.
2.1. Mobile Robot SLAM Model
Basically, there are two general steps in providing the system behaviour during mobile robot observations, that is, process and measurement models. Both models are vital to provide necessary information about the surroundings. Moreover, these two models actually ensemble important knowledge about the robot motions and relative information between robot and landmarks which are iteratively updated. A nonlinear discrete-time dynamical system is considered to describe the problem explicitly through process and measurement model as illustrated in Figure 2.
Process and measurement model of SLAM.
The process model of mobile robot in SLAM from time k to time k+1 is described as
(2)Xk+1=FXk+Buk+Gwk,(3)≡fXk,ωk,νk,δω,δν,
where Xk is the state of the mobile robot and landmarks as defined in (1), uk=ωkνkT∈Rp designates the control input, and wk is the process noise. F∈R(3+2m)×(3+2m), B∈R(3+2m)×(p), and G∈R(3+2m)×(3+2m) are the state transition matrix, control matrix, and noise covariance matrix, respectively. Since SLAM of a mobile robot is a nonlinear system, process model is defined in a nonlinear form as shown in (3). A model of two-wheel mobile robot (unicycle) is used in this study. Hence, the process model to define the kinematic movement of the mobile robot at time k+1 is shown as
(4)θk+1=θk+fθωk,νk,δω,δν,(5)xk+1=xk+νk+δνTcosθk,(6)yk+1=yk+νk+δνTsinθk,(7)Xm,k+1=Xm,k,
where ωk is mobile robot angular acceleration and νk is its velocity with associated noises, δω and δν. T is the sampling rate or the time interval of one movement step. The process model of the landmarks Xm,k+1=[xiyi]T for i=1,2,…,l is unchanged with zero noise as landmarks are assumed to be stationary, (7). Besides, the data association for SLAM is assumed perfectly given in this study.
During observation, mobile robot makes measurements using its exteroceptive sensors and the measurement model is shown as follows:
(8)zi=riφi=HiXk+υriφi,
where Hi is the measurement matrix and υriφi is the noise with covariance matrix Rk. HiXk defines that the measurement done by the mobile robot at time k is linear. However, for a nonlinear system Jacobian transformation of Hi is required as shown in (16) later. At time k + 1, the observation of ith landmark is a range ri and bearing φi which indicates relative distance and angle from mobile robot to any observed landmarks. It is assumed that the sensors on the robot are equipped with a range and bearing sensors that perform the observations of the landmarks in the environment and also encoders at the wheels for the measurement of vehicle speed. Range and bearing are defined as
(9)ri=yi-yk+12+xi-xk+12+υri,φi=arctanyi-yk+1xi-xk+1-θk+1+υφi,
where (xk+1,yk+1,θk+1) is current robot position, (xi,yi) is the position of observed landmark, and υri and υφi are the noises of both distance and angle measurements.
2.2. <inline-formula>
<mml:math xmlns:mml="http://www.w3.org/1998/Math/MathML" id="M65">
<mml:mi>H</mml:mi>
<mml:mi>∞</mml:mi></mml:math>
</inline-formula> Filter-Based SLAM
The H∞ filter algorithm shows approximately the same structure to the well-known extended Kalman filter (EKF). The presence of γ in the state error covariance owns the essence of H∞ filter and acts as the main difference to EKF algorithm. H∞ filter theoretically denotes that, for a given γ>0, the filter attempts to find a solution for an estimated state X^k that satisfies the following criteria:
(10)γ>supX0,υ,w∑k=0NXk-X^kX0-X-0P0-12+∑k=0NυkRk-12+∑k=0NwkQk-121/2,
where X0, Xk∈R3+2m is the robot (∈R3) and landmarks (∈R2m,m=1,2,…,N) states. wk and υk are the process and measurement noises with covariance of Qk≥0 and Rk>0, respectively, and P0>0 is the initial state covariance. The above equation alternatively means that the estimation error to the noise ratio is less than a certain level of γ. This method also assumes that the noise distributions are statistically bounded.
Similar to EKF, the state is predicted based on the system’s previous information and then is estimated based on the measurements data obtained from the sensors. In H∞ filter algorithm, the prediction step which starts from an initial state is given by
(11)X^k+1-=fkX^k,ωk,νk,0,0,
where X^k∈R(3+2m)×(3+2m) is the estimated augmented mobile robot and landmarks state with its associated covariance Pk+1-. The associated state error covariance of the estimation error is given by
(12)Pk+1-=∇frPkIn-γ-2Pk+∇HiTRk-1∇HiPk-1∇frT+∇gωνΣk∇gωνT,
where Pk is the state error covariance of previous estimation error and Σk is the covariance of the control noise δω and δν. The covariance matrix of the state Pk is defined in the following subsection. ∇fr and ∇gων are the Jacobian transformations with respect to the robot position and the process noise, respectively. Those Jacobian transformations are evaluated from the mobile robot motion in (4)–(7) at the current state estimate X^k. For T=1 in a case of stationary landmarks, the following are obtained:
(13)∇fr=1000-νsinθ100νcosθ010000In,∇gων=fωνθ0000fωνx0000fωνy00000,
where In is an identity matrix with an appropriate dimension. fωνθ, fωνx, and fωνy define the process noise of the robot motion about its angle and x, y positions, respectively.
The mobile robot continues the process with the observation and measurement using its sensors and this step consequently results in a measurement model as shown in (8). Hence, the mobile robot updates its current location through
(14)X^k+1+=∇frX^k+Kkzi,k-∇HiX^k,
where the updated state covariance Pk+1+ is given by
(15)Pk+1+=∇frPkψk-1∇frT+∇gωνΣk∇gωνT,
where ψ=In+∇HiTRk-1∇Hi-γ-2InPk and Kk=Pk∇HiT(∇HiPk∇HiT+Rk)-1. Since SLAM is a nonlinear system, the Jacobian transformation of measurement between mobile robot position and any ith landmark results in the following function ∇Hi:
(16)∇Hi=0-dxr-dyrdxrdyr-1dyr2-dxr2-dyr2dxr2,
where
(17)dx=xi-xk,dy=yi-yk,r=dx2+dy2.
2.3. State Error Covariance Matrix
The covariance matrix of a state estimation in SLAM is a combination matrix of mobile robot and landmark position covariance matrices and correlation between mobile robot and landmarks. Correlations between mobile robot position and landmarks estimation arise when the measurements are incorporated and thus the state error covariance becomes dense. The state error covariance Pk is defined generally as
(18)Pk=PvvPvmPvmTPmm,
where Pvv is the covariance matrix of the robot position, Pmm is the covariance matrix of the landmark position, and Pvm is cross-covariance matrix of the robot and landmark position or cross-correlation between them.
The dimension of state error covariance in SLAM is (3+2m)×(3+2m). The size of covariance matrix will grow as the robot continuously observed new landmarks in the environment. State error covariance for SLAM is fully represented in (19). The state error covariance indicates the error associated with the robot and landmark state estimations. From the state error covariance, the increment or decrement of uncertainties and errors of the estimation could be observed, which represent the precision and consistency of the estimation. Therefore, the study on the behaviour of state error covariance is one of the important issues in designing the technique of mobile robot SLAM:(19)PθθPθxPθyPθm1,xPθm1,y⋯⋯Pθmn,xPθmn,yPxθPxxPxyPxm1,xPxm1,y⋯⋯Pxmn,xPxmn,yPyθPyxPyyPym1,xPym1,y⋯⋯Pymn,xPymn,yPm1,xθPm1,xxPm1,xyPm1,xm1,xPm1,xm1,y⋯⋯Pm1,xmn,xPm1,xmn,yPm1,yθPm1,yxPm1,yyPm1,ym1,xPm1,ym1,y⋯⋯Pm1,ymn,xPm1,ymn,y⋮⋮⋮⋮⋮⋯⋯⋮⋮⋮⋮⋮⋮⋮⋯⋯⋮⋮Pmn,xθPmn,xxPmn,xyPmn,xm1,xPmn,xm1,y⋯⋯Pmn,xmn,xPmn,xmn,yPmn,yθPmn,yxPmn,yyPmn,ym1,xPmn,ym1,y⋯⋯Pmn,ymn,xPmn,ymn,y.
3. Conditions for Convergence in <inline-formula>
<mml:math xmlns:mml="http://www.w3.org/1998/Math/MathML" id="M120">
<mml:mi>H</mml:mi>
<mml:mi>∞</mml:mi></mml:math>
</inline-formula> Filter-Based SLAM
H∞ filter algorithm has a significant effect on its performance. Unlike the EKF, γ appears in the prediction and update process; therefore, it requires additional attention from the designer to design their system appropriately. The presence of γ acts as an attenuator to reduce the system uncertainties during mobile robot observation and can be adjustable by applying γ-switching technique [30]. However, in this study, γ is fixed as the purpose of the study is to find the sufficient conditions for a constant γ. If γ continuously changes, the result would approximate Kalman filter behaviour. Therefore, the results obtained may exhibit almost the same performance as the Kalman filter which is deemed unsuitable for this study.
Based on H∞ filter algorithm, the estimated state error covariance equation (15) can be simplified as
(20)Pk+1=Pk+Qk.
Equation (20) shows that, in every update, the existence of process noise Qk has a significant effect on the state error covariance. The equation is also one of the factors that provide sufficient information in H∞ filter. Thus, H∞ filter is sensitive to the initial state covariance, process, and measurement noises distributions as reported in the literature [25]. Taking into account these variables, the analysis contributes adequately to explaining the filter performance and consistency. As the H∞ filter-based SLAM is still considered as a new technique discovered for SLAM, no analytical results of convergence are available. Therefore, a theoretical study about H∞ filter convergence is essential.
Motivated by the aforementioned research gap, this study attempts to clarify the effect of initial condition of state covariance with the influence of process and measurement noise distributions on the H∞ filter behaviour in SLAM. Two case studies have been defined for the study.
Robot initial state error covariance is smaller than the landmarks initial state error covariance such that P0vv≪P0mm.
Robot initial state error covariance is big and the same as the landmarks initial state error covariance such that P0vv=P0mm.
The first case defines that the robot has more confidence about its location than the landmark. This case relies on the assumption that robot has an efficient proprioceptive sensors for sensory purposes. Next, the second case defines that both robot and landmarks initial state error covariances are unknown. Generally, this case is more alike to the problem in real SLAM application as usually no prior information is available for reference. Provided by these two conditions, a theoretical study and analysis are suggested to comprehend their influences on SLAM problem. Parallel to the proposed cases, this study also investigates the consequences of process and measurement noises to the estimation.
As remarked in the literature, the performance of H∞ filter is sensitive and depends on the design parameters such as the process and measurement noises and also the initial state error covariance. The study continues to describe explicitly that the selection of design parameters should satisfy some conditions to guarantee H∞ filter surpassing EKF performance (see [27]). Furthermore, there are certain trade-offs which are necessary between the design parameters to achieve the best solution in H∞ filter-based SLAM.
Definition 1.
The Jacobian matrix when a mobile robot observes only one new landmark in its surrounding at point A and makes n observations is given by [19]
(21)∇HA=0-dxArA-dyArAdxArAdyArA-1dyArA2-dxArA2-dyArA2dxArA2=-e-AA,
where
(22)e=01,A=dxArAdyArA-dyArA2dxArA2
evaluated at the true landmark (xm,ym) and the true robot position (xA,yA) and its elements are defined by
(23)dxA=xm-xA,dyA=ym-yA,rA=dxA2+dyA2.
The aforementioned definition will be excessively used in this study.
Definition 2.
The state vector of mobile robot and landmarks location (1)Xk follows a Gaussian distribution in which it is represented through a mean and covariance of its elements:
(24)Xk~Nx,P.
The covariance indicates the level of certainty of the mean estimation; the larger the covariance, the larger the uncertainty of the state estimation. Fisher information matrix (FIM) Ω specifies the weight of information contained in a Gaussian distribution. In the case of mobile robot SLAM, FIM indicates the information obtained by the mobile robot from each observation. FIM is the inverse of the state error covariance. The information obtained is inversely proportional to the uncertainty:
(25)Ωk=Pk-1.
Fisher information matrix at time k+1 is the summation of information matrix at time k and the new information obtained from the observation, described as follows [19]:
(26)Ωk+1=Ωk+∇HiTRk∇Hi.
The Fisher information matrix is used to determine the updated state error covariance for each update in this study. If the mobile robot starts moving from its initial position to point A and makes an observation at that point, then with respect to Definitions 1 and 2, the FIM yields the following equation:
(27)Ωk+1=P0vv00P0mm-1+-HATATRA-1-HAA-γ-2In=P0vv-1+HATRA-1HA-γ-2In-HATRA-1A-ATRA-1HAP0mm-1+ATRA-1A-γ-2In,
with HA=eA and thus from (21)∇HA=-HAA. P0vv and P0mm are the initial state error covariance for robot and landmarks, respectively. The landmarks are assumed to be stationary, and hence there are no noises affecting the prediction process for landmarks state. Equation (27) is claimed as the feasible condition [31]. This condition is very important as it defines the amount of information available at that specific time.
Before presenting the main results, the feasibility conditions for H∞ filter-based SLAM are proposed.
Theorem 3.
With the consideration of (4)–(7) and (12), the solution of the filter exists if it satisfies the feasibility conditions of γ for each case as stated below:
if P0≫R, then γ2>R;
if P0<R, then γ2>P0.
Proof.
The feasibility conditions are analysed separately for each case. To reveal these criteria clearly, consider a one-dimensional SLAM (1D SLAM) problem, that is, a robot with a single coordinate system observing landmarks. Given that for all conditions P0>0 and γ>0,
if P0≫RA, then γ2>RA.
To ensure convergency, the covariance matrix should possess positive semidefinite (PSD) matrix properties of its elements [28]. Since Ω=P-1 and P0≫R, the first element of (27) has the following criteria:
(28)HATRA-1HA-γ-2>P0-1.
Note that the left and right hand sides of (28) still exhibit a positive semidefinite matrix. Examining the case for 1D SLAM, the following equation is achieved:
(29)RA-1-γ-2>P0-1.
Using this new proposed condition and considering that the updated information must be at least a PSD matrix,
(30)RA-1-γ-2>0,RA-1>γ-2,γ2>RA.
If P0<RA, then γ2>P0.
Similar to the analysis of case (1), the first element of FIM is examined to find the consequences of P0<RA. Under the same assumption of PSD characteristics as in (1), the following results are obtained:
(31)P0-1+HATRA-1HA-γ-2>P0-1-γ-2>0.
Hence, for 1D SLAM,
(32)γ-2<P0-1,γ2>P0.
Based on the proposed Theorem 3, additional conditions are required prior to implementation stage to ensure that H∞ filter estimation is converging. Besides, the results have aided the previous findings significantly which are identified by [31]. Remark that the initial state covariances for mobile robot and landmarks are considered similar in Theorem 3. Now we have the appropriate conditions and therefore we are ready to examine the sufficient conditions for H∞ filter-based SLAM convergence of each case as defined previously.
The preceding section explains that FIM is used to interpret H∞ filter behaviour in each update. Since the main difference between H∞ filter and Kalman filter is due to the existence of γ for which the characteristics can be explicitly recognized by (27), the equation may aid in analysing its significance and influence of design parameters on H∞ filter performance.
If P0vv≪P0mm, then the landmarks have high uncertainties. Thus, it is appropriate to assume that P0mm-1→0. By this assumption, FIM can be expressed as follows:
(33)Ωk+1=P0vv-1+HATRA-1HA-γ-2In-HATRA-1A-ATRA-1HAATRA-1A-γ-2In.
Notice that the diagonal elements are the essential elements for the designer to obtain some sufficient conditions in H∞ filter. This is because each variable occupies mobile robot and landmarks uncertainties. Hence, smaller values of these variables are being demanded to achieve better state estimation. Moreover, Ω must always preserve at least a positive semidefinite matrix in every observation. These two facts are necessary conditions to secure a reliable estimation in H∞ filter-based SLAM. To illustrate this matter clearly, the following proposition is presented. Let P0vv(x), P0vv(y), and P0vv(θ) define x, y, θ robot initial state covariances.
Proposition 4.
For γ>0, in a case of a robot that has more confidence about its initial state than the landmarks state, γ selection is affected by the initial state covariance, process, and measurement noises and its selection must satisfy the following properties:
(34)γ>1P0vvθ-1+RA-1,γ>RAdx2+dy22P0vvx+Σx-1RAdx2+dy22+dx4+dx2dy2+dy2,γ>RAdx2+dy22P0vvy+Σy-1RAdx2+dy22+dy4+dy2dx2+dx2,
where Σx and Σy are the associated process noise covariance for robot x, y coordinate.
Proof.
First the diagonal elements are analysed. This consequently led us to determine both robot and landmarks information during robot observations which are represented by P0vv-1+HATRA-1HA-γ-2In and ATRA-1A-γ-2In. The former equation can substantially explain the latter equation. This is shown by the following calculations:
(35)P0vv-1+HATRA-1HA-γ-2In=P0vvθ-100P0vvxy-1+eTATRA-1eA-γ-2In=P0vvθ-1+eTRA-1e-γ-2IneTRA-1AATRA-1eP0vvxy-1+ATRA-1A-γ-2In,
where P0vv(θ) and P0vv(xy) are the initial robot state error covariance about its angle and x,y position, respectively. Note that both diagonal elements must preserve PSD in each observation. In SLAM, mobile robot heading angle acts as the primary factors that determine consistency in SLAM [19]. Hence, it should be analysed differently with other elements of the state error covariance. As each diagonal matrix element must at least possess a PSD, then, for the robot heading angle covariance P0vv(θ), the following characteristics are compulsory:
(36)P0vv(θ)-1+RA-1-γ-2>0,γ2>1P0vv(θ)-1+RA-1.
Now, the analysis proceeds for the robot and landmarks states about its x, y estimations, that is, the second diagonal element. By utilizing Definition 1 and (27) being referred,
(37)ATRA-1A=dxArA-dyArA2dyArAdxArA2RA-1dxArAdyArA-dyArA2dxArA2=dxA2rA2+dyA2rA4RA-1dxAdyArA2-dyAdxArA4RA-1dyAdxArA2-dxAdyArA4RA-1dyA2rA2+dxA2rA4RA-1.
Based on this case, it is configurable that the robot has some degree of confidence about its initial location compared to landmarks condition which consists of very large initial state covariance. Hence, the inverse of the landmarks initial state covariance is approximating zero. Substituting (37) into the second diagonal term of (35) appropriately leads to the following expression:
(38)P0vv(x)-1+dxA2rA2+dyA2rA4RA-1-γ-2>0,P0vv(y)-1+dyA2rA2+dxA2rA4RA-1-γ-2>0.
By simple algebraic rearrangement to determine γ sufficient conditions, the following is proposed to obtain better estimation result:
(39)γ2>RAdxA2+dyA22P0vv(x)-1RAdxA2+dyA22+dxA4+dxA2dyA2+dyA2,γ2>RAdxA2+dyA22P0vv(y)-1RAdxA2+dyA22+dyA4+dyA2dxA2+dxA2.
This result significantly describes that it is difficult to obtain an appropriate γ due to the nonlinear characteristics of robot movement and noises. Nevertheless, the prediction step also embraces the process noise (refer to (20)); it is found that bigger process noise required bigger γ. Next, the PSD characteristic is examined in each FIM update. It was found that FIM, which behaves as the information during observations has provided a proper selection of γ to avoid finite escape time phenomena.
Theorem 5.
Note that γ>0 and Theorem 3 is satisfied. If the mobile robot initial state covariance is very small compared to the initial state covariance of landmarks, then γ is chosen to satisfy the following equations:
γ>R,
γ>P0vv.
If else, the updated state error covariance exhibits finite escape time.
Proof.
From the properties of PSD, the determinant of the matrix must be nonnegative. This behaviour is necessary in probabilistic SLAM. As a matter of fact, this criterion is used to obtain some typical features for γ selection. The determinant of (33) gives
(40)P0vv-1+HATRA-1HA-γ-2InATRA-1A-γ-2In-HATRA-1AATRA-1HA>0.
However, the above nonlinear equation is difficult to explain the effect of γ for every respective update. We propose the analysis in linear 1D SLAM to visualize how γ influences the estimation. In 1D SLAM, the determinant eventually becomes as follows:
(41)P0vv-1+RA-1-γ-2RA-1-γ-2-RA-2>0,P0vv-1RA-1-γ-2P0vv-1-2γ-2RA-1+γ-4>0,γ-4-P0vv-1+2RA-1γ-2+P0vv-1RA-1>0,
where HA=-11 and A=1. Furthermore, it is easily recognized that as P0vv, RA>0, then the following are achieved:
(42)γ-4-P0vv-1+2RA-1γ-2+P0vv-1RA-1<γ-4-P0vv-1+RA-1γ-2+P0vv-1RA-1,(43)γ-4-P0vv-1+RA-1γ-2+P0vv-1RA-1=γ-2-P0vv-1γ-2-RA-1,γ-2-P0vv-1γ-2-RA-1>0.
Hence, there exist two different cases with two respective conditions to fulfil (43).
γ>RA and γ>P0vv.
γ<RA and γ<P0vv.
However, the above condition (2) is unlikely to happen. This is related to (27) where this condition can yield a negative definite matrix. Therefore, condition (1) is apparently the only solution for this case. Moreover, from the above analysis it explicitly identifies the relationship between γ, initial state covariance, and measurement noise. In addition, the process noise covariance is also influencing γ selection as it is included in the state error covariance prediction step.
As defined above, the results consistently show the same behaviour as Theorem 3 in which γ must be selected properly considering the environment and system situations; whenever the initial state error covariance for mobile robot P0vv is much smaller than landmarks covariance P0mm, then γ must be designed according to these two conditions. Note that Theorem 5 describes a case where the initial state covariance between mobile robot and landmarks is different, whereas Theorem 3 only considers that both initial state covariances have the same values. Furthermore, both feasible conditions proposed by Theorem 3 must be satisfied for this case.
Moreover, in navigation of mobile robot, the heading angle of mobile robot acts as an important factor to be considered in SLAM [19]. As been proposed by Theorem 3 and Proposition 4, the designer must ensure that those feasibility conditions and (36) are fulfilled to successfully implement the filter approach in their application. γ is selected by incrementally increasing its value with regards to the value defined in Theorem 3 and Proposition 4 to obtain the best solution.
This condition is the appropriate situation of an actual SLAM problem. It is obvious that if a robot is arbitrarily put in an unknown environment, then it should not have information about its initial location even though it is being equipped with high accuracy sensors. Such a situation presumes a uniform distribution for both robot and landmarks belief where both initial state covariances yield very big uncertainties. The following theorem is proposed to indicate H∞ filter-based SLAM behaviour for this respective case.
Theorem 6.
Consider that γ>0 and Theorem 3 is satisfied. There is a γ that gives the best solution to SLAM which satisfies the following, if and only if both robot and landmarks initial state covariances are very big such that robot does not have any prior information about its initial position:
(44)γ>R2.
Proof.
Assume that both robot and landmarks initial state covariances are too big. By referring to the previous case, the determinant of the updated Fisher information matrix for a robot observing landmarks at point A yields the following:
(45)P0vv-1+HATRA-1HA-γ-2InP0mm-1+ATRA-1A-γ-2In-HATRA-1AATRA-1HA>0.
To simplify this analysis, we consider again 1D SLAM problem. We know initially that in this case P0vv=P0mm=P0 and both initial state covariances are very big. By this assumption and similar steps as (41)–(43), (45) leads us to the following equation:
(46)P0-1+RA-1-γ-22-RA-2>0,γ-4-2γ-2P0-1+RA-1+P0-2+2P0-1RA-1>0.
Consider the above equation and the fact that P0≫0 and thus (P0-1→0). Hence, through factorization, the following equation is obtained:
(47)γ-4-2RA-1γ-2>0,γ-2(γ-2-2RA-1)>0.
Since in H∞ filter algorithm γ>0, thus from (47) it appears that the left hand side variables should yield positive values to ensure that the solution of the estimation is available. Therefore, the following relation of γ and measurement noise is obtained:
(48)γ-2-2RA-1>0,γ>RA2.
It is worth mentioning that the process noise still slightly affects the estimation if it is too big. If such conditions occurred, then γ must be tuned carefully to achieve a desired estimation result. Referring back to the filter algorithm, H∞ filter estimation should be the same as EKF if γ is set to be very big. Related to this fact, we propose a condition of γ where H∞ filter has better performance than EKF. If finite escape time is observable, then γ must be tuned by incrementally increasing its value to obtain better result. This is the common step in H∞ filtering thus finally approximating the same estimation behaviour to EKF. The next section identifies and evaluates our theoretical results.
4. Simulation Results
The proposed theoretical results obtained above are examined through a series of simulations. A small environment which has the parameters as described in Table 1 is considered. It is assumed that the robot exteroceptive sensors can observe their surrounding in a specific range and the process noises are small such that they can be neglected. The robot is assigned to move in some directions while doing its observations. Landmarks are also assumed to be point landmarks, stationary, and situated randomly. Comparison between H∞ filter- and EKF-based SLAM regarding map construction analysis, state error covariance update, and root-mean-square error (RMSE) evaluation is given for each case that has been analysed in the preceding section. Note that the process noises are kept consistently very small for both cases.
Simulation parameters.
Sampling time [s]
T
0.1
Process noise
Qk
1 × 10^{−7}
Feasibility condition
Feasibility condition 1
Robot and landmark initial state covariance
P0vv
10
P0mm
10
Measurement noises
Rkθ
0.002
Rk(distance)
0.002
Selection of Gamma
γ
0.85
Feasibility condition 2
Robot and landmark initial state covariance
P0vv
2
P0mm
2
Measurement noises
Rkθ
5
Rk(distance)
5
Selection of Gamma
γ
2.35
Case analysis
Case 1
Robot and landmark initial state covariance
P0vv
2
P0mm
20
Measurement noises
Rkθ
2
Rk(distance)
2
Selection of Gamma
γ
2
Case 2
Robot and landmark initial state covariance
P0vv
5
P0mm
5
Measurement noises
Rkθ
2
Rk(distance)
2
Selection of Gamma
γ
2
Figures 3 and 4 illustrate the simulation results for the proposed feasibility condition stated by Theorem 3. To evaluate the reliability of the proposed feasibility conditions, the parameters in Table 1 are selected such that they satisfy the defined conditions. Figure 3 demonstrates the results of estimation between H∞ filter (HF) and extended Kalman filter (EKF) for feasibility condition 1 where condition of P0≫R is being considered. As illustrated in the figure, HF shows better performance than EKF about the mobile robot and landmarks positions.
Feasibility condition 1: robot localization and map building performance between H∞ filter and EKF.
Feasibility condition 2: robot localization and map building performance between H∞ filter and EKF.
The analysis for feasible condition 2 is depicted in Figure 4. Based on the given figure, if the parameters selected correspond to the described condition, that is, P0<R, then the H∞ estimation exhibits better performance than EKF. According to these results, if both of the feasible conditions are not satisfied, then the estimation diverges and exhibits erroneous results. Besides, the estimation emphasised that finite escape time easily occurred during mobile robot observation. These results can be found in [28].
Figures 5–7 illustrate the simulation results for case 1 whenever the mobile robot has more confidence about its initial position in comparison to the landmarks state covariance; that is, P0vv≪P0mm. Based on these figures, it is observed that the estimation of H∞ filter outperforms EKF. Figure 5 depicts the result of mapping for both filters, while the state error covariances for the estimations are presented in Figure 6. RMSE evaluations of the landmarks estimations are presented in Figure 7. The mobile robot path estimation as well as the estimation of landmarks position in Figure 5 consistently shows that H∞ filter provides better estimation than EKF. Figure 5 depicts clearly the erroneous estimation of EKF through the path of the mobile robot. The findings can be demonstrated clearly by Figure 6 in which the state error covariance of EKF has higher value than H∞ filter for both mobile robot and landmarks position. This denotes that the estimation through EKF possesses higher uncertainties under the conditions described by case 1. Based on the RMSE evaluation for the landmarks position in Figure 7, H∞ filter also suggests that it has smaller error than EKF. However, these results can only be available if and only if the condition of γ>R has been satisfied.
Case 1: map construction performance between H∞ filter and EKF.
Case 1: state error covariance performance between H∞ filter and EKF.
Case 1: RMSE performance between H∞ filter and EKF.
Figures 8–10 illustrate the results of case 2 of initial covariances of P0vv=P0mm=5 for both robot and landmarks states. A similar performance is observed as described in Figures 5–7. The robot could estimate its current path and location with some level of certainty. The uncertainties of estimation proved that H∞ filter still surpasses EKF performance as shown in Figure 9. Moreover, it can be observed from Figure 10, that the RMSE evaluation of the landmarks position has similar characteristics to that of the case 1 (Figure 7). This shows that H∞ filter can provide a better solution in SLAM problem, if and only if Theorem 6 is satisfied in each observation. However, if the proposed conditions are not fulfilled, then the estimation becomes erroneous as explained in the literature [28], where EKF performs better.
Case 2: Map construction performance between H∞ filter and EKF.
Case 2: state error covariance performance between H∞ filter and EKF.
Case 2: RMSE performance between H∞ filter and EKF.
Even though it is imperative to ensure that the conditions specified under Theorems 5 and 6 are satisfied, note that initial state covariance and process noise have the possibilities to influence the estimation. Bigger process noise contributes to a bigger selection of γ as it may be observed from (10). Similar pattern is also observed on the initial state covariance, in which greater value of initial state covariance of heading angle may lead to unexpected result [19]. Therefore, the designer should be aware in selecting those parameters in addition to fulfilling the conditions of Theorems 3, 5, and 6.
It could be observed that the H∞ filter estimations are much superior to the EKF even when it comes to the Gaussian noise environment with an appropriate selection of γ and other parameters design. Moreover, our results support the analysis discovered in [26] as the state error covariance update converges almost to zero in the estimation. In addition, it is also found that the EKF estimation becomes more inconsistent as the initial state covariance becomes bigger [19]. This is in contrast to the EKF, where even if the initial state covariance has a much bigger value, the H∞ filter still preserves better estimation. To conclude, H∞ filter-based SLAM is one of the competitive solutions for SLAM especially for bigger initial state covariance and non-Gaussian noise environment.
5. Conclusion
This study has demonstrated that the H∞ filter may be considered as one of the best candidates to mitigate navigation issues for SLAM especially for an environment with unknown noise characteristics. It could be concluded from the two case studies that the measurement noise must be less than γ2 for a system with small process noise. Extra attention is required by the designers if both initial state covariance and process noise are larger, which consequently demands a bigger γ selection for the whole system to operate efficiently. However, to sufficiently achieve an expected performance in H∞ filter, the designer must ensure that the above given conditions are satisfied in their system design, taking into consideration the conditions of initial state covariance, process, and measurement noises distributions.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
The authors would like to thank the Ministry of Education (MoE), Malaysia, Research and Innovation Department of Universiti Malaysia Pahang (UMP), and Namerikawa Laboratory members for their continuous support and comments. This research is partly funded by the MoE Grant KPM-RDU130106 and internal Grant UMP-GRS130360.
WangH.FuG.LiJ.YanZ.BianX.An adaptive UKF based SLAM method for unmanned underwater vehicleWestM. E.SyrmosV. L.Navigation of an autonomous underwater vehicle (AUV) using robust SLAMProceedings of the IEEE International Conference on Control Applications (CCA '06)October 2006Munich, Germany1801180610.1109/CACSD-CCA-ISIC.2006.47769142-s2.0-71249128578NüchterA.SurmannH.LingemannK.HertzbergJ.ThrunS.6D SLAM with an application in autonomous mine mappingProceedings of the IEEE International Conference on Robotics and AutomationApril 2004New Orleans, La, USA199820032-s2.0-3042629062ThrunS.BurgardW.FoxD.ZlotR.BosseM.Efficient large-scale 3D mobile mapping and surface reconstruction of an underground mineProceedings of the Field and Service RoboticsJanuary 2014479493HenryP.KraininM.HerbstE.RenX.FoxD.RGB-D mapping: using Kinect-style depth cameras for dense 3D modeling of indoor environmentsForsterC.LynenS.KneipL.ScaramuzzaD.Collaborative monocular SLAM with multiple micro aerial vehiclesProceedings of the IEEE/RSJ International Conference on Intelligent Robots and SystemsNovember 2013Tokyo, Japan39623970LazaroM. T.PazL. M.PiniesP.CastellanosJ. A.GrisettiG.Multi-robot SLAM using condensed measurementsProceedings of the IEEE/RSJ International Conference on Intelligent Robots and SystemsNovember 2013Tokyo, Japan10691076SaeediS.PaullL.TrentiniM.LiH.Neural network-based multiple robot simultaneous localization and mappingÇelikK.SomaniA. K.Monocular vision SLAM for indoor aerial vehiclesDavisonA. J.ReidI. D.MoltonN. D.StasseO.MonoSLAM: real-time single camera SLAMChatterjeeA.MatsunoF.A neuro-fuzzy assisted extended kalman filter-based approach for simultaneous localization and mapping (SLAM) problemsFuC.Olivares-MendezM. A.Suarez-FernandezR.CampoyP.Monocular visual-inertial SLAM-based collision avoidance strategy for fail-safe UAV using fuzzy logic controllersAhmadH.NamerikawaT.Intermittent measurement in robotic localization and mapping with FIM statistical boundsMuracaP.PuglieseP.RoccaG.An extended Kalman Filter for the state estimation of a mobile robot from intermittent measurementsProceedings of the 16th Mediterranean Conference on Control and Automation (MED '08)June 2008Ajaccio Corsica, France1850185510.1109/MED.2008.46022512-s2.0-52949104050LynchP. M.FigueroaJ. F.Position estimation with intermittent measurementsProceedings of the American Control ConferenceJune 1991Boston, Mass, USA228022852-s2.0-0026392481Gamini DissanayakeM. W. M.NewmanP.ClarkS.Durrant-WhyteH. F.CsorbaM.A solution to the simultaneous localization and map building (SLAM) problemNietoJ.BaileyT.NebotE.Can-SLAM: combining EKF-SLAM and scan correlationHuangS.DissanayakeG.Convergence and consistency analysis for extended Kalman filter based SLAMCastellanosJ. A.Martinez-CantinR.TardósJ. D.NeiraJ.Robocentric map joining: improving the consistency of EKF-SLAMPazL. M.TardósJ. D.NeiraJ.Divide and conquer: EKF SLAM in O(n)D'AlfonsoL.GranoA.MuracaP.PuglieseP.Sensor fusion and surrounding environment mapping for a mobile robot using a mixed extended Kalman filterProceedings of the 10th IEEE International Conference on Control and Automation (ICCA '13)June 2013Hangzhou, China1520152510.1109/ICCA.2013.65650042-s2.0-84882348813MontemerloM.ThrunS.Simultaneous localization and mapping with unknown data association using FastSLAMProceeding of the IEEE International Conference on Robotics and AutomationSeptember 2003Karlsruhe, Germany198519912-s2.0-0344445478PeiF.WuM.ZhangS.Distributed SLAM using improved particle filter for mobile robot localizationSimonD.From here to infinityAhmadH.NamerikawaT.H∞ filter convergence and its application to SLAMProceedings of the ICROS-SICE International Joint ConferenceAugust 2009Fukuoka, Japan28752880AhmadH.NamerikawaT.Feasibility study of partial observability in H∞ filtering for robot localization and mapping problemProceedings of the American Control Conference (ACC '10)July 2010Baltimore, Md, USA398039852-s2.0-77957814294AhmadH.NamerikawaT.Robot localization and mapping problem with unknown noise characteristicsProceedings of the IEEE International Conference on Control Applications (CCA '10)September 2010Yokohama, Japan1275128010.1109/CCA.2010.56112722-s2.0-78649405727AhmadH.NamerikawaT.Robotic mapping and localization considering unknown noise statisticsBolzernP.ColaneriP.De NicolaoG.H∞ differential Riccati equations: convergence properties and finite escape phenomenaBolzernP.MaroniM.New conditions for the convergence of H∞ filters and predictorsAndrade-CettoJ.SanfeliuA.Vidal-CallejaT.Andrade-CettoJ.SanfeliuA.Conditions for suboptimal filter stability in SLAMProceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '04)October 2004Sendai, Japan27322-s2.0-14044252931