# An ASVSF-SLAM Algorithm with Time-Varying Noise Statistics Based on MAP Creation and Weighted Exponent

Authors: Yingzhong Tian; Heru Suwoyo; Wenbin Wang; Long Li
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

The probability-based filtering method has been extensively used for solving the simultaneous localization and mapping (SLAM) problem. Generally, the standard filter utilizes the system model and prior stochastic information to approximate the posterior state. However, in the real-time situation, the noise statistics properties are relatively unknown, and the system is inaccurately modeled. Thus the filter divergence might occur in the integration system. Moreover, the expected accuracy might be challenging to be reached due to the absence of the responsive time-varying of both the process and measurement noise statistic which naturally can enlarge the uncertainty in the continuous system. Consequently, the traditional strategy needs to be improved aiming to provide an ability to estimate those properties. In order to accomplish this issue, the new adaptive filter is proposed in this paper, termed as an adaptive smooth variable structure filter (ASVSF). Sequentially, the improved SVSF is derived and implemented; the process and measurement noise statistics are estimated by utilizing the maximum a posteriori (MAP) creation and the weighted exponent concept, and the covariance correction step is added based on the divergence suppression concept. In this paper the ASVSF is applied to overcome the SLAM problem of an autonomous mobile robot; henceforth it is abbreviated as an ASVSF-SLAM algorithm. It is simulated and compared to the classical algorithm. The simulation results demonstrated that the proposed algorithm has better performance, stability, and effectiveness.


---

## Body

## 1. Introduction

The position tracking is an unavoidable part of localization that needs to be noticed. In most cases of mobile robot navigation, a robot should have the ability to locate its position and gather certain information related to the features of the environment automatically. This task is well-known as simultaneous localization and mapping (SLAM) [1–15] which was first proposed by Smith, Self, and Cheeseman in 1988. The SLAM-based mobile robot navigation has intensively received attention because of some challenging factors that need to be solved such as wide uncertainty, system complexity, inaccurate system model, limited prior information, noise statistics of the process and measurement, computational cost, and filter divergence. Essentially, those reasons lead to the probability-based estimation [5, 8–12, 16, 17] that has been proposed in many cases of the robot navigation. The most popular method is the Extended Kalman Filter (EKF) [3, 5–12, 16–23] which is a nonlinear version of Kalman Filter (KF) [3, 5, 6, 9, 12, 17, 20, 24, 25]. Nevertheless, it has many incompatibilities and difficulties such as the deviant solution from the state trajectory, less optimal state estimation, and large estimation error due to the linearization process and computational cost [12, 19]. This limitation makes its practical application becomes limited nowadays.In order to address these problems, many nonlinear estimation methods have been approached, such as Unscented Kalman Filter (UKF) [3, 4, 6, 26], Cubature Kalman Filter [3, 6, 17, 20, 27], and Smooth Variable Structure Filter (SVSF) [1, 2, 13, 18, 21–23, 25, 28–34]. Essentially, these methods have been reducing the EKF problem in various solutions. For this reason, many researchers have switched to use one of those methods rather than using EKF. The stable performance has also been demonstrated in some cases of solving SLAM-based navigation problem as presented in [1–13, 21, 22]. Most of the mentioned filters above require the accurate system model and known stochastic prior information. However, in the real-case application, the system is inaccurately modeled, and the prior knowledge of the noise statistics is unknown or partially known. An inaccuracy of modeling the system might enlarge the estimation error [4, 26, 35]. The internal and external uncertainties also might affect the change of the statistical characteristic which undoubtedly leads to the divergences of the filter performance [4, 24, 26, 35].Regarding these possibilities, the error estimation has been approached for providing the responsive online approximation to the change of noise statistics. This strategy is well-known as the adaptive filter algorithm which modifies the conventional method with the specific attempt and combination. Mehra et al. classified the adaptive approaches into four categories: Bayesian, Maximum-Likelihood, Correlation, and Covariance Matching. And the similar types are utilized as well in this paper called MAP estimation and Covariance Correction. It is approached to enhance SVSF capability. Relatively, the SVSF is a new predictor-corrector estimator based on the sliding mode concept [2, 18, 21, 28]. Referring to some pieces of literature, it has been experiencing fast and significant development. The SVSF was first initiated in 2007 [1, 21, 28] which was a derived form referring to its successor termed a Variable Structure Filter (VSF) [34, 36] and Extended Variable Structure Filter (EVSF) [1, 2, 34]. Then we proceed with a presence of new form that revises the original SVSF by adding the error covariance matrix without affecting its accuracy and stability [1, 25, 28, 32]. As a common form of the filtering technique, it was then enhanced by involving the time-varying boundary layer width to replace its previous characteristic [23, 28, 32, 37], and even now there has been existing new second-order SVSF which satisfies both first and second sliding condition [28, 38]. Thus, it is not surprising that the SVSF has been regarded for having a better and robust performance to model uncertainty compared to other existing filters nowadays. Furthermore, the effectiveness of SVSF has also been variously shown in different applications on either the linear [25, 28] or nonlinear system [2, 18, 22, 28–32, 38] such as for the state and parameter estimation [18, 28], signal processing [28], fault detection and diagnosis [28], and target tracking [25, 28, 30]. Additionally, the characteristic of SVSF also allows it to be combined with a certain filtering method as the effort to obtain the optimal solution [34, 37, 39–41].For these reasons, the SVSF is used in this experiment. However, as a standard filter, the SVSF is originally not designed with the ability to estimate the noise statistics properties which easily changes due to the uncertainties in the integration system. Therefore, the SVSF estimator was modified and improved in this paper. First, the improved SVSF was derived based on the one-step smoothing technique [24, 42, 43]. Then by referring to the prior knowledge, the noise statistics parameter of SVSF was estimated based on the maximum a posteriori (MAP) creation [4, 14, 15, 24, 26, 27, 35] and weighted exponent concept [4, 24, 26, 27]. It aims to produce the time-varying of those parameters. Second, to provide the ability against the filter divergence, the ASVSF was completed by the ability to correct the error covariance referring to the concept of divergence suppression [4, 44, 45]. In this paper the proposed method is applied for solving the SLAM problem of a wheeled mobile robot; henceforth it is termed as the ASVSF-SLAM algorithm. Then, by simulating and comparing to the conventional of SVSF-SLAM algorithm, it can be noted that the proposed method has better stability and accuracy refers to the benchmark in terms of Root Mean Square Error (RMSE) of estimated path and map. In this case, RMSE is used as the best measure to know the residual or deviation value which represents the difference between the actual and estimated values [28]. Besides that, this validation can also be readily observed from the small gap between a reference trajectory and the predicted result represented graphically.The remaining parts of this paper are organized as follows. Section2 contains SVSF. Section 3 presents the adaptive SVSF with the derivation of the improved SVSF, suboptimal MAP estimator, modified SVSF, unbiased suboptimal MAP estimator and its time-varying, and the divergence suppression method for covariance correction step. Section 4 presents an ASVSF-SLAM algorithm which is expanded with the discussion of the motion model, direct point-based observation, and inverse point-based observation. Section 5 presents some number of comparative result and discussion. Section 6 presents the conclusion.


## 2. Smooth Variable Structure Filter

