J Intell Robot Syst DOI 10.1007/s10846-014-0089-7

Methods for Improving the Linearization Problem of Extended Kalman Filter Farhad Farhadi Yadkuri · Mohammad Javad Khosrowjerdi

Received: 20 February 2014 / Accepted: 27 July 2014 © Springer Science+Business Media Dordrecht 2014

Abstract In this paper, in order to reduce the linearization error of Kalman filters family, three new methods are proposed and their effectiveness and feasibility are evaluated by means of Simultaneous Localization and Mapping (SLAM) problem. In derivative based methods of Kalman filters family, linearization error is brought into estimation unavoidably because of using Taylor expansion to linearize nonlinear motion model and observation model. These three methods lessen the linearization error by replacing the Jacobian matrix of observation function with new formulas. Simulation results done with ‘Car Park Dataset’ indicate that all proposed methods have less linearization error than other mentioned methods and the method named Improved Weighted Mean Extended Kalman Filter (IWMEKF) performs much better than other mentioned Kalman filters in this paper on linearization error. In addition, simulation results confirm that our proposed approaches are computationally efficient. From estimation accuracy and computational complexity point of view, IWMEKF is the best solution for solving nonlinear SLAM problem among all Kalman filters mentioned in this paper.

F. Farhadi Yadkuri · M. J. Khosrowjerdi () Department of Electrical Engineering, Sahand University of Technology, Tabriz, Iran e-mail: [email protected] F. Farhadi Yadkuri e-mail: [email protected]

Keywords Kalman filters family · Simultaneous localization and mapping (SLAM) · Linearization error · Mobile robot

1 Introduction There is a dream: to have an autonomous vehicle and remove humans from vehicle operation in order to avoid human errors and prevent presence of humans in dangerous places. Simultaneous Localization and Mapping (SLAM) algorithm provides the means to make a vehicle truly autonomous. It enables a robot to operate in an unknown environment autonomously by building a map of that environment while simultaneously localizing itself within this map. The SLAM problem has become an important and attractive topic in the robot community for two decades and it has been applied to various kinds of vehicles in different environments from landing of spacecraft to underwater robots [1–5]. Probabilistic solution is often used to solve the SLAM problem [6] and it is divided into two main categories. The first category is Kalman filters family and the second one is Particle filters family. In addition, the first one consists of two main groups. The first group named derivative based methods use Taylor expansion to analytically linearize the nonlinear motion model and observation model. Extended Kalman Filter (EKF) based SLAM which falls into this group is the most common approach among

J Intell Robot Syst

all SLAM algorithms because of its simplicity and effectiveness [7–10]. The main drawback of EKF is linearization error. In [11], it is shown that iterating the correction step of Kalman filters family reduces the linearization error and stepwise increment version of EKF named Stepwise Extended Kalman Filter (SEKF) which is presented in [12] has less linearization error than EKF. Besides, in [13], a new filter named ‘Adaptive Iterated Square-Root Cubature Kalman Filter’ is presented to lessen the linearization error. The second group named derivative-free methods utilizes unscented transformation instead of Taylor expansion for state estimation process such as Unscented Kalman Filter (UKF) based SLAM [14] which is more accurate than EKF-SLAM, but its computational complexity is more than EKF-SLAM. The second category is particle filters family. The particle filters approximate the probabilistic density function of state vector using a number of discrete random samples. Because of high computational complexity, particle filter is not suitable for mapping, so FastSLAM which simultaneously uses particle filter for robot localization and EKF for mapping is first presented in [15]. Cubature FastSLAM is the other method of particle filters family which uses cubature particle filter for localization and cubature Kalman filter for mapping [16]. Thus, it is very important to find new Kalman filters with low linearization error, high estimation accuracy and low computational complexity. Since Mean Extended Kalman Filter (MEKF) reduces the linearization error of EKF by presenting a new formula instead of Jacobian matrix of observation function in EKF [12], so it can be concluded that revising the Jacobian matrix of observation function reduce the linearization error of Kalman filters family. In this paper, three new methods of Kalman filters family are proposed and SLAM problem is used to evaluate the performance of them. The main idea of these methods is to find more accurate curve fitting techniques which lessen the linearization error by replacing the Jacobian matrix of observation function with new formulas. These new formulas decrease the linearization error by increasing the accuracy of linear approximation of observation function. Simulation is carried out with ‘Car Park Dataset’ to investigate the performance of our proposed methods. Simulation results confirmed the better estimation accuracy and less linearization error of all proposed approaches than other mentioned Kalman filters in

this paper and the method named ‘Improved Weighted Mean EKF’ (IWMEKF) is the most accurate solution for solving SLAM problem and the methods named ‘Improved Mean EKF’ (IMEKF) and ‘Weighted Mean EKF’ (WMEKF) follow it in-order. Simulation results also confirm that all proposed methods are computationally efficient. Thus, with a view to estimation accuracy, linearization error and computational complexity, IWMEKF can be considered the best filter of Kalman filters family among all mentioned Kalman filters in this paper for solving SLAM problem. This paper is organized as follows: In Section II, EKF-SLAM algorithm and linearization principle is reviewed and Section III describes MEKF. Three new methods including WMEKF, IMEKF and IWMEKF are presented in Section IV and simulation results are presented to validate the effectiveness of all proposed approaches in Section V. Finally, Section VI concludes the paper.

