TWOR: Improving Modeling and Self-Localization in RFID-Tag Networks Under Colored Noise

This paper discusses the three-wheeled omnidirectional robot (TWOR) self-localization in radio frequency identification (RFID) tag environments. The nonlinear TWOR model is significantly improved by using geometric interpretation and incremental time representation in discrete time. The TWOR position and heading are self-estimated using distance measurements to RFID tags and a digital gyroscope in the presence of typical colored measurement noise (CMN). The extended unbiased finite impulse response (EFIR) is developed along with the extended Kalman filter (EKF) and their versions, cEKF and cEFIR, modified for Gauss-Markov CMN. A particle filter is used as a benchmark. It is shown that the cEFIR filter is more robust than the cEKF and almost as robust as the particle filter, although the latter is less accurate in real time.


I. INTRODUCTION
Wheeled mobile robots (WMR) are referred to as omnidirectional robotics systems that are capable of reaching every position in their environment. The first omnidirectional robot named Uranus was designed and constructed in Carnegie Melon University [1]. Since then, different kinds of such robots were exploited in industry [2], medicine, warehouses, logistics, etc. One of the most efficient types of WMR is the three-wheeled omnidirectional robot (TWOR) [3], which effectively works in indoor and outdoor environments. The TWOR has many practical advantages, such as the flexibility in travelling along complex industrial trajectories and the ability of changing the position and orientation quickly. Therefore, many efforts were made to improve TWOR modeling and control.
Since there is no standard TWOR kinematic-dynamic model yet, the authors mostly use two methodologies based on the vector approach [4] and the transformation approach [5]. The first extensive studies in this area were conducted in [6], where the kinematic model of a mobile The associate editor coordinating the review of this manuscript and approving it for publication was Ángel F. García-Fernández . robot was created using the coordinate matrix transformation. Later, the Lagrange formulation was used in [7] to represent the model with reduced-order equations using a holonomy system matrix. Most recently, a cascade structure of TWORs with an inverse kinematic block was discussed in [8] to generate the velocity references required by the predictive controller. It increases the robustness to disturbances and is also better suited for multivariate problems with friction phenomena. However, this model is not well suited to model predictive control that requires accurate modeling.
In indoor environments, the TWOR self-localization can be organized using ultra wideband (UWB) technology [9] inertial navigation system (INS), hybrid schemes [10], and radio frequency identification (RFID) tags [11]. The latter approach has attracted attention due to low cost, low (or zero) energy consumption, and a wide distance range [12]. It can also be combined with other methods. For example, a novel localization method proposed in [13] combines the RFID tag data with laser-based measurements. In [14], the capabilities of a variable power RFID tags and passive ultra-high frequency (UHF) RFID tag are employed to create networks in complex environments. A localization system designed in [15] combines two types of RFID tag-generated signals with a logical classification strategy. In [16] the authors solve the vehicle localization problem using a single antenna multi-frequency ranging scheme and compute the vehicle position through multi-parameter nonlinear optimization methods.
Regardless of the method used, the highest navigation accuracy is achieved using state estimators [17]. The Bayesian estimator developed in [15] employs process and state distributions without using a model, but is computationally inefficient. In linear Gaussian environments, the state estimation problem is optimally solved by the Kalman filter (KF). For nonlinear models, the extended KF (EKF), unscented KF (UKF), and particle filter (PF) are often used. A good robustness is offered by an unbiased finite impulse response (UFIR) filter [18] and its extended version (EFIR) [19], which do not require information of zero mean noise and initial values.
It is worth noting that RFID tag-based navigation is usually accompanied by colored measurement noise (CMN) [20], [21]. To deal with Gauss-Markov CMN, the Bryson algorithm [22] and Petovello algorithm [23] are usually used. In the Bryson algorithm, the CMN is filtered out in two phases: smoothing and filtering. In the Petovello algorithm, only one stage (filtering) is used. Most recently, several end-to-end algorithms were designed for Gauss-Markov CMN in [24] using the backward Euler method. Also, the problem with CMN was addressed by Zhou et al. in [25] using the second moment of information and by Ding et al. in [26] using the autoregressive moving average model.
In this paper, we significantly improve the TWOR nonlinear model using: 1) geometric interpretation of smoothly varying velocity profiles and 2) incremental-time representation in discrete time. We also develop the EKF and EFIR filtering algorithms and modify them as cEKF and cEFIR for CMN. We show that in RFID tag environments, the cEFIR filter significantly outperforms the EFIR filter, cEKF, and EKF under CMN. The cEFIR filter is almost as robust as the PF. However, the latter is less accurate in real time. The main contributions are the following: • An improved TWOR model for implementing hardware and software on an onboard computer.
• cEFIR and cEKF filtering algorithms for TWOR localization in CMN.
• Experimental evidence for better performance of the cEFIR filter compared to the EKF, cEKF, EFIR filter, and PF. The rest of the paper is organized as follows. Section II develops the TWOR model with incremental-time events and formulates the problem. Section III presents the TWOR extended space-state model, modifies the equations for CMN, and develops the cEFIR, EKF, cEKF, and EFIR algorithms. Section IV provides numerical simulations with synthetic and experimental data and testes the filters for robustness. Finally, concluding remarks can be found in Section V.