The SVSF is new estimator based on sliding mode concept which has been becoming increasingly popular due to its robustness and stability to disturbance and uncertainty. It utilizes a switching gain to converge the estimate within a boundary of the true state value [28, 34, 41]. The analogy of SVSF process can be seen in Figure 1.Figure 1
Smooth variable structure filter concept.Considering that it is applied to nonlinear system, the dynamic model is described as below:(1)xk=fxk-1,uk+wk-1(2)zk=hxk+vkwhere k is discrete time index, x∈Rn is the state vector, u is the control vector, z∈Rm is the measurement vector, w∈Rn and v∈Rm are small adaptive process and measurement noise, respectively, and f. and h. are the nonlinear function and measurement model, respectively. The SVSF predicts the state estimate x^k∣k-1 obtained using the previous estimate state x^k-1∣k-1 and control vector uk(3)x^k∣k-1=f^x^k-1∣k-1,ukusing the partial derivative of f^(.) with respect to x^k-1∣k which is denoted by Fx; the a priori state error covariance matrix expressed by Pk∣k-1∈Rnxn can be calculated as follows: (4)Pk∣k-1=FxPk-1∣k-1FxT+Qk-1Next, by utilizing the predicted state estimate x^k∣k-1, the corresponding predicted measurement z^k∣k-1 and the measurement error ez,k∣k-1 can be calculated as follows: (5)z^k∣k-1=h^x^k∣k-1(6)ez,k∣k-1=zk-z^k∣k-1Referring to [34, 41], it is considered that(7)A=ez,k∣k-1abs+γez,k-1∣k-1abs(8)ψk=A̿-1HPk∣k-1HTSk-1-1(9)Sk=HPk∣k-1HT+Rk(10)satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1and then the SVSF gain KkSVSF is calculated as below:(11)KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1where .̿ indicates the diagonal term, γ refers to the convergence rate with element 0<γii≤1, ψ refers to the boundary layer width, Rk is the measurement error covariance matrix, and H is linear measurement matrix, whereas .+ and ∘ refer to the pseudo-inverse and Schur matrix multiplication, respectively.Utilizing the equation above, the updated statex^k∣k, updated state error covariance matrix Pk∣k, and new corresponding measurement error ez,k+1∣k+1 are described as follows:(12)x^k∣k=x^k∣k-1+KkSVSFez,k∣k-1(13)Pk∣k=I-KkSVSFHPk∣k-1I-KkSVSFHT+KkSVSFRkKkSVSFT(14)ez,k∣k=zk-h^x^k∣kEquations (1)–(14) summarize all steps of SVSF. Note that the estimation process is stable and converges to the existence subspace if the following condition is satisfied [28]:(15)ez,k-1∣k-1abs>ez,k∣kabs


## 3. Adaptive SVSF

As can be analysed from the SVSF presented above, the noise statistic parameter is always constant for all processes and considered well-known initially. However, in the real application, it is impossible to define those parameters accurately by means they are partially known or even unknown. It might degrade the filtering performance. For these reasons, the adaptive SVSF algorithm is proposed in this paper. The process was initially started by reconsidering the prior information of the dynamic nonlinear system modelled as described in (1) and (2). Secondly, it is considered that the process noise wk, measurement noise vj, and initial state vector x0 are assumed to be mutually uncorrelated for any discrete time index j or k; then the mean E[.] and covariance cov[.] of the process and measurement noise can be defined as follows: (16)Ewk=qk,covwk,wj=QkδkjEvk=rk,covvk,vj=Rkδkjcovwk,vj=0where δ refers to Kronecker delta function.The prior information above is initially assumed to be not equal to zero. Additionally,Qk and Rk are positive definite symmetric matrix; then the MAP estimates of qk, Qk, rk, Rk, and Xk can be obtained by calculating the maximum value of the following conditional probability density function:(17)L=pXk,q,Q,r,R∣Zkwhere Xk=[x1,x2,…,xk] and Zk=z1,z2,…,zk. Next, applying the Bayes rule and referring to the property of the conditional probability, where L is proportional to pXk,q,Q,r,R,Zk/p[Zk] thus since its marginal likelihood p[Zk] plays no role in the optimization, then(18)L=pZk∣Xk,q,Q,r,RpXk∣q,Q,r,Rpq,Q,r,Rwhere pq,Q,r,R is regarded to be constants obtained from the prior information. Then a posteriori distribution p[Xk,q,Q,r,R∣Zk] can be calculated by multiplying pZk∣Xk,q,Q,r,R with pXk,q,Q,r,R∣Zk as derived below:(19)pXk∣q,Q,r,R=px0∏i=1kpxi∣xi-1,q,Q=12πn/2P01/2exp⁡-12x0-x^0P0-12∏i=1k12πn/2Q1/2exp⁡-12xi-fxi-1-qQ-12=12πn/2P01/2exp⁡-12x0-x^0P0-12×12πnk/2Qk/2exp⁡-12∑i=1kxi-fxi-1-qQ-12=12πn/22πnk/2P0-1/2Q-k/2exp⁡-12x0-x^0P0-12+∑i=1kxi-fxi-1-qQ-12=12πnk+n/2P0-1/2Q-k/2exp⁡-12x0-x^0P0-12+∑i=1kxi-fxi-1-qQ-12(20)pZk∣Xk,q,Q,r,R=∏i=1kpzi∣xi,r,R=∏i=1k12πm/2R1/2exp⁡-12zi-hxi-rR-12=12πmk/2R-k/2exp-12∑i=1kzi-hxi-rR-12(21)pZk∣Xk,q,Q,r,RpXk∣q,Q,r,Rpq,Q,r,R==12πmk/2R-k/2exp⁡-12∑i=1kzi-hxi-rR-12×12πnk+n/2P0-1/2Q-k/2exp⁡-12x0-x^0P0-12+∑i=1kxi-fxi-1-qQ-12×pq,Q,r,RNow supposing that(22)C=12πmk/212πnk+n/2P0-1/2pq,Q,r,Rexp⁡12x0-x^0P0-12then (21) can be simplified as follows:(23)L=CR-k/2Q-k/2exp⁡-12∑i=1kzi-hxi-rR-12+∑i=1kxi-fxi-1-qQ-12Furthermore, to find the maximized parameter of the posterior distribution, firstly, take a logarithm as the monotonic function to simplify the calculation; secondly, find the first derivative of L with respect to qk, Qk, rk, and Rk separately; then finally end it by equating it with zero. These steps are organized and derived as follows.Forln⁡L is equal to(24)ln⁡C-k2ln⁡R-k2ln⁡Q-12∑i=1kzi-hxi-riR-12+∑i=1kxi-fxi-1-qi-1Q-12Then q^k,r^k,Q^k,R^k are (25)q^k=∂ln⁡L∂q=1k∑i=1kx^i∣k-fx^i-1∣k(26)Q^k=∂ln⁡L∂Q=1k∑i=1kx^i∣k-fx^i-1∣k-qi-1x^i∣k-fx^i-1∣k-qi-1T(27)r^k=∂ln⁡L∂r=1k∑i=1kzi-hx^i∣k(28)R^k=∂ln⁡L∂R=1k∑i=1kzi-hx^i∣k-rizi-hx^i∣k-riT


### 3.1. Suboptimal MAP Noise Estimator

