IFAC Symposium on Intelligent Autonomous Vehicles (IAV), 2004, Lisboa (Portugal)This work presents an analysis of the state estimation error dynamics for a linear system within the Kalman filter based approach to Simultaneous Localization and Map Building. Our objective is to demonstrate that such dynamics is marginally stable. The paper also presents the necessary modifications required in the observation model, in order to guarantee zero mean stable error dynamics. Simulations for a one-dimensional robot and a planar vehicle are presented.This work was supported by the project 'Supervised learning of industrial scenes by means of an active vision equipped mobile robot.' (J-00063).Peer Reviewe

Andrade-Cetto, Juan

Sanfeliu, Alberto

Vidal-Calleja, Teresa A.

Digital.CSIC

ESTIMATOR STABILITY ANALYSIS IN SLAMTeresa Vidal-Calleja, Juan Andrade-Cetto,Alberto SanfeliuInstitut de Robtica i Informtica Industrial, UPC-CSICLlorens Artigas 4-6, Barcelona, 08028 Spain{tvidal, cetto, sanfeliu}@iri.upc.esAbstract: This work presents an analysis of the state estimation error dynamicsfor a linear system within the Kalman ﬁlter based approach to SimultaneousLocalization and Map Building. Our objective is to demonstrate that suchdynamics is marginally stable. The paper also presents the necessary modiﬁcationsrequired in the observation model, in order to guarantee zero mean stable errordynamics. Simulations for a one-dimensional robot and a planar vehicle arepresented.Keywords: SLAM, Kalman Filter, Observability1. INTRODUCTIONThe study of stochastic models for SimultaneousLocalization and Map Building (SLAM) in mobilerobotics has been an active research topic forover ﬁfteen years. Within the Kalman ﬁlter (KF)approach to SLAM, seminal work (Smith andCheeseman, 1986) suggested that as successivelandmark observations take place, the correlationbetween the estimates of the location of such land-marks in a map grows continuously. This observa-tion was later ratiﬁed (Dissanayake et al., 2001)with a proof showing that the estimated map con-verges monotonically to a relative map with zerouncertainty. They also showed how the absoluteaccuracy of the map reaches a lower bound deﬁnedonly by the initial vehicle uncertainty.In a dynamic system, such as the one typicallyused in SLAM, where both the state and mea-surement dynamics are corrupted by noise, it isimportant to know whether it is possible or notto reconstruct the entire state space from outputThis work was supported by the Spanish Council ofScience and Technology under project DPI 2001-2223.measurements. In this paper, we present an anal-ysis of the state estimator that shows how, withthe typical measurement model, it is not possibleto obtain zero mean error state estimation.Zero mean state estimation does not pose a prob-lem in SLAM if a perfectly accurate relative mapis sought, such as the one in (Newman, 1999).However, when building an absolute map, andat the same time estimating the absolute vehiclelocation purely from sensor measurements andodometry, the results of the Kalman ﬁlter-basedapproach to SLAM will be subject to the errorproduced at the very ﬁrst iteration.Unfortunately, the state space constructed by ap-pending the robot pose and the landmark loca-tions is partially observable (Andrade-Cetto andSanfeliu, 2004), and the consequence is precisely,that the absolute map accuracy is bounded by theinitial vehicle uncertainty. In this paper, we de-velop the equations for the simplest case in SLAM,a one-dimensional vehicle with one landmark, inorder to show the behavior of the observer. Weare able to see in the end, that the entire closedloop system, including the estimator, has a polein one, making the ﬁlter marginally stable. Oneway to make sure that the entire system is fullyobservable, is by slightly modifying the observa-tion model, either anchoring the state estimate toone landmark, or by adding an external sensor.The paper is structured as follows. The systemand the estimation procedures are presented inSection 2. In Section 3 we show the marginallystable estimation error dynamics for the one-landmark monobot vehicle, and the way to im-prove the behavior of the ﬁlter. In the same sectionsimulations results are presented. Section 4 showsresults for a planar vehicle model. Conclusions arepresented in the last section.2. STATE ESTIMATION IN SLAM2.1 The Kalman FilterConsidering the problem of SLAM as stochasticestimation of a discrete-time linear system, andexpressed as a vector diﬀerence equation withadditive white Gaussian noise that models unpre-dictable disturbances; the dynamic plant equationis simplyxk+1 = Fxk +Guk + vk (1)where xk is the augmented state vector formedby appending the vehicle state estimate and thelandmark location estimates, uk is a known inputvector, and vk, is the k-th term of a sequence ofzero-mean Gaussian process noise with covarianceVk = E[vkvk].The measurement equation iszk = Hxk +wk (2)with wk the k-th term of a sequence of zero-mean white Gaussian measurement noise withcovariance Wk = E[wkwk].Using the aforementioned linear system, the algo-rithm for the optimal state estimator (the Kalmanﬁlter) is as follows:First, compute the a priori state predictionxk+1|k = Fxk|k +Guk (3)Followed by an a priori measurement predictionzk+1|k = Hxk+1|k (4)Next, compute the a posteriori state estimate,known also as the update of the state estimatexk+1|k+1 = xk+1|k +K(zk+1 − zk+1|k)(5)where K is the Kalman gain for optimal estima-tion in the mean square error sense.Finally, replacing the system from equations 3 and4 in equation 5, the closed-loop system becomesxk+1|k+1 = (F−KHF)xk|k +(G−KHG)uk +Kzk+1 (6)In order to compute the optimal ﬁlter gain for thelinear system one needs:The state prediction covariancePk+1|k = FPk|kF +Vk, (7)the innovation covarianceSk+1 =Wk+1 +HPk+1|kH (8)and ﬁnally, the ﬁlter gainK = Pk+1|kHk+1S−1k+1 (9)The update of the state covariance is computedwithPk+1|k+1 = Pk+1|k −KSk+1K (10)For constant plant and sensor covariances, thesteady state value for the covariance matrix isgiven by the solution of the Riccati equationP = F(P−PH(HPH +W)−1HP)F +V(11)Such solution to the Riccati equation will con-verge, in the linear case, to a positive semi deﬁnitesteady state covariance in terms of Pr,0|0, V, W,and the total number of landmarks n (Gibbens etal., 2000). Notice however, that for the nonlinearcase, the computation of the Jacobians F and Hwill in general also depend on the steady statevalue of x.Now, consider the case of suboptimal ﬁltering, inwhich covariance inﬂation is used for decorrelation(Julier, 2003). In that case, partial observability ofthe pair {F,H} might produce in some cases anunbounded value for P, unless, full observabilityis guaranteed (Bar-Shalom et al., 2001).For the optimal ﬁlter, under detectability, thereis at least one positive semi deﬁnite solution ofequation 11 such that the ﬁlter is marginallystable (Kailath et al., 2000). We show next how inthe linear case of SLAM, any solution of equation11 produces a marginally stable solution for theestimation error.3. STATE ESTIMATE ERROR DYNAMICSDeﬁning the estimation error x˜k asx˜k = xk − xk|k (12)Then, with the appropriate substitutions we ob-tainx˜k+1 = (F−KHF)x˜k + (I−KH)vk −Kwk+1(13)xr,k|k xr,k+1|k xr,k+1ur,k vr,kxlFig. 1. Monobot, one-dimensional mobile robot.Unfortunately, the state vector formed by append-ing the vehicle position estimate with the land-mark location estimates is not fully observable,and only partially controllable (one-controllable inthe case of the monobot); making one eigenvalueof the matrix (F−KHF) lay on the unitary circle.That means the matrix (F−KHF) is marginallystable as a consequence of the partial observabil-ity. The pole equal to one corresponds to a stateestimate signal corrupted with constant ampli-tude.3.1 One Landmark MonobotConsider the one-dimensional robot (monobot)from Figure 1. The robot location is xr,k and themotion command is uk. The robot error dynamicsis modeled with the additive term vk, and theentire system is simplyxr,k+1 = xr,k + uk + vk (14)xl,k+1 = xl,k (15)The map for this simple model is just the staticlandmark xl. The observation model for suchlandmark iszk+1 = xl,k+1 − xr,k+1 + wk (16)with wk the landmark observation error.For any value of the Kalman gain in this simplestSLAM conﬁguration, K = [k1, k2], the eigenval-ues of the matrixF−KHF =[k1 + 1 −k1k2 −k2 + 1](17)are {1, k1 − k2 + 1}. Appendix A shows the actualform of the eigenvalues of (F−KHF) as a functionof the variances V and W, in the very ﬁrstiteration of the algorithm. The result is thatregardless of the value of these variances, the errordynamics will be marginally stable, and the steadystate error will tend to a constant value dependanton the initial conditions, instead of an error withzero mean.3.2 Full observability in SLAMTwo diﬀerent forms to make the system observ-able are presented in (Andrade-Cetto and San-feliu, 2004). That is, two diﬀerent ways to make(F−KHF) asymptotically stable. One is by ﬁxingFig. 2. One landmark monobot characteristicpolynomial for the ﬁrst iteration of (F −KHF).a global reference at the origin. In that case, themeasurement model for the one landmark linearsystem (monobot) with anchor becomes[z(0)kzk]=[−1 0−1 1]x+[w(0)kwk](18)The anchor is taken as a global reference at theworld origin. No map state is needed for it. Thezero-th superscript in the measurement vector isused for the consistent indexing of landmarks andobservations with respect to the original model. Itcan be easily shown that the observability matrixfor this augmented measurement model is fullrank.The stability of the matrix (F−KHF) is subjectto the values on the entries in K =[k11 k12k21 k22].Appendix B shows explicitly these terms for theﬁrst iteration of a one landmark monobot; andpresents also an expression for the computationof the characteristic polynomial of (F − KHF),assuming equal anchor and landmark variances.Figure 2 plots for example, such characteristicpolynomial for initial variances Pr,0|0 = V =W = 1. The roots in the plot indicate thevalue for the eigenvalues of (F − KHF), λ1 =0.1491, λ2 = 0.4663; and as we can see, for thefully observable SLAM case, these lay within theunitary circle, thus guaranteeing ﬁlter stability,and consequently, zero mean state estimation.In general terms, given that in the fully observablecase the observability matrix has full rank, forany values of V,W, and Pr,0|0, the eigenvaluesof (F−KHF) will always be inside the unit circleof the complex z plane.3.3 SimulationsNext, we show the eﬀects of marginal stabilityon a vehicle under Brownian motion; and how0 20 40 60 80 100−3−2−1012Iterationx k (m)0 20 40 60 80 100−0.2−0.100.10.20.3Iterationx r,k−x r,k|k (m)a) b)0 20 40 60 80 100−0.500.5IterationKalman GainKrKf0 20 40 60 80 100−0.4−0.3−0.2−0.100.10.20.30.4Iterationx f,k−x f,k|k (m)c) d)Fig. 3. KF SLAM for a one landmark monobotunder Brownian motion.0 20 40 60 80 100−3−2−1012Iterationx k (mm)0 20 40 60 80 100−0.2−0.100.10.20.3Iterationx r,k−x r,k|k (m)a) b)0 20 40 60 80 100−0.500.5IterationKalman GainKrKf0 20 40 60 80 100−0.4−0.3−0.2−0.100.10.20.30.4Iterationx f,k−x f,k|k (m)c) d)Fig. 4. KF SLAM for a twenty landmarksmonobot under Brownian motion.these eﬀects are eliminated once full observabilityis guaranteed. Figures 3 and 4 plot the resultsof using the original Kalman ﬁlter approach toSLAM for a monobot that starts at location xr,0 =−1m, and is governed by Brownian motion only.All landmarks are located at x(i)l = 1m. Theplots correspond as follows: a) full state estimate,b) vehicle estimation error with 2σ uncertaintybounds, c) vehicle and landmark Kalman gains,and d) landmark estimation error, also with 2σuncertainty bounds.The vehicle and landmark estimation errors donot converge to a zero mean signal (plots b andd). However, thanks to marginal ﬁlter stability,the ﬁlter does converge to a constant value. Thisvalue is less sensitive to the initial conditions whena large number of landmarks is used. Figures 5 and6, in which partial observability has been revised,show zero mean steady state estimation error, and0 20 40 60 80 100−3−2−1012Iterationx k (m)0 20 40 60 80 100−0.2−0.100.10.20.3Iterationx r,k−x r,k|k (m)a) b)0 20 40 60 80 100−0.500.5IterationKalman GainKrKf0 20 40 60 80 100−0.4−0.3−0.2−0.100.10.20.30.4Iterationx f,k−x f,k|k (m)c) d)Fig. 5. KF SLAM for a one landmark plus anchormonobot under Brownian motion.0 20 40 60 80 100−3−2−1012Iterationx k (mm)0 20 40 60 80 100−0.2−0.100.10.20.3Iterationx r,k−x r,k|k (m)a) b)0 20 40 60 80 100−0.500.5IterationKalman GainKrKf0 20 40 60 80 100−0.4−0.3−0.2−0.100.10.20.30.4Iterationx f,k−x f,k|k (m)c) d)Fig. 6. KF SLAM for a twenty landmarks plusanchor monobot under Brownian motion.smaller vehicle and landmark variances than thosein Figures 3 and 4.4. PLANAR VEHICLEThe dynamics of a more realistic mobile robot andsensor are governed by the following discrete-timenonlinear state transition model,xk+1 = f (xk,uk,vk) (19)zk = h (xk,wk) (20)The state vector xk contains usually the pose (andsometimes the velocity) of the robot xr,k at timestep k, and a vector of stationary landmarks x(i)l ,i.e.,xk =[xr,kx(i)l](21)where, in the case of our planar robot xr,k =[xk yk θk] are the horizontal and vertical ve-hicle position and orientation, respectively. Thevector uk represents the velocity commands, andvk and wk are unmodeled motion and measure-ment noises, respectively.The Extended Kalman Filter (EFK) is used in thisnon-linear case; and the linearization of f and his evaluated at the a priori predictions xk+1|k andzk+1|k as a Taylor series with the higher orderterms dropped.(Andrade-Cetto and Sanfeliu, 2004) show thatwhen using such linearized model for a planarvehicle, the resulting Jacobians still produce apartially observable SLAM, and proposes a wayto make the system fully observable, similar tothe linear case. For example, with the aid of ameasurement model for a ﬁxed global referenceﬁxed at the originh(0) = −Rt+w(0) (22)with w(0) the anchor observation error, andR =[cos θ − sin θsin θ cos θ], t = [xk, yk] (23)Visibility of a two dimensional anchor to aid asglobal reference guarantees fuul observability inthis particular planar vehicle case.4.1 ExperimentsFigure 7 shows a run of the EKF SLAM algorithmover a 300m2 area in the second ﬂoor of theUSC SAL building with data acquired with alaser range ﬁnder 1 . The eﬀects of nonlinearities,together with the marginal stability of the ﬁlterproduce large localization errors at the end of anearly 90m loop.The map obtained with the fully observableSLAM algorithm for the same data set is shownin Figure 8. In this run, the ﬁrst observed land-mark is used as an anchor. Furthermore, and toguarantee full observability during the entire runof the algorithm, every landmark revised morethan 50 times by the ﬁlter was removed from thestate vector, but its observations were still usedto revise the remaining state vector elements.5. CONCLUSIONSPartial observability in SLAM produces a marginallystable ﬁlter, resulting in a unit norm eigenvalue forthe error dynamics matrix (F − KHF), regard-less of the plant and sensor variance parameters1 Data from the Robotics Data Set Repository (Howardand Roy, 2003). Thanks to Andrew Howards.Fig. 7. EKF SLAM for a planar vehicle.Fig. 8. EKF fully observable SLAM for a planarvehicle.chosen. Marginal stability has the consequence ofnon-zero steady state vehicle and landmark esti-mation error. Even when a perfect relative mapis computed, the absolute vehicle and landmarkestimates will have non-zero steady state error;unless full observability is guaranteed.6. APPENDICESA. Eigenvalues of (F−KHF) on the first iterationfor a partially observable one landmark monobot.According to equations 1 and 2, the matrices ofour reduced system areF =[1 00 1]; G =[10]; H =[−1 1 ]The KF gain for the one landmark monobot iscomputed as follows. First, the initial covarianceis evaluated (Newman, n.d.)P0 =[[1 −10 1] [Pr 00 W0]−1 [ 1 −10 1]]−1=[Pr PrPr W0 + Pr]where Pr = Pr,0|0 is the initial robot variance,and W0 =W0 is the initial sensor error variance.We obtain the initial a priori estimate for thecovariance matrix from equation 7,P1|0 =[V0 + Pr PrPr W0 + Pr]with V0 the initial variance for the plant error.The innovation term becomesS1 = V0 + W0 + W1Assuming the plant and measurement variancesare constant, the last expression reduces toS1 = V + 2WThe KF gain for the ﬁrst iteration in our systemis thus,K1 =1V + 2W[−VW]Substituting the above result in equation 17, theeigenvalues of the ﬁlter dynamics matrix at theﬁrst iteration for a one landmark monobot are{1,W/(V + 2W )}. A pole in one means marginalstability of (F− FKH).B. Eigenvalues of (F−KHF) on the first iterationfor a fully observable one landmark monobot.Substituting for the fully observable measurementmodel we have that, for the ﬁrst ﬁlter iterationk11 = (−2PrV W + W (0)(2W (V + Pr) + PrV ))/γk12 =−V W (0)(Pr + W (0))/γk21 = (−PrV W −W (0)(V W + 2PrW + PrV ))/γk22 = (PrV W +W (0)(V W + 2PrW + Prv + WW (0)))/γwhereγ = 2V WPr + W (0)(2V W + 2V Pr +4WPr + V W (0) + 2WW (0))The characteristic polynomial of (F − KHF),assuming equal anchor and landmark variances isf(x) = x2 − W (2V Pr + W (3W + 2V + 4Pr))γx+W 3(W + Pr)(4V Pr + 2W 2 + 3V W + 4WPr)γThe eigenvalues of the matrix (F − KHF), theroots of f(x), are1γ(W (V Pr +32W 2 + V W + 2WPr)± 12√κ)with κ = W 2(W 4+8PrWV 2+4V 2W 2+4V 2P 2r ).REFERENCESAndrade-Cetto, J. and A. Sanfeliu (2004). Theeﬀects of partial observability in SLAM. To bepresented at IEEE International Conferenceon Robotics and Automation.Bar-Shalom, Y., X. Rong Li and T. Kirubara-jan (2001). Estimation with Applications toTracking and Navigation. John Wiley & Sons.New York.Dissanayake, M. W. M. G., P. Newman, S. Clark,H. F. Durrant-Whyte and M. Csorba (2001).A solution to the simultaneous localizationand map building (SLAM) problem. IEEETrans. Robot. Automat. 17(3), 229–241.Gibbens, P. W., G. M. W. M. Dissanayake andH. F. Durrant-Whyte (2000). A closed formsolution to the single degree of freedom si-multaneous localisation and map building(SLAM) problem. In: Proc. IEEE Int. Conf.Decision Control. Sydney. pp. 408–415.Howard, A. and N. Roy (2003). The robotics dataset repository (Radish).Julier, S. J. (2003). The stability of covari-ance inﬂation methods for SLAM. In: Proc.IEEE/RSJ Int. Conf. Intell. Robots Syst.. LasVegas. pp. 2749–2754.Kailath, T., A. H. Sayed and B. Hassibi (2000).Linear Estimation. Information and SystemSciences Series. Prentice Hall. Upper SaddleRiver.Newman, P. M. (1999). On the Structure and So-lution of the Simultaneous Localisation andMap Building Problem. PhD thesis. The Uni-versity of Sydney. Sydney.Newman, P. M. (n.d.). A short note on feature ini-tialization and relocation. Technical report.Smith, R. C. and P. Cheeseman (1986). On therepresentation and estimation of spatial un-certainty. Int. J. Robot. Res. 5(4), 56–68.

Estimator stability analysis in SLAM

Abstract

Similar works

Full text

Available Versions

Digital.CSIC