II. TWOR KINEMATIC AND PROBLEM FORMULATION
The TWOR manufactured as the Festo Robotino Mobile Robotic Development Platform [27] is pictured in Fig. 1. It is designed for planar motion with three degrees of freedom (DOF).
Depending on the types of dynamic constraints, TWOR can be controlled in each of the DOF independently. Such TWOR is called non-holonomic and can travel in any direction without orientation restrictions. In this paper, we are dealing with this type of TWOR.
Considering the results obtained in [5] and [6], we first modify the kinematic diagram of the TWOR with perpendicular rollers as sketched in Fig. 2. The robot is driven by displacements in its wheels. Assuming a no-slip condition, the displacement u j , j ∈ [1,3], in the jth wheel can be found by projecting the displacement vector u j into the local coordinate system 25584 VOLUME 11, 2023 (x L , y L ) and using the contribution of the arc segment caused by the rotation ϕ of the robot. This gives where R is the radius of the robot platform. Further, we will use (1) to represent the TWOR dynamics in state space. The problem can now be formulated as follows. Using the kinematic diagram shown in Fig. 2, we would like to represent the TWOR nonlinear dynamics in discrete-time state-space. We also wish to develop the EFIR filter, cEFIR filter, EKF, and cEKF to provide the TWOR self-localization in RFID tag environments under CMN and test them along with the PF by simulations and using experimental data.

III. STATE SPACE REPRESENTATION AND FILTERING ALGORITHMS
Looking at the kinematic diagram (Fig. 2) and considering the TWOR in two neighbouring discrete-time points, we can now represent the TWOR dynamics in nonlinear state space.

A. NONLINEAR STATE EQUATION
The TWOR reasonable state variables are u x L , u y L , and ϕ. Using these variables and (1), we rewrite the jth displacement u j , j ∈ [1,3], in terms of the increments u x L , u y L , and ϕ as  where the vector U i of displacements and the vector U L of state variables are defined by comparing (2a) and (2b), and A is a mapping matrix defined by (2a). Note that the vector U L , which contains all information about the robot location in local coordinates, can be determined in terms of U i by inverting (2b). Since the TWOR kinematics ( Fig. 2) suggests that δ = π/6, we thus have There are two ways how to represent the robot dynamics in state space. One can either assign the robot states and approximate the trajectory using a polynomial state model [10] or project the current robot position one step ahead using equations (2a) and (2b) [11], [28]. In this paper, we follow the second way as being physically more appealing.
To find the position in the global coordinate system, we introduce the discrete-time index n, apply the matrix rotation operator R(ϕ n ), obtain U = R(ϕ n )U L and U = R(ϕ n )A −1 U i , and provide some transformations. This gives where u 12n = u 1n − u 2n and u 123n = u 1n + u 2n − 2 u 3n . In accordance with the robot odometry, u xn , u yn , and ϕ n are increments in the robot coordinates x and y and and heading angle ϕ.
The robot coordinates and heading considered as states x 1n = x xn , and x 2n = x yn , and x 3n = ϕ n at n can now be represented with the nonlinear functions where the increments u xn , u yn , and ϕ n are given by (3)-(5). Now, for ϕ n−1 = x 3(n−1) , we have the following nonlinear functions with respect to the robot states, Next, we introduce the state vector x n = [x 1n x 2n x 3n ] T and the control vector u n = [ u 1n u 2n u 3n ] T , whose components are variables of increments u xn , u yn , and ϕ n given by (6)- (8). We also suppose that the TWOR process noise is zero mean, white Gaussian, and additive and introduce the noise vector w n = [ w 1n w 2n w 3n ] T ∼ N (0, Q) with the covariance Q = E{w n w T n }. At this point, the TWOR dynamics can be represented in discrete-time state-space with the following nonlinear state equation where the components of the nonlinear vector function f = [ f 1n f 2n f 3n ] T are specified by (6)-(8).

