Combined extended FIR/Kalman filtering for indoor robot localization via triangulation

A combined unbiased finite impulse response (UFIR) and Kalman filtering algorithm is proposed for mobile robot localization via triangulation utilizing noisy measurements. We consider a mobile robot travelling on an indoor floorspace with three nodes in a view. Under the not well-known initial robot state and noise statistics, the extended Kalman filter (EKF) may produce unacceptable estimates. The iterative extended UFIR (EFIR) filter ignores the noise statistics, but requires N initial points of linear measurements which are unavailable. The combined EFIR/Kalman algorithm utilizes N first EKF estimates with approximately set initial conditions and noise statistics as linear measurements for EFIR filter. It is shown that the combined algorithm is more accurate than EKF in robot localization under the real operation conditions. Simulations are provided for piecewise and circular robot trajectories.


Introduction
Mobile robot applications require automatic localization or self-localization often in a way fast, accurate and low cost.Although the problem has been solved during decades by various methods [1,2], the traditional triangulation is still used in many cases.If a robot has a direction indicator such as a compass, then at least two nodes (beacons or landmarks) with known coordinates are required in order to localize a robot and determine its pose (heading) via triangulation.Otherwise, at least three nodes are required.
For the three-node triangulation, two methods are available.The first method implies that all stationary nodes are active (beacons) and the robot has a rotating receiver [3,4].The approach has been adopted from ships and airplanes navigation where it is most common.Since the beacons can be mounted and installed very accurately, the method guarantees accurate positioning information with minimal processing [1], provided that the beacons are detected reliably in a robot view.Because the environment can be particularly structured for applications [5], this method can be implemented in different ways.Beacons (active landmarks) based systems can be organized using sonar [4], optical beams [6], radio frequency propagation [7] and interferometry [8], and cameras [9] sensitive to infrared emission [10].
The second method implies using passive landmarks or reflectors and rotating transmitter-receiver.A typical implementation of this method use maintained on robot a laser scanner that revolves in a horizontal plane and three or more stationary retroreflectors mounted at known locations in the environment [1].Infrared emission is also used [11].In recent years, has gained currency a scheme in which cameras placed on robot board trace several passive landmarks [12] available on a known or unknown map.One more approach implies measuring distances between a robot and several passive or active radio frequency identification (RFID) tags [13].
In the three-node triangulation system, three angles are often measured between the robot heading and the directions to the nodes.These angles are coupled with the robot plane coordinates and heading by nonlinear equations.The equations can be solved for unknown robot coordinates and heading.However, accuracy is commonly insufficient in noisy environments and optimal estimators are required.The estimation theory offers several useful methods to solve the triangulation problem.One of the most common approaches is the extended Kalman filter (EKF) proposed by Cox [14] and others.It suggests expanding the nonlinear functions to the first-order Taylor series and then using linear Kalman filtering.The EKF was used in tracking and robotics extensively [1][2][3]15,16] and has become a tool for moving vehicular navigation, tracking, localization, and self-localization [17,18].The benefit of the EKF resides in the following facts: It solves the nonlinear equations while providing optimal denoising.
Its fast recursive algorithm requires small memory and is thus low-cost.
It practically does not demonstrate divergence under the normal conditions of triangulation.
However, EKF has also several widely recognized flaws: Biased estimates, because noise is nonadditive in the robot dynamics formulation.Divergence under the conditions of large nonlinearities and large noise [19] that is typical for boundaries of the floorspace area.
High sensitivity to noise statistics; that is, the performance of EKF may be poor if the noise covariance matrices and initial errors are not well known [20].Large errors under the industrial uncertainties and when noise is nonwhite Gaussian (heavy-tailed or Gaussian with outliers) [20].
Referring to these drawbacks, several alternative approaches have been developed in recent decades.The deterministic technique called the unscented transform was used in [21] to transfer the mean and variance through nonlinearities.A relevant filter called the unscented Kalman filter (UKF) has demonstrated better performance than EKF when the state-space models is highly nonlinear.The unscented transform is also applicable to high order statistics.For continuous-time state-space models decomposed into ''cells,'' a grid-based method can be used to approximate the posterior probability density function (pdf) of the process.The approach has resulted in the hidden Markov model (HMM) filters [22] useful for tracking [23].A sequential Monte Carlo (SMC) method also known as a particle filter (PF) [24] was developed to estimate Bayesian models associated with Markov chains in discrete-time domain.Note that SMC is akin to HMM that was developed for continuous models.Because PFs are based on point mass (''particle'') representations of the process pdf, they were successfully applied to nonlinear state-space models and were shown to be especially useful for robot self-localization [25].Another alternative to the EKF that has an infinite impulse response (IIR) is the extended finite impulse response (EFIR) filter recently proposed in [26].Unlike the EKF, UKF, and optimal FIR (OFIR) filters [27,28], the EFIR filter totally ignores the noise statistics and initial error statistics.Similarly to PFs, the EFIR filter exploits most recent past measurements which number is required to be optimal N opt [29].Note that scalar N opt can be ascertained by using test reference measurements, so in a way easier than that to determine the noise statistics.
Although the above listed advanced methods have improved the Kalman estimate and generated interest of designers, some flaws still remain.The UKF relies on noise statistics such as the mean and variance which are not always known to the engineer [20] especially when a system is time-varying.On the other hand, the PFs, SMC method, and EFIR filters often need big data in order to outperform EKF.
In this paper, we propose a combined EFIR/Kalman algorithm that implies using N first EKF estimates as linear measurements for the EFIR filter under the real operation conditions when the initial robot state and noise statistics are not well-known.The rest of the paper is organized as follows.Section 2 discusses the triangulation problem in state space.Section 3.2 presents the extended Kalman and FIR filtering algorithms.Examples of robot localization using the EKF and EFIR/Kalman algorithms are given in Section 4. Finally concluding remarks can be found in Section 5.