2 EKF-SLAM Algorithm The purpose of SLAM is to estimate the robot pose and landmarks location simultaneously by using observations obtained from internal and external sensors which are installed on the robot. In this section, EKF-SLAM algorithm is described in details. Motion model and observation model are shown in Fig. 1. In SLAM problem, the vehicle pose Xv , state vector of the landmarks Xl and control input uk−1 are given by Xv = [xv , yv , θv ]T Xl = [x1 , y1 , ..., xi , yi , ..., xN , yN ]T uk−1 = [vk−1 , αk−1 ]T where (xv , yv ) and (xi , yi ) are the coordinates of the vehicle and ith landmark. θv , α and v represent the bearing and steering angle of the vehicle and velocity of the back axle. The state vector is described as X = [XvT , XlT ]T

(1)

Landmarks are fixed features which can easily be re-observed and distinguished from the environment. Thus, the process model are formulated as follows       f (Xk−1,v , uk−1 ) Wk−1,v Xk,v (2) = + Xk,l 0 Xk−1,l

J Intell Robot Syst

where f (.) and Wk−1,v represent the motion function and zero mean Gaussian motion noise respectively. Besides, the observation model is described as follows

time k − 1. Covariance of [5] Pk−

Zk = h(Xk ) + Vk

 = 

(3) =

− Xk

is calculated as follows



− − Pvv,k Pvl,k − − Plv,k Pll,k

+ T +Q ∇fk−1 Pvv,k−1 ∇fk−1 k−1 + T Plv,k−1 ∇fk−1

+ ∇fk−1 Pvl,k−1 + Pll,k−1

 (6)

where  h(Xk ) =

rk βk



  (xk,v − xk,i )2 + (yk,v − yk,i )2 = yk,v −yk,i arctan( xk,v −xk,i ) − θk,v + π2

+ + Plv,k−1 = Pvl,k−1

(4)

where h(.) and Vk represent the observation function for ith landmark and observation noise respectively. In addition, rk represents the distance between the vehicle and ith landmark (range) and βk is the bearing of the ith landmark with respect to the vehicle (bearing). The first step of EKF is to predict the state vector. A prior estimation of state vector at time k is computed by using motion model as 

− X k,v − X k,l



 =

f (X+ k−1,v , uk−1 )



+ X k−1,l

(5)

+ Pvv,k−1

+ X k−1,l

and are the estimated vehicle where pose and estimated state vector of the landmarks at

(7) + Pvl,k−1

and are the covariance on the where state vector of the vehicle and covariance between + state vector of the vehicle and landmarks. Pll,k−1 and Qk−1 represent the covariance on the state vector of the landmarks and covariance of Wk−1,v . Jacobian matrix of motion function at time k − 1is formulated as follows ∂f  ∇fk−1 = (8)  + ∂Xv X k−1,v Observations rk and βk are obtained from external sensors such as laser range finder and eventual landmarks are extracted from observations. Then, coordinates of each extracted landmark is estimated as follows [17] −

+ X k−1,v

T

X k,lnew

= g( − , Zk ) X k,v  − = x− + rk cos(βk + θ − − π2 ) k,v k,v new ≡ x k,l − − + r sin(β + − − π ) = k k y y θ k,v 2 k,lnew

(9)

k,v

− − T where X − k,lnew = [ x k,lnew , y k,lnew ] represents the coordinates of the extracted landmark and g(.) is the inverse function of h(.). In data association process each extracted landmark is associated to a database which has been set up to store all observed landmarks and their iteration numbers. If extracted landmark does not exist in database, it is added to database and its iteration number is set to 1, but iteration number of re-observed landmark is added to 1 and if it is lower than a certain number, the observations of this re-observed landmark is used to correct the predicted − state vector overX − k and its covariance Pk as follows ∂h  (10) ∇hk =  − ∂X X k

Kk = Pk− ∇hTk (∇hk Pk− ∇hTk + Rk )−1 +

Fig. 1 Motion model and observation model

Xk

=

−

Xk

+ Kk (Zk − h(

−

Xk

))

(11) (12)

J Intell Robot Syst

Pk+ = (I − Kk ∇hk )Pk−

(13)

where Kk and ∇hk are the Kalman gain and the Jaco+ bian matrix of observation function. Rk , X + k and Pk represent the covariance matrix of observation noise, final estimation of state vector at time k and its covariance, respectively. If iteration number is equal to N, corresponding landmark is added to state vector as follows  T − −T − − = , , (14) X k,new X k x k,lnew y k,lnew Then predicted covariance matrix Pk− is extended as follows [5] ⎤ ⎡ − − Pvl−new ,k Pvv,k Pvl,k − − − Pll,k Pll−new ,k ⎦ Pk,new (15) = ⎣ Plv,k − − Plnew v,k Plnew l,k Pl− new lnew ,k where − Pl− = Gv Pvv,k = Pvl−T new v,k new ,k

(16)

− = Gv Pvl,k = Pll−T Pl− new l,k new ,k

(17)

− Pl− = Gv Pvv,k GTv + GV RGTV new lnew ,k

(18)

  where Gv = ∂g/∂Xv  − ,Zk and GV = X k,v   ∂g/∂V  − ,Zk represent the Jacobian of g with X k,v respect to Xv and V . Then, predicted state and its covariance are corrected by replacing X − k,new and − in Eqs. 10 to 13. Pk,new Graphic fundamental of linearization principle used in EKF is shown in Fig. 2. Posterior state estimate of EKF ( X + k ) is obtained by linearizing the observation function (h(X)) up to first order term of Taylor expansion and replacing Z = Zk in the approximated line. Approximated line is described as follows Z = h(X) ≈ h(X0 ) + ∇h(X − X0 )