B. NONLINEAR OBSERVATION EQUATION
The indoor RFID tag environment can be organized with k-tags T j , j ∈ [1, k], having known coordinates (χ j , µ j , ν j ) in a way such that at least three tags are available at each n. The TWOR position can be determined via the distances d jn measured between the tags and the TWOR reader in the presence of additive noise v jn . The TWOR heading angle ϕ n = x 3n is measured in global coordinates directly by a digital gyroscope as φ n in the presence of additive noise v ϕn , and the observation equations can thus be written as Since the RFID tag-based measurement noise in colored [20], [21], we represent v jn by the Gauss-Markov model where ξ jn ∼ N (0, R j ) is zero mean white Gaussian driving noise with the covariance R j and 0 < j < 1 is the coloredness factor, which zero value jn = 0 makes v jn white. For the sake of stability j should not exceed unity.
The nonlinear observation equation can now be written as where y n = [ y 1n y 2n . . . y kn y (k+1)n ] T ∈ R k+1 is the observation vector, the components of the nonlinear function where η n ∼ N (0, R) ∈ R k+1 is zero mean white Gaussian with the covariance R and the coloredness factor The TWOR state-space model represented by the nonlinear equations (9), (11), and (12) can now be extended to use linear estimators as we will do next.

C. EXTENDED SPACE-STATE MODEL
The standard approach to apply linear state estimators to nonlinear models is to expand nonlinear functions f (x n−1 , u n ) and h(x n ) using the first-order Taylor series. The function f (x n−1 , u n ) can be expanded around x n−1 as [19] f wherex n−1 is the available past estimate and F n = ∂f ∂x x n−1 is the Jacobian matrix. The extended state equation can then be formalized with wherex − n x n|n−1 = F nxn−1|n−1 is the available prior estimate at n and H n = ∂h ∂x x − n is the Jacobian matrix. By introducing a new observation vector ς n = y n −s n , n is a known function, the extended observation equation (11) attains the form where v n is the Gauss-Markov CMN (12). The extended equations (13) and (14) can further be modified for CMN using measurement differencing as in the following.

D. EXTENDED MODEL FOR CMN
By introducing a new observation z n , using measurement differencing, and following [24], we obtain where D n = H n − n and v n = n w n + η n .
Next, we remove the bias error from (15) asz n = z n − nũn , where nũn is known. This gives a new observation equation in which, as required,v n is white Gaussian with the properties As can be seen,v n is correlated with w n . To apply the EKF, a new Kalman gain is required for time-correlatedv n and w n .