The complicated multistep smoothing term (x^i∣k and x^i-1∣k) in (25)-(28) might cause inefficiency of the MAP estimate; therefore to find the conventional and efficient recursive form the simplification is needed. Note that the recursive update process only utilizes the estimate value at time k-1 and k; thus the simplification can be conducted by replacing x^i-1∣k with x^i-1∣i in (25) and (26) and xi∣k with xi∣i in (27)-(28). Therefore, the suboptimal of MAP noise estimator can be expressed as follows:(29)q^k=1k∑i=1kx^i∣i-fx^i-1∣i(30)Q^k=1k∑i=1kx^i∣i-fx^i-1∣i-qi-1x^i∣,i-fx^i-1∣i-qi-1T(31)r^k=1k∑i=1kzi-hx^i∣i(32)R^k=1k∑i=1kzi-hx^i∣i-rizi-hx^i∣i-riTAs can be analysed from the sequence equations above the estimate value of xi-1∣i is not provided by the SVSF. Therefore, modifying the original form of SVSF is needed to compute the noise statistics estimator effectively


### 3.2. Modified SVSF

The process of modifying the SVSF can be done by calculating the one-step smoothing of the SVSF gain and its corresponding estimate value using the fixed point smoothing algorithm [24, 42, 43]. This process can be summarized as follows:(33)x^k∣k-1=f^x^k-1∣k-1+qk-1(34)Pk∣k-1=FxPk-1∣k-1FxT+Qk-1(35)z^k∣k-1=h^x^k∣k-1+rk(36)ez,k∣k-1=zk-z^k∣k-1(37)A=ez,k∣k-1abs+γez,k-1∣k-1abs(38)Sk=HPk∣k-1HT+Rk(39)ψk=A̿-1HPk∣k-1HTSk-1-1(40)satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1(41)KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1(42)x^k-1∣k=x^k-1∣k-1+KkSVSFez,k∣k-1Then considering that the prior state x^k-1∣k replaces the term of x^k-1∣k-1 in the normal SVSF, the remaining part of modified SVSF is chained as follows:(43)x^k∣k-1=f^x^k-1∣k+qk-1(44)z^k∣k-1=h^x^k∣k-1+rk(45)ez,k∣k-1=zk-z^k∣k-1(46)Sk=HPk∣k-1HT+Rk(47)A=ez,k∣k-1abs+γez,k-1∣k-1abs(48)ψk=A̿-1HPk∣k-1HTSk-1-1(49)satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1(50)KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1(51)x^k∣k=x^k∣k-1+KkSVSFez,k∣k-1(52)Pk∣k=I-KkSVSFHPk∣k-1I-KkSVSFHT+KkSVSFRkKkSVSFT(53)ez,k∣k=zk-h^x^k∣kNow, the estimate values x^i∣i and x^i-1∣i can be adopted from (51) and (42), respectively


### 3.3. Unbiased Suboptimal MAP Noise Estimator

Next, to guarantee that the recursive process and measurement noise statistics are unbiased, the modified suboptimal MAP noise estimators are then derived referring to unbiased estimation.First, by substituting (43) into (51), the general term x^i∣i-fx^i-1∣i in (29) and (30) can be rewritten as follows:(54)x^i∣i-fx^i-1∣i=KkSVSFez,i+qi-1Similarly, replacing x^i∣i with (51), the general term zi-hx^i∣i in (31) and (32) can be written as follows:(55)zi-hx^i∣i=zi-hx^i∣i-1+KiSVSFez,iNote that ez,i=ez,i∣i-1, and the suboptimal MAP estimation (28)-(31) can be rearranged as follows:(56)q^k=1k∑i=1kKkSVSFez,i+qi-1(57)Q^k=1k∑i=1kKkSVSFez,iez,iTKkSVSFT(58)r^k=1k∑i=1kI-HKiSVSFez,i+ri(59)R^k=1k∑i=1kI-HKiSVSFez,iez,iTI-HKiSVSFTSince the innovation ez and its covariance ezezT are contained in the process and measurement noise estimator, therefore, (60)ez,i=hx~i∣i-1+vi-riand referring to state error covariance in (52), we have (61)KiSVSFez,iez,iTKiSVSFT=Pi∣i-Pi∣i-1+Pi∣i-1HTKiSVSFT+HKiSVSFPi∣i-1Next, considering that the expectations are (62)Eez,i=0(63)Eez,iez,iT=HPi∣i-1HT+Rithen by substituting (60)-(63) into (56)-(59), we have(64)Eq^k=qk(65)Er^k=rk(66)EQ^k=-Qk+E1k∑i=1kPi∣i+Pi∣i-1HTKiSVSFT+HKiSVSFPi∣i-1-Fi-1Pi-1∣i-1Fi-1T(67)ER^k=Rk+E1k∑i=1kHKiSVSFez,iez,iTHTKiSVSFT-HKiSVSFez,iez,iT-ez,iez,iTHTKiSVSFTHPi∣i-1HTNote that qk,rk,Qk,Rk are the representation of the suboptimal MAP estimation in (56)-(59); thus the unbiased MAP estimation can be summarized as follows:(68)q^k=1k∑i=1kKiSVSFez,i+qi-1(69)r^k=1k∑i=1kI-HKiSVSFez,i+ri(70)Q^k=1k∑i=1kPi∣i-1-Fi-1Pi-1∣i-1Fi-1T(71)R^k=1k∑i=1k2I-HKiSVSFez,iez,iTI-HKiSVSFT-ez,iez,iT+HPi∣i-1HT


### 3.4. Time-Varying Unbiased Noise Estimator and Weighting Exponentially

The time-varying noise estimator is proposed in this paper. According to the unbiased suboptimal MAP estimator calculated in (68)-(71), then the time-varying unbiased noise estimators are derived as follows: (72)q^k=q^k-1+1kKkSVSFez,k(73)r^k=r^k-1+1kI-HKkSVSFez,k(74)Q^k=k-1kQ^k-1+1kPk∣k-1-Fk-1Pk-1∣k-1Fk-1T(75)R^k=k-1kR^k-1+1k2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTThe following alternative forms (76)-(79) are regarded as the modified form of the time-varying unbiased noise statistics estimator (72)-(75). It is calculated by referring to the fading memory weighted exponent method and by utilizing the weighting coefficient dk to replace the exponential 1/k. (76)q^k=q^k-1+dkKkSVSFez,k(77)r^k=r^k-1+dkI-HKkSVSFez,k(78)Q^k=1-dkQ^k-1+dkPk∣k-1-Fk-1Pk-1∣k-1Fk-1T(79)R^k=1-dkR^k-1+dk2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTwhere the residual measurement ez,k=ez,k∣k-1 and weighting coefficient can be written. (80)βi=dkbi-1,i=1,…,ndk=1-b1-bkb is fading factor satisfying 0<b<1 and βi is the i-th weighting factor, which is defined as βi=βi-1b and satisfied ∑i=1kβi.


### 3.5. Addition of Divergence Suppression Method