(19)

where X0 is the point that observation function is linearized around it and in EKF, it is the predicted state vector ( X − k ). If Z is replaced with Zk , X indicates the posterior state estimate of EKF. In Fig. 2, line (1) which passes from (X0 , h(X0 )) and (Xk , Zk ) is the most accurate approximated line of the observation function because its slope is given by mreal =

Zk − h(X0 ) Xk − X0

(20)

Fig. 2 Graphic fundamental of EKF

and in the approximated line as given by Eq. 19, if Hk and Z are replaced by mreal and Zk respectively, X will be exactly equal to real state vector Xk . Thus, in Fig. 2, line (2) is more accurate approximation of observation function than line (3) because the slope of line (2) is closer to mreal than slope of line (3). In other words, slope of line (2) is more precise than slope of line (3).

3 Mean Extended Kalman Filter (MEKF) The detailed process of MEKF is similar with that of EKF and the main difference between them is that Jacobian matrix of observation function in EKF is   replaced with the mean of ∂h/∂X  − and ∂h/∂X Xk Xk [12]. ∂h  ∂h  Hk = ( (21)  − + X )/2 ∂X X k ∂X k Kalman gain is calculated with Hk as follows Kk = Pk− HkT (Hk Pk− HkT + Rk )−1

(22)

Jacobian matrix of observation function is given by   ∂h A B 0 0 0 ... −A −B ... 0 0 = (23) C D −1 0 0 ... −C −D ... 0 0 ∂X where  A= √ C=

xv −xi ,B (xv −xi )2 +(yv −yi )2 yv −yi − (x −x )2 +(y −y )2 , D v v i i

yv −yi (xv −xi )2 +(yv −yi )2 xv −xi (xv −xi )2 +(yv −yi )2

=√ =

(24)

J Intell Robot Syst

In Eq. 23, three former columns and two non-zero columns are the Jacobian of observation function with respect to the vehicle pose and coordinates of the ith landmark. Since real state Xk is unknown, the Jacobian matrix of observation function at Xk can be calculated with xk − xk,i and yk − yk,i which represent the landmark coordinates with respect to the vehicle [12]. xk − xk,i = − yk − yk,i

rk × sign(cos(βk + θk − π/2))  1 + tan2 (βk + θk − π/2)

(25)

= −

rk × sign(sin(βk + θk − π/2)) × |tan(βk + θk − π/2)|  1 + tan2 (βk + θk − π/2) (26)

where θk is unknown and its estimation θ − is used k,v instead of it [12]. Eqs. 25 and 26 are deduced from observation function described in Eq. 4. Graphic fundamental of MEKF is shown in Fig. 3 [12]. In this figure, lines (1), (2) and (3) indicate the most precise approximated line and approximated line obtained from MEKF and EKF respectively.

4 Three New Methods of Kalman Filters Family EKF suffers from linearization error. In EKF, linearization process is carried out with Taylor series − expansion  up to first order term around X k and  ∂h/∂X  − is considered as the slope Xk

of linear approximation of observation function. But in MEKF, the slope of linear approximation of observation function is equivalent to the mean of    − ∂h/∂X  and ∂h/∂X Xk . In [12], it was shown Xk that MEKF has less linearization error than EKF; therefore, it can be concluded that inaccuracy of the formula used to compute the slope of linear approximation of observation function is the reason why EKF suffers from linearization error. In this paper, three new methods of Kalman filters family are proposed. These three methods reduce the linearization error by replacing the Jacobian matrix of observation function in EKF with new formulas. 4.1 Weighted Mean Extended Kalman Filter (WMEKF) In WMEKF, a new method is presented for computing the slope of linear approximation of observation function. The details of this method are explained. First, the region between h( X − k ) and Zk is partitioned into n equal parts as follows Z(j ) = [r(j ), β(j )]T j = h( − ) + (Zk − h( − )), j = 0, 1, ..., n Xk n Xk

(27)

where Z(n) is a real observations at time k, but Z(j ) for j = 0, 1, ..., n − 1 is hypothetical observations which is not obtained from sensors. State vectors corresponding to hypothetical observations are assumed as follows X(0) =

−

Xk

, X(1), ..., X(j ), ..., X(n) = Xk

(28)

Jacobian matrix of observation function at all X(j )s is calculated and hypothetical Jacobiansare obtained as follows ∂h  ∂h  ∂h  X=X(0) , X=X(1) , ..., X=X(j ) , ..., ∂X ∂X ∂X ∂h  T (29) X=X(n) ] ∂X