E. EXTENDED KALMAN FILTER FOR CMN
To derive the Kalman gain for time-correlated w n andv n , we define the estimation error as n = x n −x n and the prior estimation error as − n = x n −x − n , wherex − n is the prior estimate of x n . Next, we follow [24] and consider the following functions: the prior error covariance P − n = E{ − n − T n }, the measurement residuals n =z n − D nx − n , the innovation covarianceS n = E{s ns T n } obtained using (23), S n = D n P − n D T n + H n Q n T n + n Q n D T n + R n , (24) where n = H n − D n , the estimation error n = (I − K n D n )F n n−1 + (I − K n D n )w n − K nvn , (25) where I denotes the Identity matrix with appropriate dimensions. The error covariance obtained using (25) as Because the trace of P n is convex, we put to zero the derivative applied to the trace of (26) with respect to the bias correction gain K n as whose solution gives the optimal bias correction (Kalman) gain for correlated noise, Finally, substituting K n taken from (28) into (26) gives A pseudo code of the cEKF for time-correlated w n andv n is listed as Algorithm 1.

Algorithm 1 cEKF for Correlated w n andv n
Data: ς n ,x 0 , u n , n , P 0 , Q n , R n Result:x n , P n begin for n = 1, 2, · · · do z n = ς n − n ς n−1 ; The algorithm requires the nonlinear functions to update the estimates and the corresponding matrices of the linearized equations.

F. EXTENDED UFIR FILTER FOR CMN
The advantage of the robust UFIR filter originally derived in [29] is that it does not require any information about the zero mean noise. The only tuning factor in this filter is the averaging horizon, which must be optimal, N opt , to minimize the mean square error (MSE). To develop the UFIR filter for the nonlinear model (9) and (10) using Taylor series, we follow [18] and use the linear model (13) and (18). On the horizon [m, n] of N points, from m = n − N + 1, the state equation (13) can be extended as [19] X m,n = F m,n x m + B m,n (U m,n + W m,n ), where T are the extended vectors, the extended block matrices are and the product matrix is defined as We assume that the initial state x m is known, and thereforẽ u m = 0 and w m = 0. Similarly, by assigning the extended vectors Y m,n = [z T m z T m+1 · · · z T n ] T and V m, where the extended block observation matrix is given by and other block matrices are specified as L m,n = D m,n E m,n , G m,n = D m,n B m,n , and D m,n = diag(D m D m+1 · · · D n ). The algorithm requires an initialization on a short initial horizon over a data vector Y m,s , where s = m + K − 1 and K is the number of the states, and the estimate will be unbiased, if the condition E{x n } = E{x n } is satisfied. To specify the statex s , we sequentially project it from m to s through the nonlinear function f (x n ). To obtain the unavailable statex m during the first N points, we employ a supporting KF. We also refer to [28] and assign G s = I .

Algorithm 2 cEFIR Filtering Algorithm for CMN
Data: ς n Result:x n begin The pseudo code of the cEFIR filtering algorithms developed for CMN is listed as Algorithm 2. It is worth noting that, since the cEFIR filter ignores zero mean noise, the time-correlation between the white w n andv n is ignored, unlike in cEKF. To determine N opt in the MSE sense [30], we first compute the error covariance of the cEFIR filter by (26), where we set K n = G n D T n , P n = P − n − (2P − n D T n + 2Q n T n + G n D T n S n )D n G n . (36) We then numerically find N opt by minimizing the trace of (35) as shown in [30].

G. COMPUTATIONAL COMPLEXITY
In this section, we discuss the computational complexity of the estimation algorithms. We also provide a comparison of the discussed algorithms with the PF. The bottleneck of the cEKF is in the computation of the residual covariance S n , and of the cEFIR filter in the computaiton of the gain matrix G n . In both cases, the complexity is O(n 3 ), in terms of floating operation point (FLOP). The efficiency in terms of FLOPS is illustrated in Table 1, where K is the number of system states, M is the number of measurements, and N opt is the horizon length for the cEFIR filter. This table suggests that the EKF and cEKF filters consume the smallest time, since these filters operate on a one-step horizon. To measure the consumed time, we run the algorithms for a straight TWOR movement in a simulated environment with 10 tags. The tags are deployed with equal intervals of 0.5 m along the path at a height of 1 m above the floor. The trajectory is computed over 500 points to feed the algorithms. Note that extra parameters required by the cEFIR and PF algorithms increase the computing time.
To make a correct comparison, we use the following criteria: • Run all algorithms for n = 0 to obtain near the same RMSEs.  • Set the horizon N opt such that the RMSEs of the cEFIR filter and cEKF become near equal.
• Run the PF with the number of particles equal to N opt . The measured computation time is shown in Fig. 3. We see that the proposed cEFIR algorithm requires more time than the cEKF, and that the PF is the slowest filter. We also see that the computation time increases from 0.108 s in the EKF to 0.2848 s in the cEKF filter, 3.0521 s in the EFIR filter, 5.1906 s in the cEFIR filter, and 6.7301 s in the PF. It is worth noting that, as a compensation for the large computation time, the filter becomes more robust that we will see in the next section.