Next, to prevent the occurrence of filter divergence, the covariance correction based on the divergence suppression concept is involved [4]. First, the convergence condition is derived referring to the covariance matching creation as described below:(81)vkTvk≤S.trEvkTvkwhere S is an adjustable coefficient presetting which is satisfied (S≥1) and the residual sequence vk=zk-hx^k,k-1. Executing (81), if the convergence condition is satisfied, (76)-(79) are applied; otherwise, the correction method of the covariance Pk∣k-1 is suggested against the divergence occurrence.(82)Pk∣k-1=λk.Pk∣k-1where λk is known as the adaptive weighting coefficient which is calculated based on the fading factor formula [44, 45] as summarized as follows:(83)C0,k=vkTvkk=1ρC0,k+vkTvk1+ρk>1(84)Nk=trC0,k-RT(85)Mk=trPk,k-1(86)λ0=trNktrMk(87)λk=λ0λ0≥11λ0<1where tr(.) refers to the matrix trace and ρ is the forgetful factor satisfied 0<ρ≤1 (typically to be set 0.95). Note that increasing the factor will create a smaller proportion of the information before time k [4]. It causes the residual vector effect to become prominent so that the filter tracking ability will increase.Up to this point, the adaptive SVSF can be summarized as follows.ASVSF Algorithm(1) Modified SVSF (88) x ^ k ∣ k - 1 = f ^ x ^ k - 1 ∣ k - 1 + q k - 1 P k ∣ k - 1 = F x P k - 1 ∣ k - 1 F x T + Q k - 1(2) Covariance Correction (89) C 0 , k = v k T v k ρ C 0 , k + v k T v k 1 + ρ N k = t r C 0 , k - R T M k = t r P k , k - 1 λ 0 = t r N k t r M k λ k = λ 0 λ 0 ≥ 1 1 λ 0 < 1 P k ∣ k - 1 = λ k . P k ∣ k - 1(3) Modified SVSF (90) z ^ k ∣ k - 1 = h ^ x ^ k ∣ k - 1 + r k e ˘ z , k ∣ k - 1 = z k - z ^ k ∣ k - 1 A = e ˘ z , k ∣ k - 1 a b s + γ e z , k - 1 ∣ k - 1 a b s S k = H P k ∣ k - 1 H T + R k ψ k = A ̿ - 1 H P k ∣ k - 1 H T S k - 1 - 1 s a t ψ - 1 e ˘ z , k ∣ k - 1 ̿ = 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ ≥ 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ i f  - 1 < ψ - 1 e ˘ z , k ∣ k - 1 ̿ < 1 - 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ ≤ - 1 K k S V S F = H + A ̿ ∘ s a t ψ - 1 e ˘ z , k ∣ k - 1 ̿ e ˘ z , k ∣ k - 1 ̿ - 1 x ^ k - 1 ∣ k = x ^ k - 1 ∣ k - 1 + K k S V S F e ˘ z , k ∣ k - 1(4) Improved SVSF (91) x ^ k ∣ k - 1 = f ^ x ^ k - 1 ∣ k + q k - 1 z ^ k ∣ k - 1 = h ^ x ^ k ∣ k - 1 + r k e z , k ∣ k - 1 = z k - z ^ k ∣ k - 1 S k = H P k ∣ k - 1 H T + R k A = e z , k ∣ k - 1 a b s + γ e z , k - 1 ∣ k - 1 a b s ψ k = A ̿ - 1 H P k ∣ k - 1 H T S k - 1 - 1 s a t ψ - 1 e z , k ∣ k - 1 ̿ = 1 ψ - 1 e z , k ∣ k - 1 ̿ ≥ 1 ψ - 1 e z , k ∣ k - 1 ̿ i f   - 1 < ψ - 1 e z , k ∣ k - 1 ̿ < 1 - 1 ψ - 1 e z , k ∣ k - 1 ̿ ≤ - 1 K k S V S F = H + A ̿ ∘ s a t ψ - 1 e z , k ∣ k - 1 ̿ e z , k ∣ k - 1 ̿ - 1 x ^ k ∣ k = x ^ k ∣ k - 1 + K k S V S F e z , k ∣ k - 1 P k ∣ k = I - K k S V S F H P k ∣ k - 1 I - K k S V S F H T + K k S V S F R k K k S V S F T e z , k ∣ k = z k - h ^ x ^ k ∣ k(5) Time-Varying Noise Statistic (92) q ^ k = q ^ k - 1 + d k K k S V S F e z , k r ^ k = r ^ k - 1 + d k I - H K k S V S F e z , k Q ^ k = 1 - d k Q ^ k - 1 + d k P k ∣ k - 1 - F k - 1 P k - 1 ∣ k - 1 F k - 1 T R ^ k = 1 - d k R ^ k - 1 + d k 2 I - H K k S V S F e z , k e z , k T I - H K k S V S F T - e z , k e z , k T + H P k ∣ k - 1 H TThe ASVF applies an improved/modified SVSF involving the divergence suppression method aiming to correct the state error covariance. Additionally, it is also completed by the time-varying unbiased MAP estimator so that ASVSF is able to estimate the noise statistics recursively.


## 3.1. Suboptimal MAP Noise Estimator

The complicated multistep smoothing term (x^i∣k and x^i-1∣k) in (25)-(28) might cause inefficiency of the MAP estimate; therefore to find the conventional and efficient recursive form the simplification is needed. Note that the recursive update process only utilizes the estimate value at time k-1 and k; thus the simplification can be conducted by replacing x^i-1∣k with x^i-1∣i in (25) and (26) and xi∣k with xi∣i in (27)-(28). Therefore, the suboptimal of MAP noise estimator can be expressed as follows:(29)q^k=1k∑i=1kx^i∣i-fx^i-1∣i(30)Q^k=1k∑i=1kx^i∣i-fx^i-1∣i-qi-1x^i∣,i-fx^i-1∣i-qi-1T(31)r^k=1k∑i=1kzi-hx^i∣i(32)R^k=1k∑i=1kzi-hx^i∣i-rizi-hx^i∣i-riTAs can be analysed from the sequence equations above the estimate value of xi-1∣i is not provided by the SVSF. Therefore, modifying the original form of SVSF is needed to compute the noise statistics estimator effectively


## 3.2. Modified SVSF

The process of modifying the SVSF can be done by calculating the one-step smoothing of the SVSF gain and its corresponding estimate value using the fixed point smoothing algorithm [24, 42, 43]. This process can be summarized as follows:(33)x^k∣k-1=f^x^k-1∣k-1+qk-1(34)Pk∣k-1=FxPk-1∣k-1FxT+Qk-1(35)z^k∣k-1=h^x^k∣k-1+rk(36)ez,k∣k-1=zk-z^k∣k-1(37)A=ez,k∣k-1abs+γez,k-1∣k-1abs(38)Sk=HPk∣k-1HT+Rk(39)ψk=A̿-1HPk∣k-1HTSk-1-1(40)satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1(41)KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1(42)x^k-1∣k=x^k-1∣k-1+KkSVSFez,k∣k-1Then considering that the prior state x^k-1∣k replaces the term of x^k-1∣k-1 in the normal SVSF, the remaining part of modified SVSF is chained as follows:(43)x^k∣k-1=f^x^k-1∣k+qk-1(44)z^k∣k-1=h^x^k∣k-1+rk(45)ez,k∣k-1=zk-z^k∣k-1(46)Sk=HPk∣k-1HT+Rk(47)A=ez,k∣k-1abs+γez,k-1∣k-1abs(48)ψk=A̿-1HPk∣k-1HTSk-1-1(49)satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1(50)KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1(51)x^k∣k=x^k∣k-1+KkSVSFez,k∣k-1(52)Pk∣k=I-KkSVSFHPk∣k-1I-KkSVSFHT+KkSVSFRkKkSVSFT(53)ez,k∣k=zk-h^x^k∣kNow, the estimate values x^i∣i and x^i-1∣i can be adopted from (51) and (42), respectively