Triangulation and filtering problem
Various triangulation methods can be applied to indoor robot localization.Giving no preference to any of those methods, we suppose that three nodes A, B, and C (beacons or passive marks) are mounted in an indoor space as shown in Fig. 1.A robot travels in the direction d with all three nodes in a view.A detailed schematic two-dimensional geometry of a moving robot is given in Fig. 2. It is supposed that a node B(0, 0) is placed in the corner that is a center of the indoor space planar Cartesian coordinates and two other nodes have coordinates A(0, y 1 ) and C(x 3 , 0).A robot travels in its own planar Cartesian coordinates ðx r ; y r Þ with a center at M(x, y); that is, the robot direction always coincides with axis x r .We suppose that all nodes are observable by a robot or otherwise the nodes can observe a robot.The angles u 1 ; u 2 , and u 3 between axis x r and the directions to the relevant nodes are supposed to be measurable by commercially available means.The robot behavior is controlled by left and right wheels installed at a distance b and the stabilizing wheel is not shown.The incremental distances robot travels by these wheels are d L and d R , respectively.
The incremental distance d n at the mid-axis point and the incremental change u n in pose can be found at discrete time index n from the robot odometry as In turn, the unknown coordinates x n and y n and pose U n can be obtained by the robot kinematics with equations in which the values x nÀ1 ; y nÀ1 , and U nÀ1 at time n À 1 are projected to time n by the time-variant incremental distances d Ln and d Rn via (1) and (2).Note that all the values in (3)-( 5) are practically not exact and have some additive random components.
In the triangulation problem illustrated in Fig. 2, state variables x n ; y n , and U n are observable indirectly via the measured angles u 1n ; u 2n , and u 3n as where i = 1, 2, 3 and the exactly known mod 2p angles h 1n ; h 2n , and h 3n existing from Àp to p are given by where Q in ¼ y i À y n ; I in ¼ x i À x n , and the node known coordinates y i and x i are: y 1 -0; y 2 ¼ y 3 ¼ 0; x 1 ¼ x 2 ¼ 0, and x 3 -0.A solution to (6) gives us the robot location, xn and ỹn , and pose e U n corrupted by noise: where and e U n is the mod 2p angle specified similarly to (7).Because xn ; ỹn , and e U n are not always available for arbitrary node coordinates, we use ( 8)- (10) in this paper only as reference noisy ''linear'' measurements.