IV. SELF-LOCALIZATION IN RFID-TAG ENVIRONMENTS
We now simulate the TWOR self-localization in RFID tag environments using the cEKF and cEFIR filter along with the EKF, EFIR filter, and PF. When no data arrive from a tag, the covariance of the measurement noise is set large, and the filter ignores this measure. In this case, lost data are replaced by prediction organized using the observation model and previously estimated state. Thereby, the observation vector always has the same dimention. To test an estimator for robustness against CMN when the coloredness factor ranges as 0.05 0.95 with a step 0.05, we consider two scenarios of filter tuning: 1) tuning to = 0.05 and 2) tuning to = 0.95. For the FIR filters, we additionally measure the optimal horizon N opt as function of . To find N opt , we compute the RMSE for different values of N and choose N = N opt that corresponds to the minimum RMSE. The number of particles for the PF is set equal to N opt .
It is worth noting that multipath effects affect RF measurements when there is no direct path between a tag and an antenna. This condition is know as Non-line-of-Sight (NLOS). The effect of NLOS reduces the effective power of the transmitted signal, leading to a biased value of Received Signal Strength Intensity (RSSI). These phenomena are main causes of tracking error in our experiment.
In what follows, we provide two investigations. In the first case, we test the filters by the TWOR circular trajectory in an indoor space with a constant motor displacement and measurement noise. In the second case, all filters are tested by experimental data available from [31].

A. CIRCULAR TRAJECTORY IN INDOOR ENVIRONMENT
The RFID tag network and the TWOR planned circular trajectory in an indoor navigation space are shown in Fig. 4a, where colored blocks represent furniture. RFID tag-based measurements corrupted by CMN with = 0.95 are depicted with ×, and we notice that multipath channels affect the measurements. We use 12 long-range UHF RFID tags (Chip Alien H3, Protocols ISO18000-6B/C, Dimensions: 78×30×8 mm). The center of the RFID ALR8698 antennas is located at 0.5 m from the floor, and the RFID tags are located 1 m above the center of the antennas. All tags are spaced 2 m apart and attached to the walls in a square room (8×8)m. Each tag has a unique ID number and exactly known coordinates of location. We use a custom-build TWOR that operates based on an Nvidia Jetson TX2 board and has a radius of R = 0.18 m. The interrogator ALR-F800 (Alien Technology) interacts with the tags within the reader range, collects data, and transmits data to a host computer. The reader interrogation velocity is 1000 tags per second, but we employ only 12 tags. To mitigate the effect of the RFID antenna on the system performance, we use a circularly polarized antenna Alien ALR-8698. We create the observation vector using received signal strength information (RSSI) from the reader. At the test stage, we measure some RSSI values to estimate the constant parameters from the log-normal propagation model. We set the power loss coefficient for offices as n = 33, as recommended by Radiocommunication Sector of International Telecommunication Union ITU-R P.1238-7.
The TWOR dynamics is described by the nonlinear state space equations (9) and (11), where the number of the tags is k = 12. The CMN v n in the observation (11) is modeled by the Gauss-Markov model (12), where the diagonal covariance matrix of white Gaussian noise η n is R = diag (σ 2 η . . . σ 2 η σ 2 ) with σ η = 20 cm and σ = π/360 rad. To apply the linear filters, we use the extended equations (13) and (14) and the new observation equations (15) and (18). The system noise diagonal covariance matrix Q = diag (σ 2 w σ 2 w σ 2 w ) has σ w = 10 cm. To reduce self-localization errors under CMN and test the estimators for robustness, we next conduct investigations by setting different values of the factor . To cover possible cases observed in real data, we change from 0.05 to 0.95 with a step of 0.05 and consider the following options. Before conducting this investigation, we realise that = 0.05 corresponds to N opt = 152 and = 0.95 corresponds to N opt = 400. Then we tune the EFIR and cEFIR filters to = 0.05 and N opt = 152, and apply to the model with other values of n . We then run the Monte Carlo (MC) simulation 100 times and show the average root MSEs (RMSEs) produced by the filters in Fig. 5. What we see is that the EFIR filters produce the smallest and consistent estimates when < 0.5 and that the cEKF tuned to = 0.05 gives slightly larger errors. Also, no significant difference between the EFIR estimates is observed when < 0.5. On the VOLUME 11, 2023   contrary, under the heavy CMN with = 0.95, all filters produce large errors, although the EFIR filters give more accuracy.
The RMSEs produced by the EFIR and cEFIR filters tuned to = 0.05 for the process affected by = 0.05, = 0.5, and = 0.95 are listed in Table 2, where we test the filters by N opt = 152 and N = 400 for each . The RMSEs produced by the EKF and cEKF tuned to = 0.05 are listed in Table 3 (first row). We see that the cEFIR filter produces the smallest errors (bolded) when 152 N 400 that definitely speaks in favor of its robustness to N . The benchmark PF demonstrates the highest robustness in Fig. 5, but this filter is less accurate then the cEFIR filter.
2) FILTERS TUNING TO n = 0.95 In the robust mode, we tune the filters to the worst error case of = 0.95 and N opt = 400. Then we reduce in the model from 0.95 to 0.05, compute the RMSEs, and put the results to