## 3.3. Unbiased Suboptimal MAP Noise Estimator

Next, to guarantee that the recursive process and measurement noise statistics are unbiased, the modified suboptimal MAP noise estimators are then derived referring to unbiased estimation.First, by substituting (43) into (51), the general term x^i∣i-fx^i-1∣i in (29) and (30) can be rewritten as follows:(54)x^i∣i-fx^i-1∣i=KkSVSFez,i+qi-1Similarly, replacing x^i∣i with (51), the general term zi-hx^i∣i in (31) and (32) can be written as follows:(55)zi-hx^i∣i=zi-hx^i∣i-1+KiSVSFez,iNote that ez,i=ez,i∣i-1, and the suboptimal MAP estimation (28)-(31) can be rearranged as follows:(56)q^k=1k∑i=1kKkSVSFez,i+qi-1(57)Q^k=1k∑i=1kKkSVSFez,iez,iTKkSVSFT(58)r^k=1k∑i=1kI-HKiSVSFez,i+ri(59)R^k=1k∑i=1kI-HKiSVSFez,iez,iTI-HKiSVSFTSince the innovation ez and its covariance ezezT are contained in the process and measurement noise estimator, therefore, (60)ez,i=hx~i∣i-1+vi-riand referring to state error covariance in (52), we have (61)KiSVSFez,iez,iTKiSVSFT=Pi∣i-Pi∣i-1+Pi∣i-1HTKiSVSFT+HKiSVSFPi∣i-1Next, considering that the expectations are (62)Eez,i=0(63)Eez,iez,iT=HPi∣i-1HT+Rithen by substituting (60)-(63) into (56)-(59), we have(64)Eq^k=qk(65)Er^k=rk(66)EQ^k=-Qk+E1k∑i=1kPi∣i+Pi∣i-1HTKiSVSFT+HKiSVSFPi∣i-1-Fi-1Pi-1∣i-1Fi-1T(67)ER^k=Rk+E1k∑i=1kHKiSVSFez,iez,iTHTKiSVSFT-HKiSVSFez,iez,iT-ez,iez,iTHTKiSVSFTHPi∣i-1HTNote that qk,rk,Qk,Rk are the representation of the suboptimal MAP estimation in (56)-(59); thus the unbiased MAP estimation can be summarized as follows:(68)q^k=1k∑i=1kKiSVSFez,i+qi-1(69)r^k=1k∑i=1kI-HKiSVSFez,i+ri(70)Q^k=1k∑i=1kPi∣i-1-Fi-1Pi-1∣i-1Fi-1T(71)R^k=1k∑i=1k2I-HKiSVSFez,iez,iTI-HKiSVSFT-ez,iez,iT+HPi∣i-1HT


## 3.4. Time-Varying Unbiased Noise Estimator and Weighting Exponentially

The time-varying noise estimator is proposed in this paper. According to the unbiased suboptimal MAP estimator calculated in (68)-(71), then the time-varying unbiased noise estimators are derived as follows: (72)q^k=q^k-1+1kKkSVSFez,k(73)r^k=r^k-1+1kI-HKkSVSFez,k(74)Q^k=k-1kQ^k-1+1kPk∣k-1-Fk-1Pk-1∣k-1Fk-1T(75)R^k=k-1kR^k-1+1k2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTThe following alternative forms (76)-(79) are regarded as the modified form of the time-varying unbiased noise statistics estimator (72)-(75). It is calculated by referring to the fading memory weighted exponent method and by utilizing the weighting coefficient dk to replace the exponential 1/k. (76)q^k=q^k-1+dkKkSVSFez,k(77)r^k=r^k-1+dkI-HKkSVSFez,k(78)Q^k=1-dkQ^k-1+dkPk∣k-1-Fk-1Pk-1∣k-1Fk-1T(79)R^k=1-dkR^k-1+dk2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTwhere the residual measurement ez,k=ez,k∣k-1 and weighting coefficient can be written. (80)βi=dkbi-1,i=1,…,ndk=1-b1-bkb is fading factor satisfying 0<b<1 and βi is the i-th weighting factor, which is defined as βi=βi-1b and satisfied ∑i=1kβi.


## 3.5. Addition of Divergence Suppression Method

Next, to prevent the occurrence of filter divergence, the covariance correction based on the divergence suppression concept is involved [4]. First, the convergence condition is derived referring to the covariance matching creation as described below:(81)vkTvk≤S.trEvkTvkwhere S is an adjustable coefficient presetting which is satisfied (S≥1) and the residual sequence vk=zk-hx^k,k-1. Executing (81), if the convergence condition is satisfied, (76)-(79) are applied; otherwise, the correction method of the covariance Pk∣k-1 is suggested against the divergence occurrence.(82)Pk∣k-1=λk.Pk∣k-1where λk is known as the adaptive weighting coefficient which is calculated based on the fading factor formula [44, 45] as summarized as follows:(83)C0,k=vkTvkk=1ρC0,k+vkTvk1+ρk>1(84)Nk=trC0,k-RT(85)Mk=trPk,k-1(86)λ0=trNktrMk(87)λk=λ0λ0≥11λ0<1where tr(.) refers to the matrix trace and ρ is the forgetful factor satisfied 0<ρ≤1 (typically to be set 0.95). Note that increasing the factor will create a smaller proportion of the information before time k [4]. It causes the residual vector effect to become prominent so that the filter tracking ability will increase.Up to this point, the adaptive SVSF can be summarized as follows.ASVSF Algorithm(1) Modified SVSF (88) x ^ k ∣ k - 1 = f ^ x ^ k - 1 ∣ k - 1 + q k - 1 P k ∣ k - 1 = F x P k - 1 ∣ k - 1 F x T + Q k - 1(2) Covariance Correction (89) C 0 , k = v k T v k ρ C 0 , k + v k T v k 1 + ρ N k = t r C 0 , k - R T M k = t r P k , k - 1 λ 0 = t r N k t r M k λ k = λ 0 λ 0 ≥ 1 1 λ 0 < 1 P k ∣ k - 1 = λ k . P k ∣ k - 1(3) Modified SVSF (90) z ^ k ∣ k - 1 = h ^ x ^ k ∣ k - 1 + r k e ˘ z , k ∣ k - 1 = z k - z ^ k ∣ k - 1 A = e ˘ z , k ∣ k - 1 a b s + γ e z , k - 1 ∣ k - 1 a b s S k = H P k ∣ k - 1 H T + R k ψ k = A ̿ - 1 H P k ∣ k - 1 H T S k - 1 - 1 s a t ψ - 1 e ˘ z , k ∣ k - 1 ̿ = 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ ≥ 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ i f  - 1 < ψ - 1 e ˘ z , k ∣ k - 1 ̿ < 1 - 1 ψ - 1 e ˘ z , k ∣ k - 1 ̿ ≤ - 1 K k S V S F = H + A ̿ ∘ s a t ψ - 1 e ˘ z , k ∣ k - 1 ̿ e ˘ z , k ∣ k - 1 ̿ - 1 x ^ k - 1 ∣ k = x ^ k - 1 ∣ k - 1 + K k S V S F e ˘ z , k ∣ k - 1(4) Improved SVSF (91) x ^ k ∣ k - 1 = f ^ x ^ k - 1 ∣ k + q k - 1 z ^ k ∣ k - 1 = h ^ x ^ k ∣ k - 1 + r k e z , k ∣ k - 1 = z k - z ^ k ∣ k - 1 S k = H P k ∣ k - 1 H T + R k A = e z , k ∣ k - 1 a b s + γ e z , k - 1 ∣ k - 1 a b s ψ k = A ̿ - 1 H P k ∣ k - 1 H T S k - 1 - 1 s a t ψ - 1 e z , k ∣ k - 1 ̿ = 1 ψ - 1 e z , k ∣ k - 1 ̿ ≥ 1 ψ - 1 e z , k ∣ k - 1 ̿ i f   - 1 < ψ - 1 e z , k ∣ k - 1 ̿ < 1 - 1 ψ - 1 e z , k ∣ k - 1 ̿ ≤ - 1 K k S V S F = H + A ̿ ∘ s a t ψ - 1 e z , k ∣ k - 1 ̿ e z , k ∣ k - 1 ̿ - 1 x ^ k ∣ k = x ^ k ∣ k - 1 + K k S V S F e z , k ∣ k - 1 P k ∣ k = I - K k S V S F H P k ∣ k - 1 I - K k S V S F H T + K k S V S F R k K k S V S F T e z , k ∣ k = z k - h ^ x ^ k ∣ k(5) Time-Varying Noise Statistic (92) q ^ k = q ^ k - 1 + d k K k S V S F e z , k r ^ k = r ^ k - 1 + d k I - H K k S V S F e z , k Q ^ k = 1 - d k Q ^ k - 1 + d k P k ∣ k - 1 - F k - 1 P k - 1 ∣ k - 1 F k - 1 T R ^ k = 1 - d k R ^ k - 1 + d k 2 I - H K k S V S F e z , k e z , k T I - H K k S V S F T - e z , k e z , k T + H P k ∣ k - 1 H TThe ASVF applies an improved/modified SVSF involving the divergence suppression method aiming to correct the state error covariance. Additionally, it is also completed by the time-varying unbiased MAP estimator so that ASVSF is able to estimate the noise statistics recursively.