Triangulation state-space model
We now introduce a state vector x n ¼ ½x n y n U n T of unknown variables and an input vector u n ¼ ½d Ln d Rn T of incremental distances.We suppose that random components in these values are zero mean white Gaussian and uncorrelated.We unite these components in a state noise vector w n ¼ ½w xn w yn w Un T and in an input noise vector e n ¼ ½e Ln e Rn T .Following (3)-( 5), the robot nonlinear state equation can thus be written as where f n ¼ ½f 1n f 2n f 3n T has components given by ( 3)- (5).
Noise vectors w n and e n are zero mean, Efw n g ¼ 0 and Efe n g ¼ 0, have the covariances, É , and a property E w i e T j n o ¼ 0 for all i and j.
Next, we introduce an observation vector z n ¼ ½u 1n u 2n u 3n T , nonlinear function vector , and measurement additive noise vector v n ¼ ½v 1n v 2n v 3n , and write the state observation equation as in which noise v n is also supposed to be white Gaussian with zero mean Efv n g ¼ 0, the covariance and properties ¼ 0 for all i and j.The robot stochastic dynamics is thus represented with the state-space model ( 12) and (13).

Expanded state-space model
In order to apply Kalman filtering, the nonlinear statespace model (12) and (13) needs to be expanded to the first-order Taylor series.With respect to (12), such an expansion can be provided at n À 1 as where xn is the estimate 1 of x n .Here, , and E n ¼ @fn @e e nÀ1 ;x À n are Jacobian and u n ¼ f n ðx nÀ1 ; u n ; 0; 0Þ À F n xnÀ1 is known.An expansion (14) implies that an incremental difference u n À u nÀ1 is insignificant on a unit time step and can thus be neglected.Yet, because all of the values are supposed to be known exactly at n À 1, we set w nÀ1 ¼ 0 and e nÀ1 ¼ 0.
For the triangulation Eqs. ( 3)-( 5), matrix F n ¼ @fn @x xnÀ1 can be written as where d n is given by ( 1), u n by ( 2), and ÛnÀ1 is the pose Because noise w n is additive with respect to the components of x n in (3)-( 5) and w nÀ1 ¼ 0, we also have and transform the input matrix E n ¼ @fn @e e nÀ1 ;x À n to where , and Reasoning similarly, we expand h n ðx n Þ at n as where is Jacobian, where the zero mean noise vectors wn and ẽn have the covariances, respectively, The prior estimation error P À n and estimation error P n are specified by where xÀ n and xn can be either EFIR or EKF estimate.

Extended filtering algorithms
For the expanded state-space model ( 20) and ( 21), below we give the EKF and EFIR algorithms and discuss the combined EFIR/Kalman algorithm.

Extended KF algorithm
Provided ( 20) and ( 21), the EKF algorithm is listed in Table 1, in which the initial state estimate x0 as well as the covariances P 0 ; R; Q , and L are supposed to be known in order for EKF to be suboptimal.In this algorithm, n does not exceed some constant M. Since the fully known x0 ; P 0 ; R; Q , and L is not the case for industrial applications, we admit that the approximations of these values may cause unacceptable estimation errors.

Extended unbiased FIR filtering
Unlike the recursive EKF, the iterative EFIR filter [26] utilizes measurements z n available on an interval of N past neighboring points from m ¼ n À N þ 1 to n.The EFIR filter totally ignores the covariances R; Q ; L, and P 0 .Instead, it requires an optimal averaging interval N opt in order for the performance to be suboptimal.A standard way to determine N opt is to provide test measurements for a known (reference) robot trajectory and minimize MSE given by (25).
The EFIR filtering estimate has the Kalman form in which l ranges from m þ K to n, where K is the number of the states.For each time index n, the output is taken when l ¼ n.The bias correction gain is defined here iteratively via the generalized noise power gain involving known matrices H n and F n and ignoring the covariances.