Jh = [

Fig. 3 Graphic fundamental of MEKF [12]

All of X(j )s excluding X(0) are unknown; thus, Jacobian matrix of observation function at X(0) is computed by substituting predicted state vector in Eq. 23 and Eq. 24 and Jacobian matrix of observation function at X(1), X(2), ..., X(n) can be calculated

J Intell Robot Syst

with x(j )−xi (j ) and y(j )−yi (j ) which are described as follows x(j ) − xi (j )

Predicted state and real observations at time k = 1176 are described as follows − Xk

=

(30)

r(j ) × sign(cos(β(j ) + θk − π/2)) −  1 + tan2 (β(j ) + θk − π/2)

,

j = 1, ..., n

and estimated observations are given by h(

y(j ) − yi (j ) r(j ) × sign(sin(β(j ) + θk − π/2)) × |tan(β(j ) + θk − π/2)| =−  1 + tan2 (β(j ) + θk − π/2) , j = 1, ..., n (31)

Equations 30 and 31 are obtained by replacing r(j ) and β(j ) instead of rk and βk in Eqs. 25 and 26. is Besides, θk is unknown and its estimation θ − k,v used instead of it in Eqs. 30 and 31. Then a scalar weight is allocated to each Jacobian in Eq. 29 and the slope of linear approximation of observation function is calculated as Hkw

=

1 ∂h  ∂h  [s0 ( X=X(0) ) + s1 ( X=X(1) ) n

∂X ∂X si i=0

+... + sj (

∂h  ∂h  X=X(j ) ) + ... + sn ( X=X(n) )] ∂X ∂X

(32)

Now Kalman gain is computed with Hkw instead of ∇hk in Eq. 11 as T T Kk = Pk− Hkw (Hkw Pk− Hkw + Rk )−1

(33)

Final estimation of state vector at time k and its covariance are obtained with following formulas. +

Xk

=

−

Xk

+ Kk (Zk − h(

Pk+ = (I − Kk Hkv )Pk−

−

Xk

))

= [2.8496, 3.0996, 1.2689, ..., 2.4213, 5.4293]T Zk = [2.2798, −0.9085]T

−

Xk

) = [h1 (

−

Xk

), h2 (

−

Xk

)]T = [2.3687, −0.9913]T

Besides, real state vector which is obtained by GPS is as follows Xk = [2.6934, 3.3465, 1.0921, ..., 2.2772, 5.5880]T In this example, the linearization of h1 (X) around X − k using MEKF and WMEKF are investigated. The slope of the most accurate linear approximation is described as follows Mreal =

rk − h1 ( X − k) xk −

− xk

In general, three case exist for Mreal : 1) Mreal falls   between ∂h1 /∂x X= − and ∂h1 /∂x X=Xk . 2) Mreal Xk is out  of mentioned interval and is near to  ∂h1 /∂x X= − . 3) Mreal is out of mentioned interval Xk  and is near to ∂h1 /∂x X=Xk . In the first case, second case and third case, we can increase the accuracy of Hkw by allocating big weights for middle Jacobians, the first Jacobian and last Jacobian of Jh in Eq. 29 respectively. Mreal is unknown and it is impossible to determine its case, but choosing suitable weights can be carried out by examining all three mentioned set of weights. Mreal and slope of linear approximation of observation function obtained with MEKF

(34)

(35)

Graphic fundamental of WMEKF is shown in Fig. 4 and Hkw is the weighted mean of the slope of line (1), (2) and (3). In contrast with MEKF, Hkw is adjustable in WMEKF because of using specific weights for each Jacobian. Thus, in WMEKF, choosing appropriate weights will improve the accuracy of linear approximation of observation function and lead to higher estimation accuracy than MEKF. This characteristic is shown in following example with real data named ‘Car Park Dataset’ [17].

(36)

Fig. 4 Graphic fundamental of WMEKF for n = 2

J Intell Robot Syst Table 1 Hk for MEKF and Hkw for WMEKF at k= 1176 Mreal

Hk (MEKF)

Hkw (WMEKF)

Weights for WMEKF

0.5691 0.5691 0.5691 0.5691 0.5691 0.5691 0.5691 0.5691 0.5691 0.5691

-0.0859 -0.0859 -0.0859 -0.0859 -0.0859 -0.0859 -0.0859 -0.0859 -0.0859 -0.0859

-0.2144 -0.0859 0.2193 0.0741 0.1141 0.1506 0.1755 0.3142 0.5671 0.5691

[1, 0, ..., 0, 1, 1, 1] [1, 0, ..., 0, 1] [1, 0, ..., 3] [4, 0, ..., 1] [7, 0, ..., 1] [10, 0, ..., 0.6] [10, 0, ..., 0.1] [10, 0, ..., 0, −2] [10, 0, ..., 0, −4.2] [10, 0, ..., 0, −4.2125]

and WMEKF are shown in Table 1. Mreal and Hk for MEKF are 0.5691 and −0.0895, respectively, but Hkw for WMEKF depends on weights. Hypothetical Jacobians for n = 12 is described as follows Jh = [0.1808, −0.2807, −0.2873, −0.2939, −0.3005, −0.3071, ... −0.3136, −0.3202, −0.3267, −0.3332, −0.3397, −0.3462, −0.3526]T Mreal falls into second case and the accuracy of Hkw can be increased by devoting large value to the first weight and allocating small values to other weights in Jh . Thus, in WMEKF Hkw is adjustable and choosing the best weights can lead to much accurate Hk . On the other hand, only one set of weights is considered for all time steps in WMEKF-SLAM and specifying the case of Mreal is impossible in each time step, so it is too difficult to choose weights for gaining Hkw exactly equal to Mreal , but the following approach can help us to select the best weights for obtaining higher accuracy of Hkw . The main idea of this method is to determine the most effective weights on the accuracy of Hkw and allocate large values to them. Firstly, each n consecutive Jacobians is put into a cluster and in each case a large weight is devoted to one cluster. After investigating the estimation accuracy of each case, the most effective cluster that is caused the best estimation accuracy is selected. Secondly, all mentioned steps are repeated for the most effective cluster and clustering continues until the most effective weights are specified. Finally, large values are devoted to these weights and small weights are allocated to other weights.