## 4. Adaptive SVSF-Based SLAM Algorithm

The proposed method is approached to address the SLAM problem for the mobile robot. The used robot contains two driven wheels. By supposing this, it is simulated and initially located in certain planar environment. Then the kinematic configuration of the robot movement can be shown in Figure2.Figure 2
Kinematic configuration of wheeled mobile robot.Assuming thatxr and yr are the spatial position and θr is the orientation of the robot, then the robot state vector can be expressed as xR=xr,yr,θrT.The robot moves based on the odometry and differential steering system, therefore, since the control vector contains two control vectors represented byu=ur,ulT for right wheel ur and left wheel velocity ul, respectively, then the motion model is expressed as follows:(93)xR,k=xRA,vr=vlxRB,vr≠vl(94)xRA=xr,kAyr,kAθr,kA=xr,k-1Ayr,k-1Aθr,k-1A+vcos⁡θr,k-1Asin⁡θr,k-1A0(95)xRB=xr,kByr,kBθr,kB=xr,k-1Byr,k-1Bθr,k-1B+Rr+wr2sin⁡θr,k-1B+α-sin⁡αRr+wr2-cos⁡θr,k-1B+α+cos⁡ααwhere k is time index, Rr is the radius caused by the motion with Rr=vl/α, wr is width of the robot, and ..A and ..B are the different motion condition with respect to the measured velocity v=[vr,vl]T. In fact, the robot motion is always followed by unavoidable small noise. Thus the measured right-wheel vr and left-wheel velocity can be regarded as follows with existence of the small perturbation n=[nr,nl]T:(96)v=u+n(97)nr=ζ1ur+ζ2ul-urnl=ζ1ul+ζ2ul-urwhere ζ1 is the move factor and ζ2 is the turn factor.


### 4.1. Direct Point-Based Observation Model

The state vector is composed not only by robot state vectorxR but also by the landmark state represented by xLi=xL,xi,xL,yiTand the full state vector can be denoted by x=xr,yr,θr,xL,x1,xL,y1,…,…,xL,xm,xL,ymT, where xL,xi and xL,yi represent the i-th landmark position for i=1,2,…,m.Figure3 illustrates that the robot is measuring the i-th detected landmark by utilizing the laser scanner. The position of the laser scanner is denoted by xlsR=[xls,yls]T; then the direct point-based observation model can be calculated as follows: (98)xlsyls=xryr+dlscos⁡θrsin⁡θr(99)δLiβLi=xL,xi-xls2+xL,yi-yls2arctan⁡xL,yi-ylsxL,xi-xlswhere dls is the displacement of laser scanner and δLi and βLi are the distance and bearing sensed by laser scanner. Up to this point, we have satisfied the measurement function h(.). Similarly, by considering that the measurement is followed by small perturbation denoted by r=rδ,rβT, we have(100)zi=δLiβLi+rδrβThen, since (93)-(100) are derived completely, both state transition in (33) and measurement function in (35) are satisfied. Next, H+can be calculated as follows: (101)Fps,i=1000⋯0000⋯00100⋯0000⋯00010⋯0000⋯00000⋯0100⋯00000⋯︸2i-20010⋯︸2m-2i0(102)Hps,i=Hrpsx,iHrpsx,i=HTHHT-1,rows ofHare  linearly  independentHrpsx,i=HHT-1HT,columns  ofHare  linearly  independent(103)H+=Fps,iTHps,iwhere H is the Jacobian matrix of h(.) with respect to the predicted state in (33)-(53).Figure 3
Graphical representation of landmark detection.


### 4.2. Inverse Point-Based Observation

Generally, new observed landmark must be initialized and added to the state vector. In this experiment, an inverse point-based observation model initiates the mapping process by utilizing the information of the current robot and landmark position which can be written as follows:(104)xLnew=h-1xR,k,zi(105)xL,xnewxL,ynew=xryr+δLicos⁡θrcos⁡βLi-sin⁡θrsin⁡βLisin⁡θrcos⁡βLi+cos⁡θrsin⁡βLiAt this point, the derivation of SVSF and SLAM algorithm used for wheeled mobile robot are fully formulated. Therefore, by chaining them we have ASVSF-SLAM algorithm.


### 4.3. Adaptive SVSF Based SLAM Algorithm