Table 1
Extended Kalman filtering algorithm.To avoid singularities, iterative calculation of ( 28) starts with m þ K and values at s ¼ m þ K À 1 are computed in short batch forms [30].In the case of triangulation (K ¼ 3), values xs and G s can be found as However, linear measurement y n is generally unavailable for (31) and Y s;m can be specified only in particular cases, like in ( 8)- (10).Therefore, in the EFIR/Kalman algorithm we substitute y n with the EKF estimates.
The iterative EFIR filtering algorithm is finally listed in Table 2, in which xn is an iterated estimate.As can be seen, this algorithm needs only N and K to start computing and updating all the matrices, provided z n and y n .No noise statistics are involved.The required N opt will be determined in Section 4.

Estimation of robot location
In this section, we apply the EKF and combined EFIR/ Kalman filter that is the EFIR filter in which y n is substituted with the EKF estimate xEKF n to the triangulation problem specified in Fig. 2. For zero mean white Gaussian noise having variances r 2 x ; r 2 y , and r 2 U in coordinates x n and y n and heading U n , we specify the noise matrix Q as Assuming that noise in the incremental distances d Ln and d Rn has the variances r 2 L and r 2 R , matrix L is specified as Finally, for the measurement noise having variances r 2 u1 ; r 2 u1 , and r 2 u1 , we specify matrix R as Provided the initial error P 0 and state x 0 , the EKF algorithm (Table 1) can be run.
In order to find N opt for the EFIR filter, below we test it by a reference robot trajectory implying known (test) model x n .Optimal N opt can then be found at a minimum of the estimation error P n given by (25) for the EFIR filtering estimate xn .

Indoor robot localization
We now consider a mobil robot travelling stepwise on an indoor floorspace.In our simulation, the noise standard deviation in coordinates x n and y n is allowed to be 1 sm and in heading U n to be 0.5°.Accordingly, we set r 2 x ¼ r 2 y ¼ 10 À4 m 2 and r 2 U ¼ 7:62 Â 10 À5 rad 2 in matrix Q given by (33).We also allow the noise standard deviation of 1 sm in the incremental distances d Ln and d Rn and set given by (34).Assuming that measurements of all angles are provided with the noise standard deviation of 0.5 , we finally set u1 ¼ 1:218 Â 10 À3 rad 2 in matrix R given by (25).Note that these deviations and covariance matrix structures (matrices should not obligatorily be diagonal) are typically not well known to the engineer.Therefore, further we will suppose that Q ; L, and R as well as the initial state x 0 and initial error P 0 are known approximately.
In order to determine N opt for the EFIR filter, we set a test reference trajectory y n ¼ x n in the algorithm (Table 2) and minimize MSE (25) by changing N. The estimation root MSEs (RMSEs) related to coordinates x n and y n are sketched in Fig. 3. Here, we also show the N-invariant RMSEs provided by the EKF under the ideal conditions of exactly and for 11: xn ¼ xn 12: and for Output: xn known Q ; L; R; x 0 , and P 0 .Fig. 3 reveals that N opt ¼ 30 corresponds to x n and N opt ¼ 23 to y n so that we accept their mean N opt ¼ 27 for the EFIR filter.Fig. 3 also demonstrates that suboptimal EKF produces lower MSEs than the unbiased EFIR one.Such a discrepancy is predictable for these filters, although the difference between the estimates is not large at N opt .Next, we investigate the case when the EFIR filter operates at N opt , but Q ; L, and R are not fully known for EKF.We thus introduce a correcting coefficient p and substitute these matrices with p 2 Q ; L=p 2 , and R=p 2 .Fig. 4 shows what goes on with the RMSEs if to change p from 0.1 to 10.As can be seen, that is only when 0:6 < p < 1:4 that EKF outperforms the EFIR filter.Note that errors in the EKF can be significantly larger if the initial state x 0 and errors P 0 are set incorrectly.
To show a consistency of EKF and EFIR estimates under the ideal conditions of fully know noise statistics, initial values, and N opt , we sketch in Fig. 5 the estimates of mobil robot location on an indoor floorspace.Here, we also show the location coordinates xn and ỹn obtained by ( 8)- (10).
Even a quick look at this figure shows that the direct estimates are too rough for applications and that the filters produce consistent although slightly different estimates xn and ŷn .In the time domain, the behaviors of xn ; ŷn , and Ûn are sketched in Fig. 6.
In order to learn discrepancies between the EKF and EFIR/Kalman estimates in more detail, Fig. 7 shows errors computed as differences between the actual model coordinates and their estimates.Here, we suppose that the noise statistics are unknown and let p ¼ 5. Also, we admit a 10% error in the initial robot state.Note that this error can be larger in applications.To run the EFIR/Kalman algorithm, we substitute in Table 2 the unknown linear measurement y n with the EKF estimate xEKF n computed using Table 1.Typical results shown in Fig. 7 reveal the following.The combined EFIR/Kalman filter is more accurate for the robot location coordinates.However, even with the admitted errors in the noise statistics and initial state, the EKF has appeared to be more successful in the estimation of robot heading.