B. EXPERIMENTAL TESTING
To validate the results obtained by simulations, we next consider the mobile robot experimental trajectory available from [31]. The measurement vector is created for a mesh of 16 tags placed in a square grid on a distance of 5 m to the nearest neighbor. From the dataset we take the RFID_global_robot fields to compute the distance to each tag in the same state-space model. To test the filters for robustness, we consider two options: 1) tuning to = 0.05 and 2) tuning to = 0.95. The workspace for the TWOR is shown in Fig.6 with the tags deployed on the floor. Also, this figure shows the estimated trajectory in the worst case of = 0.95.
The localization RMSEs are listed in Table 4 for EFIR and cEFIR and in Table 5 for EKF and cEKF. The RMSEs produced by the filters are sketched in Fig. 7. For filters tuned to = 0.05, the RMSEs are depicted with →, and for = 0.95 with ←. We see that when tuned to = 0.05 with N opt = 82 and to = 0.95 with N opt = 245, the cEFIR filter gives the minimum errors consistent with the simulations.

V. CONCLUSION
The extended filtering algorithms developed in this paper have produced acceptable errors for TWOR self-localization in RFID tag indoor environments under CMN. This has become possible by improving the TWOR model using geometric interpretation for smoothly varying velocity profiles and incremental-time representation in discrete time. The results obtained by simulations and experimentally have revealed that under CMN the cEFIR filter outperforms the cEKF, EFIR filter, and EKF in terms of accuracy and robustness. The PF demonstrates a bit better robustness, but this filter is less accurate than the cEFIR filter in real time localization. It also follows that the cEFIR filter has the highest robustness under harsh operation conditions with heavy CMN. We now modify the TWOR dynamic model for linear state space representation in order to use the FIR and Kalman approaches straightforwardly. The comparative results will be reported in near future.  . He was rewarded a title, Honorary Radio Engineer of the USSR, in 1991, has received the Royal Academy of Engineering Newton Research Collaboration Program Award, in 2015, the IEEE Latin America Eminent Engineer Award, in 2021, and several best conference paper awards. He was invited many times to give tutorial, seminar, and plenary lectures. His current interests include optimal and robust state estimation with applications.