begin algorithm(i)
Initialization(106)x^0,P0,ez,0,q^0,Q^0r^0,R0,γ(ii)
Prediction update(one-step smoothing method)(107)x˘k∣k-1=f^x^k-1∣k-1+q^k-1Pk∣k-1=FxPk-1∣k-1FxT+Q^k-1(iii)
Observation and Updateforall feature observationifcorrespondence is found before seen landmarkcalculate a posteriori measurement error(108)z^k∣k-1=h^x˘k∣k-1+r^ke˘z,k∣k-1=zk-z^k∣k-1Covariance correction(109)vk=e˘z,k∣k-1vkTvk≤S.trEvkTvkC0,k=vkTvkk=1ρC0,k+vkTvk1+ρk>1Nk=trC0,k-RTMk=trPk,k-1λ0=trNktrMkλk=λ0λ0≥11λ0<1Pk∣k-1=λk.Pk∣k-1Compute one-step smoothing SVSF gain(110)A=e˘z,k∣k-1abs+γez,k-1∣k-1absSk=HPk∣k-1HT+R^kψk=A̿-1HPk∣k-1HTSk-1-1satψ-1e˘z,k∣k-1̿=1ψ-1e˘z,k∣k-1̿≥1ψ-1e˘z,k∣k-1̿if-1<ψ-1e˘z,k∣k-1̿<1-1ψ-1e˘z,k∣k-1̿≤-1KkSVSF=H+A̿∘satψ-1e˘z,k∣k-1̿e˘z,k∣k-1̿-1Calculate the smoothed statex^k-1∣k(111)x^k-1∣k=x^k-1∣k-1+KkSVSFe˘z,k∣k-1Conduct the improved SVSF(112)x^k∣k-1=f^x^k-1∣k+q^k-1z^k∣k-1=h^x^k∣k-1+r^kez,k∣k-1=zk-z^k∣k-1Sk=HPk∣k-1HT+R^kA=ez,k∣k-1abs+γez,k-1∣k-1absψk=A̿-1HPk∣k-1HTSk-1-1satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1Update state and covariance(113)x^k∣k=x^k∣k-1+KkSVSFez,k∣k-1Pk∣k=I-KkSVSFHPk∣k-1I-KkSVSFHT+KkSVSFRkKkSVSFTEstimate noise statistics(114)ez,k=ez,k∣k-1q^k=q^k-1+dkKkSVSFez,kr^k=r^k-1+dkI-HKkSVSFez,kQ^k=1-dkQ^k-1+dkPk∣k-1-Fk-1Pk-1∣k-1Fk-1TR^k=1-dkR^k-1+dk2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTUpdate the prior measurement error(115)ez,k∣k=zk-h^x^k∣kend ifend for(iv)
Map Managementforall nonassociated new featureInitialize a new feature(116)xLnew=h-1xR,k,ziIncrement the state vector by addingxLnewto the current state vectorx^current(117)x^increment=x^current,xLnewJumpx^incrementto Observation and Update ProcessTime Increment(118)k=k+1end forend begin


## 4.1. Direct Point-Based Observation Model

The state vector is composed not only by robot state vectorxR but also by the landmark state represented by xLi=xL,xi,xL,yiTand the full state vector can be denoted by x=xr,yr,θr,xL,x1,xL,y1,…,…,xL,xm,xL,ymT, where xL,xi and xL,yi represent the i-th landmark position for i=1,2,…,m.Figure3 illustrates that the robot is measuring the i-th detected landmark by utilizing the laser scanner. The position of the laser scanner is denoted by xlsR=[xls,yls]T; then the direct point-based observation model can be calculated as follows: (98)xlsyls=xryr+dlscos⁡θrsin⁡θr(99)δLiβLi=xL,xi-xls2+xL,yi-yls2arctan⁡xL,yi-ylsxL,xi-xlswhere dls is the displacement of laser scanner and δLi and βLi are the distance and bearing sensed by laser scanner. Up to this point, we have satisfied the measurement function h(.). Similarly, by considering that the measurement is followed by small perturbation denoted by r=rδ,rβT, we have(100)zi=δLiβLi+rδrβThen, since (93)-(100) are derived completely, both state transition in (33) and measurement function in (35) are satisfied. Next, H+can be calculated as follows: (101)Fps,i=1000⋯0000⋯00100⋯0000⋯00010⋯0000⋯00000⋯0100⋯00000⋯︸2i-20010⋯︸2m-2i0(102)Hps,i=Hrpsx,iHrpsx,i=HTHHT-1,rows ofHare  linearly  independentHrpsx,i=HHT-1HT,columns  ofHare  linearly  independent(103)H+=Fps,iTHps,iwhere H is the Jacobian matrix of h(.) with respect to the predicted state in (33)-(53).Figure 3
Graphical representation of landmark detection.


## 4.2. Inverse Point-Based Observation

Generally, new observed landmark must be initialized and added to the state vector. In this experiment, an inverse point-based observation model initiates the mapping process by utilizing the information of the current robot and landmark position which can be written as follows:(104)xLnew=h-1xR,k,zi(105)xL,xnewxL,ynew=xryr+δLicos⁡θrcos⁡βLi-sin⁡θrsin⁡βLisin⁡θrcos⁡βLi+cos⁡θrsin⁡βLiAt this point, the derivation of SVSF and SLAM algorithm used for wheeled mobile robot are fully formulated. Therefore, by chaining them we have ASVSF-SLAM algorithm.


## 4.3. Adaptive SVSF Based SLAM Algorithm

begin algorithm(i)
Initialization(106)x^0,P0,ez,0,q^0,Q^0r^0,R0,γ(ii)
Prediction update(one-step smoothing method)(107)x˘k∣k-1=f^x^k-1∣k-1+q^k-1Pk∣k-1=FxPk-1∣k-1FxT+Q^k-1(iii)
Observation and Updateforall feature observationifcorrespondence is found before seen landmarkcalculate a posteriori measurement error(108)z^k∣k-1=h^x˘k∣k-1+r^ke˘z,k∣k-1=zk-z^k∣k-1Covariance correction(109)vk=e˘z,k∣k-1vkTvk≤S.trEvkTvkC0,k=vkTvkk=1ρC0,k+vkTvk1+ρk>1Nk=trC0,k-RTMk=trPk,k-1λ0=trNktrMkλk=λ0λ0≥11λ0<1Pk∣k-1=λk.Pk∣k-1Compute one-step smoothing SVSF gain(110)A=e˘z,k∣k-1abs+γez,k-1∣k-1absSk=HPk∣k-1HT+R^kψk=A̿-1HPk∣k-1HTSk-1-1satψ-1e˘z,k∣k-1̿=1ψ-1e˘z,k∣k-1̿≥1ψ-1e˘z,k∣k-1̿if-1<ψ-1e˘z,k∣k-1̿<1-1ψ-1e˘z,k∣k-1̿≤-1KkSVSF=H+A̿∘satψ-1e˘z,k∣k-1̿e˘z,k∣k-1̿-1Calculate the smoothed statex^k-1∣k(111)x^k-1∣k=x^k-1∣k-1+KkSVSFe˘z,k∣k-1Conduct the improved SVSF(112)x^k∣k-1=f^x^k-1∣k+q^k-1z^k∣k-1=h^x^k∣k-1+r^kez,k∣k-1=zk-z^k∣k-1Sk=HPk∣k-1HT+R^kA=ez,k∣k-1abs+γez,k-1∣k-1absψk=A̿-1HPk∣k-1HTSk-1-1satψ-1ez,k∣k-1̿=1ψ-1ez,k∣k-1̿≥1ψ-1ez,k∣k-1̿if-1<ψ-1ez,k∣k-1̿<1-1ψ-1ez,k∣k-1̿≤-1KkSVSF=H+A̿∘satψ-1ez,k∣k-1̿ez,k∣k-1̿-1Update state and covariance(113)x^k∣k=x^k∣k-1+KkSVSFez,k∣k-1Pk∣k=I-KkSVSFHPk∣k-1I-KkSVSFHT+KkSVSFRkKkSVSFTEstimate noise statistics(114)ez,k=ez,k∣k-1q^k=q^k-1+dkKkSVSFez,kr^k=r^k-1+dkI-HKkSVSFez,kQ^k=1-dkQ^k-1+dkPk∣k-1-Fk-1Pk-1∣k-1Fk-1TR^k=1-dkR^k-1+dk2I-HKkSVSFez,kez,kTI-HKkSVSFT-ez,kez,kT+HPk∣k-1HTUpdate the prior measurement error(115)ez,k∣k=zk-h^x^k∣kend ifend for(iv)
Map Managementforall nonassociated new featureInitialize a new feature(116)xLnew=h-1xR,k,ziIncrement the state vector by addingxLnewto the current state vectorx^current(117)x^increment=x^current,xLnewJumpx^incrementto Observation and Update ProcessTime Increment(118)k=k+1end forend begin