Robot crossing the indoor boundaries
Although the model shown in Fig. 2 strictly restricts the floorspace, the structured environments may weaken the restrictions in some directions.We therefore end up with investigations of the cases when a robot may cross the indoor boundaries and temporary travels beyond the floorspace.8)- (10) and extended filters of a robot traveling stepwise on a floorspace of dimensions (30 Â 20) m in noisy environment.The EFIR and EKF produce consistent estimates.Fig. 8 shows what may happen if a robot temporary crosses the floorspace lower boundary (Fig. 8a) and left boundary (Fig. 8b).Although these cases do not cover all feasible situations, they give an impression about possible errors.As can be seen in Fig. 8a, when a robot crosses a lower boundary, both filters demonstrate brightly pronounced divergence and cannot be useful anymore.However, through some time both filters return back to the actual trajectory and their estimates can be used even beyond the floorspace.One can also observe in Fig. 8b that there is no divergence in the filters outputs when a robot temporary crosses the left boundary.This situation, however, is not regular and divergence can be watched in both filters at each of the boundaries.

Conclusions
We can now summarize some trade-off between the combined EFIR/Kalman algorithm and the EKF (Table 1) and EFIR filter (Table 2) in applications to the triangulation problem specialized by Fig. 2. The iterative EFIR filter ignores the noise statistics and the initial error statistics that is a distinctive advantage against the EKF which requires all these measures as well as the initial state.If these measures are unknown or not well-know as it usually is in industrial applications than EKF may produce unacceptable errors.On the other hand, the iterative EFIR filter needs N initial linear measurements which are unavailable.In this regard, a combined EFIR/Kalman algorithm utilizing N first EKF estimates as linear measurements while Estimation error, sm

Fig. 1 .
Fig. 1.A mobile robot platform traveling on an indoor floorspace in the direction d with the three nodes A, B, and C in a view.

n 7 :
and for Output: xn 1 xnjk means the estimate at n via measurement from the past to k. Below, we use the following notations: xn , xnjn and xÀ n , xnjnÀ1 .

Fig. 3 .
Fig. 3. RMSEs in the EFIR estimates as functions of N. Values N opt ¼ 30 and N opt ¼ 23 correspond to x n and y n , respectively, with the mean N opt ¼ 27 for the EFIR filter.RMSEs in the N-invariant EKF estimates are dashed.

Fig. 6 .Fig. 4 .
Fig. 6.Mobile robot localization corresponding to Fig. 5 in the time domain: (a) coordinate x n , (b) coordinate y n , and (c) heading U n .

Fig. 5 .
Fig. 5. Localization by (8)-(10) and extended filters of a robot traveling stepwise on a floorspace of dimensions(30 Â 20)  m in noisy environment.The EFIR and EKF produce consistent estimates.

Fig. 7 .Fig. 8 .
Fig. 7. Typical estimation errors as differences between the actual values and estimates shown in Fig. 6: (a) coordinate xn, (b) coordinate y n , and (c) headingU n .The combined EFIR/Kalman filter produces lower errors than EKF (p ¼ 5 and 1:1x 0 ) for x n and y n .