In Fig. 4, to facilitate the introduction of WMEKF the number of hypothetical Jacobians (n) is assumed equal to 2. However in a real problem a bigger value of n may be needed for reaching higher estimation accuracy. Although bigger n leads to higher estimation accuracy, it also increases the computational complexity. So it is necessary to set a balance between estimation accuracy and computational complexity. For example, in this paper for simulation n is equal to 12. On the other hand, a more nonlinear system with higher dimension variables may increase the computational complexity of WMEKF algorithm because a more non-linear system needs bigger n to have better estimation accuracy than a less non-linear system. As a solution, we should set a balance between estimation accuracy and computational complexity in WMEKF by choosing an appropriate number for n and suitable weights. 4.2 Improved Mean Extended Kalman Filter (IMEKF) In this section, an improved version of MEKF named Improved MEKF is presented. In IMEKF, the final estimation of MEKF ( X + k ) is updated by re-linearizing the observation function around X + k and considering following equation as the slope of linear approximation of observation function. ∂h  HkI = (37) X ∂X k Equation 37 introduces a different method for calculating slope of linear approximation of observation function because in this formula Jacobian matrix of

J Intell Robot Syst

observation function is computed at real state vector (Xk ). Since Xk is unknown, so HkI is calculated by is used instead of θk in using Eqs. 23 to 26 and θ + k these equations. Then Kalman gain is computed as T T (HkI Pk+ HkI + Rk )−1 Kk = Pk+ HkI

(38)

+ Finally, X + k and its covariance Pk which are obtained by MEKF are corrected as follows +

X kI

=

+

Xk

+ Kk (Zk − h(

+

Xk

(39)

))

+ PkI = (I − Kk HkI )Pk+

(40)

+ PkI

+ X kI

and are the final state estimation and where final covariance matrix obtained by IMEKF. A more non-linear system with higher dimension variables may also increase the computational complexity of IMEKF. IMEKF is a fixed approach and it is not possible to adjust the estimation accuracy and computational complexity and set a balance between them. However, based on simulation results in Section 5, using IMEKF for solving SLAM problem leads to a desirable estimation accuracy and computational complexity. 4.3 Improved Weighted Mean Extended Kalman Filter (IWMEKF) Prediction step in Improved Weighted Mean EKF (IWMEKF) is similar with EKF and MEKF and it is calculated with Eqs. 5 to 8, but correction step is a combination of correction step in WMEKF and IMEKF. At first, predicted estimate X − k is corrected using correction step of WMEKF which is explained + in Eqs. 27 to 35 and X + k and Pk are obtained. Then observation function is re-linearized around X + k and slope of linear approximation of observation function is described as follows ∂h  HkI = X ∂X k It should be noted that Jacobian matrix of observation function is computed at Xk in this formula. Then Kalman gain is calculated with HkI as

+ where X + kI W and PkI W are the final state estimation and its covariance which are obtained by IWMEKF. IWMEKF is not a fixed methods and it is possible to adjust estimation accuracy and computational complexity similar to WMEKF by using proper value for n and appropriate weights. So IWMEKF has the capacity of setting a balance between estimation accuracy and computational complexity when it is applied to a different problem with more non-linear system and higher dimension variables. This mentioned balance is provided in simulation section by putting n=12 and using suitable weights.

5 Simulation with ‘Car Park Dataset’ Simulation is carried out with ‘Car Park Dataset’ [17] in order to evaluate the performance of all Kalman filters mentioned in this paper for solving SLAM problem. The ‘Car Park Dataset’ which is popular benchmark dataset in SLAM research community was collected by Australian Centre for Field Robotics (ACFR) in the Car Park, Sydney with 15 artificial landmarks and using a vehicle shown in Fig. 5 [17]. The vehicle equipped with different sensors is shown in Fig. 5. A GPS was used to provide ground truth information, vehicle velocity and steering angle were collected with an inertial sensor. A laser range finder was used to measure range and bearing of landmarks

T T Kk = Pk+ HkI (HkI Pk+ HkI + Rk )−1

Finally, X + k and its covariance are updated for the second time as: +

X kI W

=

+

Xk

+ Kk (Zk − h(

+ + PkI W = (I − Kk HkI )Pk

+

Xk

)) Fig. 5 The vehicle used for data collection [12]

J Intell Robot Syst

with respect to the vehicle. In addition, laser data contains intensity information and high intensity signals belong to the artificial landmarks. Thus, those observations with high intensity information are extracted from laser data as eventual landmarks and nearest neighbor method (NN) is used for data association step. The vehicle shown in Fig. 5 is a kind of car-like mobile robot and bicycle model is considered for its motion model [18]. The motion model is described as follows

⎧ ⎨ x˙v = v cos(θ ) y˙v = v sin(θ ) ⎩˙ θv = v/L tan(α)

(41)