## 5. Result and Discussion

In order to verify the effectiveness and accuracy, the proposed method was simulated. It was compared with classical algorithm in term of RMSE of the estimated path and map. Ideally, the absence of noise in both process and measurement gives the reference trajectory as the base, while the presence of these noises following the process and measurement will obviously give the difference to both expected path and map. Henceforth this difference is termed as the residual values. Based on it the RMSE can be calculated to qualitatively represent the filter performance referring to definition of SLAM which are locating the robot current location (path) when a robot detects new feature in the environment and mapping all observed features in the environment (map). Therefore, on the other hand it can be defined that by knowing the different between the actual and predicted position of the path and map, the effectiveness and accuracy of the proposed method can be validated. Initially, some parameters related to the robot and the proposed algorithm were defined as can be seen below:(119)wr=33cm,dls=14cm,γ=15e-2andez,0=0.1;0.5π180Furthermore, the initial state and its error covariance were defined as follows: (120)x^o=0035π180,Po=1.52000120002.5π1802According to the initial noise statistic there would be two different simulation cases presented in this paper. Both can be shown below.(i)
1stTest: the initial process and measurement noise are considered as follows: (121)q^o=0.03π180,Q^o=0.03200π1802,r^o=0.23π180,R^o=0.22003π1802Then the result of SVSF-SLAM and SVSF-SLAM algorithm can be compared based on Figure 4.Figure 4
SVSF and ASVSF-SLAM algorithm.Figure4 illustrates the performance of SVSF and ASVSF-SLAM algorithm applied for autonomous robot. It depicts that the ASVSF-SLAM algorithm provides better solution proven by the successful in following the reference path. For more detail, it can be analysed by the result shown in Figure 5.Figure 5
RMSE of estimated path.Figure5 shows the RMSE of different algorithm performances in estimating the path. Comparing to the conventional approach, ASVSF-SLAM algorithm has better accuracy pointed by the smaller RMSE in all benchmarks. Furthermore, in an effort to provide more comparative result, the different RMSE of estimated map is also presented as shown in Figure 6.Figure 6
RMSE of estimated map.Figure6 shows the different quality of SVSF/ASVSF-SLAM algorithm in estimating the location of the landmark. According to this figure, the proposed method shows less optimal results in the estimated map for x-coordinate, but it still shows better results in estimating the y-coordinate landmark compared with SVSF-SLAM algorithm. Thus, ASVSF-SLAM algorithm can still guarantee the filter stability. To confirm this statement, Table 1 is presented.Table 1
RMSE of SVSF and ASVSF-SLAM algorithm.
Algorithm x-Pose(cm) y-Pose(cm) Heading(rad) Landmark x-Coordinate (cm) Landmark y-Coordinate (cm) SVSF-SLAM 5.5823 9.7952 0.1045 11.2296 14.4148 ASVSF-SLAM 4.6835 2.3872 0.0989 13.2414 11.5277In average, the proposed method gives higher quality in estimating path and landmark. It is significantly shown by the better achievement compared to SVSF-SLAM algorithm in almost all benchmark.(ii)
2stTest: the initial process and measurement noise are considered as follows:(122)q^o=0.085π180,Q^o=0.082005π1802,r^o=0.83π180,R^o=0.82003π1802Like the previous experiment, the general performance of SVSF and ASVSF-SLAM algorithm is evaluated from the graphical performance shown in Figure 7.Figure 7
SVSF and ASVSF-SLAM algorithm.Figure7 depicts that the increment of initial noise statistic does not affect the stability of ASVSF-SLAM. Therefore, it can be noted that the proposed method provides more stable filter compared with SVSF-SLAM algorithm. For the 2nd Test, the SVSF and ASVSF-SLAM performance are also compared in terms of RMSE. It is depicted by Figure 8.Figure 8
Estimated path.According to Figure8, ASVF-SLAM algorithm shows its effectiveness in locating current robot position. It is proven by the smaller RMSE for the all benchmark. Then, it can be noted that the accuracy of ASVSF-SLAM algorithm is guaranteed even though there exists a noise statistic increment. Additionally, the result of estimated map is also presented as shown in Figure 9.Figure 9
RMSE of estimated map.Similarly, Figure9 shows that the SVSF-SLAM has better solution of estimating map for x-coordinate, pointed by small different, while the proposed method shows better solution in estimating y-coordinate significantly. Therefore, ASVSF-SLAM performed well-enough in estimating the landmark. It is confirmed by Table 2.Table 2
RMSE of SVSF and ASVSF-SLAM algorithm.
Algorithm x-Pose(cm) y-Pose(cm) Heading(rad) Landmark x-Coordinate (cm) Landmark y-Coordinate (cm) SVSF-SLAM 5.9065 10.0448 0.1099 10.8078 13.6891 ASVSF-SLAM 4.6835 2.3872 0.0989 11.4657 11.0790This table shows clearly that the ASVSF-SLAM algorithm provides a stable filter. Additionally, according to the result of1st and 2nd Test presented above, it can be noted that the ASVSF-SLAM has better quality in providing solution under noise statistics increment. The presence of one-step smoothing and time-varying of the noise statistic does not damage the characteristic of SVSF. But instead, it precisely increases the accuracy of SVSF proven by the achievement in Figures 5 and 8.


## 6. Conclusion

In this paper the ASVSF algorithm is proposed to solve the SLAM dynamic problem since there exist the inaccurate model, dynamic system uncertainty, and unknown noise statistics. The recursive time-varying noise estimator is derived based on MAP creation and weighted exponent which tunes the SVSF by utilizing the one-step smoothing method. It results in the enhanced SVSF. Additionally, to provide the ability of avoiding filter divergences, the ASVSF also applied implies a covariance correction process adopted from the divergence suppression concept. The ASVSF was implemented for solving SLAM problem as the proposed algorithm named as ASVSF-SLAM algorithm. It was simulated and compared to the SVSF-SLAM. The simulated results in terms of RMSE of estimated path and map were analysed and it has been showing the effectiveness and robustness of the ASVSF-SLAM algorithm.



---

*Source: 2765731-2019-05-09.xml*