The motion model of the vehicle is shown in Fig. 6. Eq. 41 represents the pose of the center of the back axle, but GPS and laser range finder are installed at the front of the vehicle. Thus, to facilitate the update process, the motion model should be corrected to represent the pose of the GPS and laser range finder. After coordinate transformation, discrete model of motion model is described as follows [19].

 ⎤ ⎤ ⎡ xk−1,v + t vk−1 cos(θk−1,v ) − vk−1 xk,v L tan(αk−1 )(a sin(θk−1,v ) + b cos(θk−1,v )) ⎣ yk,v ⎦ = ⎣ yk−1,v + t vk−1 sin(θk−1,v ) + vk−1 tan(αk−1 )(a cos(θk−1,v ) − b sin(θk−1,v )) ⎦ + Wk−1 L θk,v θk−1,v + t vk−1 L tan(αk−1 ) ⎡

vk−1 =

vk−1,e 1−

H L

(43)

tan(αk−1 )

where t represents the sampling time and v is the velocity of the back axle, but ve obtained from inertial sensor represents the velocity of the back left wheel. In addition, the constants L, a, b and H are shown in Fig. 5 and the observation model was described in Eq. 4. Estimation accuracy of mentioned solutions to SLAM is evaluated by means of Mean Absolute Error (MAE) criteria. (xev , yev ) and (xel , yel ) represent the MAE in the estimation of the vehicle and landmark coordinates. These criteria are formulated as follows    nt     1  xev xk,v − xk,M  (44) =  yk,v − yk,M  yev nt

the maximum and average distance between true path (GPS) and estimated path.  nt dv−max = max( (xk,v − xk,M )2 + (yk,v − yk,M )2 ) k=1

(46)

dv−ave

nt  1  (xk,v − xk,M )2 + (yk,v − yk,M )2 = nt k=1

(47) dl−max and dl−ave represent the maximum and average distance between true coordinates and estimated coordinates of the landmarks.  nl dl−max = max( (xi.l − xi,L )2 + (yi,l − yi,L )2 ) (48) i=1

k=1

 Xl error =

xel yel



 nl   xi,l − xi,L  1    = yi,l − yi,L  nl

(45)

i=1

where (xk,v , yk,v ) and (xk,M , yk,M ) are the estimated coordinates and real coordinates of the vehicle. (xi,l , yi,l ) and (xi,L , yi,L ) represent the estimated coordinates and real coordinates of the ith landmark. In addition, nt is the number of sample data collected with GPS and nl represents the number of artificial beacons in the environment. In addition, other criteria are used to evaluate the estimation accuracy. dv−max and dv−ave demonstrate

(42)

Fig. 6 Motion model and sensors [19]

J Intell Robot Syst Table 2 Simulation results for EKF-SLAM, IEKF-SLAM, SEKF-SLAM and MEKF-SLAM Vehicle

Landmarks

MAE Algorithms EKF-SLAM IEKF-SLAM SEKF-SLAM MEKF-SLAM

xev 0.1660 0.1630 0.1635 0.1518

Distance yev 0.1372 0.1369 0.1350 0.1339

dv−max 1.0662 1.0218 0.9839 0.8527

MAE dv−ave 0.2312 0.2284 0.2273 0.2177

xel 0.1867 0.1787 0.1790 0.1821

Distance yel 0.1627 0.1633 0.1631 0.1628

dl−max 0.5323 0.5195 0.5197 0.5049

dl−ave 0.2633 0.2573 0.2572 0.2521

All quantities are described in terms of meter

dl−ave =

nl  1  (xi,l − xi,L )2 + (yi,l − yi,L )2 (49) nl i=1

Firstly, the estimation accuracy of MEKF-SLAM algorithm is compared with EKF-SLAM, IEKFSLAM and SEKF-SLAM and simulation results for these algorithms are shown in Table 2. As shown in Table 2, IEKF-SLAM performs better than EKFSLAM on estimation accuracy, so it can be concluded that iterating the correction step presented in [11] improve the estimation accuracy. In addition, SEKFSLAM which is proposed in [12] is more accurate than EKF-SLAM with a view to simulation results in Table 2 and it can be concluded from Table 2 that MEKF-SLAM algorithm which is presented in [12] is the most accurate method among previous approaches. Thus, replacing the Jacobian matrix of observation function with new formula (used in MEKF) is more effective than iterating the observation-updating step and stepwise increment idea. Secondly, three proposed algorithms are compared with MEKF-SLAM algorithm and corresponding simulation results are shown in Table 3. According to Table 3, it is seen that WMEKF-SLAM estimates the vehicle coordinates and landmark coordinates more

accurate than MEKF-SLAM which is presented in [12]; thus, the formula used to compute Hkw in WMEKF is more effective than corresponding formula (Hk ) used in MEKF. In addition, simulation results in Table 3 indicate that IMEKF-SLAM has much better performance than MEKF-SLAM in estimation accuracy point of view, so it can be concluded that re-linearization  of observation function and considering ∂h/∂X Xk as the slope of linear approximation of observation function in IMEKF algorithm lessens the linearization error. Also it can be deduced from Table 3 that IMEKF-SLAM performs much better than WMEKF-SLAM on estimation accuracy and linearization error and simulation results validate the better estimation accuracy of IWMEKF-SLAM than MEKF-SLAM, WMEKF-SLAM and IMEKF-SLAM. Therefore, all three proposed approaches have lower linearization error than MEKF-SLAM, SEKF-SLAM, IEKF-SLAM and EKF-SLAM. Besides, IWMEKF is the best filter of Kalman filters family among all mentioned methods in this paper. Besides, IMEKF-SLAM and WMEKF-SLAM follow IWMEKF in-order with a view to simulation results. Moreover, in order to compare the computational complexity of all mentioned SLAM algorithms, the running time for each

Table 3 Simulation results for MEKF-SLAM, WMEKF-SLAM, IMEKF-SLAM and IWMEKF-SLAM Vehicle

Landmarks

MAE Algorithms MEKF-SLAM WMEKF-SLAM IMEKF-SLAM IWMEKF-SLAM

xev 0.1518 0.1511 0.1410 0.1403

Distance yev 0.1339 0.1338 0.1336 0.1335

dv−max 0.8527 0.8526 0.7839 0.7911

MAE dv−ave 0.2177 0.2171 0.2112 0.2106

xel 0.1821 0.1808 0.1729 0.1717

Distance yel 0.1628 0.1628 0.1624 0.1624

dl−max 0.5049 0.5045 0.5309 0.5297

dl−ave 0.2521 0.2512 0.2508 0.2500

J Intell Robot Syst

SLAM algorithm is recorded in Table 4. It should be noted that the running time of each algorithm is not constant and it may vary randomly, so the average running time over 20 simulations of each SLAM approach is recorded in Table 4. It can be concluded from Table 4 that EKF-SLAM is the best filter of all mentioned SLAM approaches in computational complexity point of view and MEKFSLAM comes to the second place because the computation method of Hk for MEKF is a little more complex than ∇hk in EKF. Also, the formula of Hkw in WMEKF is a little more complex than that of MEKF; thus, the running time of WMEKFSLAM is a little more than that of MEKF-SLAM. Besides, IMEKF needs to update predicted state and its covariance two times in one correction process, but IEKF and SEKF update the predicted state − (X− k ) and its covariance (Pk ) N (iteration number) times in one correction process, so IMEKF-SLAM, IEKF-SLAM and SEKF-SLAM follow WMEKFSLAM in-order with a view to their running time as indicated in Table 4. Finally, the running time of IWMEKF-SLAM is just a little longer than that of SEKF-SLAM. Besides, the performance of WMEKF and IWMEKF depend on the number of hypothetical Jacobians (n) and increasing the value of n will make these approaches more accurate and more complex. In addition, when WMEKF and IWMEKF are used to solve a different problem with more non-linear system and higher dimension variables complexity may be increased. Thus, a suitable value for n should be selected to make a balance between computational complexity and estimation accuracy. In this simulation, it is done by putting n is equal to 12. In brief, choosing one of our proposed methods depend on the goal and kind of application which the proposed methods are used. IWMEKF has more complexity and more estimation accuracy than other proposed approaches and WMEKF has less complexity and less estimation accuracy than other proposed

Fig. 7 path and map estimated with EKF-SLAM in comparison with real path obtained from GPS and real position of landmarks (solid line: real path, dashed line: estimated path, the stars and pluses: estimated position and real position of artificial beacons)

approaches. Besides, IMEKF has a desired estimation accuracy and computational complexity. Thus, if estimation accuracy is the most important factor, IWMEKF is the most suitable approach and if computational complexity is the main factor, WMEKF is the most appropriate solution. In addition, if both estimation accuracy and computational complexity are important, IMEKF can be a good solution. However, the best estimation accuracy belongs to IWMEKF and it is possible to adjust the estimation accuracy and computational complexity in IWMEKF. In spite of this, the running time of all mentioned SLAM approaches in this paper is very close to each other and all running times are small in comparison with the data collecting time (112 s), so it can be concluded that IWMEKF is the most appropriate solutions to the SLAM problems among all Kalman filters mentioned in this paper in linearization error, estimation accuracy and computational complexity. In Figs. 7, 8 and 9, path and map estimated with EKF-SLAM, MEKF-SLAM and IWMEKF-SLAM are compared with the real path obtained by GPS

Table 4 Running time Algorithms

EKFSLAM

IEKFSLAM

SEKFSLAM

MEKFSLAM

WMEKFSLAM

IMEKFSLAM

IWMEKFSLAM

Running time(s)

1.6442

1.7626

1.7714

1.6584

1.7220

1.7456

1.7822

J Intell Robot Syst

concluded from Fig. 8 that the performance of MEKFSLAM is more accurate than EKF-SLAM. On the other hand, Fig. 9 indicates that the performance of IWMEKF-SLAM is more stable than MEKF-SLAM and EKF-SLAM.

6 Conclusions

Fig. 8 path and map estimated with MEKF-SLAM in comparison with real path obtained from GPS and real position of landmarks (solid line: real path, dashed line: estimated path, the stars and pluses: estimated position and real position of artificial beacons)

and real position of artificial beacons respectively. In Figs. 7, 8 and 9, solid line and dashed line represent the real path collected with GPS and estimated path. For all figures, the pluses and stars are the real position and estimated position of the artificial beacons. From Fig. 7, it can be seen that some parts of the estimated path is far away from the real path and estimated position of artificial beacons is not matched with real position of artificial beacons, but it can be

Fig. 9 path and map estimated with IWMEKF-SLAM in comparison with real path obtained from GPS and real position of landmarks (solid line: real path, dashed line: estimated path, the stars and pluses: estimated position and real position of artificial beacons)

In this paper, reducing the linearization error of Kalman filters family is investigated and it is concluded that correcting the formula used to compute the slope of the linear approximation of observation function can lessen the linearization error. Three new methods named WMEKF, IMEKF and IWMEKF are proposed to decrease the linearization error by revising the formula used to calculate Jacobian matrix of observation function in EKF. Simulation results done with ‘Car Park Dataset’ validate the effectiveness and reliability of our proposed methods for solving SLAM problem. They indicate that all our proposed methods have better estimation accuracy than other mentioned methods in this paper and the best estimation accuracy belongs to IWMEKF-SLAM algorithm. IMEKF-SLAM and WMEKF-SLAM come to the second and third place in-order with a view to simulation results. On the other hand, when computational complexity is discussed, EKF-SLAM and MEKF-SLAM comes to the first place in-order and WMEKF-SLAM and IMEKFSLAM follow them in-order. IEKF-SLAM, SEKFSLAM and IWMEKF-SLAM have just a little more computational complexity than IMEKF-SLAM. However, since the running time of all mentioned SLAM approaches in this paper is very close to each other and all running times are small in comparison with the data collecting time (112 s), it can be deduced that our proposed approaches performs better than other mentioned approaches on linearization error, estimation accuracy and computational complexity and IWMEKF is the most suitable solution for SLAM problem. On the other hand, our proposed methods have its limitation. First of all, computational complexity of WMEKF and IWMEKF increases with using more hypothetical Jacobians (n). Secondly, applying these two methods to a more non-linear system with higher dimension variables may increase the complexity. Thus, it is important to make a balance between

J Intell Robot Syst

estimation accuracy and computational complexity. In addition, reducing the dependence of Kalman Filters Family on the characteristic of system such as nonlinearity and dimension variables can be an important research topic in future.

9.

10.

References 1. Lee, D., Kim, D., Lee, S., Myung, H., Choi, H.-T.: Experiments on localization of an AUV using graph-based SLAM. In: 2013 10th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), pp. 526–527 (2013) 2. Lourenco, P., Guerreiro, B.J., Batista, P., Oliveira, P., Silvestre, C.: Preliminary results on globally asymptotically stable simultaneous localization and mapping in 3-D. In: American Control Conference (ACC), 2013, pp. 3087– 3092 (2013) 3. Pailhas, Y., Capus, C., Brown, K., Petillot, Y.: Design of artificial landmarks for underwater simultaneous localisation and mapping. In: IET Radar, Sonar & Navigation, vol. 7, pp. 10–18 (2013) 4. Yoon, S., Hyung, S., Lee, M., Roh, K.S., Ahn, S., Gee, A., Bunnun, P., Calway, A., Mayol-Cuevas, W.W.: Realtime 3D simultaneous localization and map-building for a dynamic walking humanoid robot. Adv. Robot. 1–14 (2013) 5. Roy, D.: Simultaneous Localization and Mapping for an Autonomous Vehicle using Extended Kalman Filter. Indian Institute of Technology (2004) 6. Thrun, S., Fox, D., Burgard, W.: A probabilistic approach to concurrent mapping and localization for mobile robots. Mach. Learn. 31(1-3), 29–53 (1998) 7. Abrate, F., Bona, B., Indri, M.: Experimental EKF-based SLAM for mini-rovers with IR sensors only. In: EMCR (2007) 8. Weingarten, J., Siegwart, R.: EKF-based 3D SLAM for structured environment reconstruction. In: 2005 IEEE/RSJ

11.

12.

13.

14.

15.

16.

17. 18. 19.

International Conference on Intelligent Robots and Systems, 2005 (IROS 2005), pp. 3834–3839 (2005) Ceriani, S., Marzorati, D., Matteucci, M., Sorrenti, D.G.: Single and multi camera simultaneous localization and mapping using the extended Kalman Filter. Journal of Mathematical Modelling and Algorithms in Operations Research, 1–35 (2013) Wang, H., Wang, J., Qu, L., Liu, Z.: Simultaneous localization and mapping based on multilevel-EKF. In: 2011 International Conference on Mechatronics and Automation (ICMA), pp. 2254–2258 (2011) Shojaie, K., Ahmadi, K., Shahri, A.M.: Effects of iteration in Kalman filters family for improvement of estimation accuracy in simultaneous localization and mapping. In: 2007 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, pp. 1–6 (2007) Zhou, W., Zhao, C., Guo, J.: The study of improving Kalman filters family for nonlinear SLAM. J. Intell. & Robot. Syst. 56, 543–564 (2009) Chen, Z., Dai, X., Jiang, L., Yang, C., Cai, B.: Adaptive iterated square-root cubature Kalman filter and its application to SLAM of a mobile robot. TELKOMNIKA Indones. J. Electr. Eng. 11, 7213–7221 (2013) Martinez-Cantin, R., Castellanos, J.A.: Unscented SLAM for large-scale outdoor environments. In: 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005 (IROS 2005), pp. 3427–3432 (2005) Montemerlo, M.: A Factored Solution to the Simultaneous Localization and Mapping Problem with unknown Data Association. Ph.D. thesis school of computer science Carnegie Mellon University, Pittsburgh, USA (2003) Song, Y., Li, Q., Kang, Y.: CFastSLAM: a new Jacobian free solution to SLAM problem. In: 2012 IEEE International Conference on Robotics and Automation (ICRA), pp. 3063–3068 (2012) http://www.cas.kth.se/SLAM/ (2002) Corke, P.: Robotics, Vision and Control: Fundamental Algorithms in MATLAB, vol. 73. Springer (2011) Guivant, J., Nebot, E., Baiker, S.: Autonomous navigation and map building using laser range sensors in outdoor applications. J. Robot. Syst. 17, 565–583 (2000)