Two-Layer Nonlinear FIR Filter and Unscented Kalman Filter Fusion With Application to Mobile Robot Localization

In this paper, we propose a new state estimator called the two-layer nonlinear ﬁnite impulse response (TLNF) ﬁlter and adopt this new ﬁlter and unscented Kalman ﬁlter (UKF) as subﬁlters to create the fusion TLNF/UK ﬁlter. The TLNF ﬁlter is constructed with measurements that are redeﬁned by weighting the estimated states acquired through minimizing the cost function based on the Frobenius norm. The efﬁcient iterative form of the TLNF ﬁlter is also developed in this paper. Using the fact that the UKF and the TLNF ﬁlter each takes a different type of memory structure, the fusion TLNF/UK ﬁlter is designed as a robust nonlinear state estimator taking both advantages of each ﬁlter. To obtain the best fusion estimates, probabilistic weights are computed based on Bayes’ rule and the likelihood of each ﬁlter. Both simulation and experimental results for mobile robot indoor localization have shown that the fusion TLNF/UK ﬁlter achieves a higher level of accuracy and robustness under practical situations.


I. INTRODUCTION
In the area of signal processing and control, designing an optimal and robust state estimator for dynamic systems is of great importance. In linear systems, various state estimators have been studied for decades; in particular, the Kalman filter has been widely used as an optimal solution in many fields, such as communication, data assimilation, and target tracking. However, most industrial processes in the real world have nonlinear characteristics. To tackle nonlinear model complexity, nonlinear state estimators have been suggested. The most notable nonlinear state estimator is the unscented Kalman filter (UKF) [1], [2], which supplements the inherent disadvantage of linearization errors generated in the extended Kalman filter (EKF) [3], [4] with less computational burden than the particle filter (PF) [5]- [7]. The UKF gives an accurate estimate for nonlinear systems in ideal conditions because it minimizes variance in the estimation error and does The associate editor coordinating the review of this manuscript and approving it for publication was Lin Zhang . not undergo the linearization procedure. However, because the UKF is an infinite impulse response (IIR) filter that uses all past measurements, mismodeling errors can accumulate, which often leads to divergence of the filter. Furthermore, in many practical situations, the model parameters are not a priori or only known to some extent so that the impact of model uncertainty is inevitable. To tackle the disadvantages of the IIR filter, the finite impulse response (FIR) filter has been proposed in many studies [8]- [10].
Instead of using all the past measurements, the FIR filter uses the most recent measurement data on a fixed time interval, called a horizon, which determines the performance of the filter [11]. It prevents old errors from accumulating beyond the horizon size while estimating states. Furthermore, a benefit of using the FIR filter resides in the fact that it shows robust performance against incorrect noise statistics and model uncertainty. While the IIR filter may diverge due to mismodeling or accumulated errors, the FIR filter does not diverge and is less sensitive to mismodeling [12]- [14].
Initially, the FIR structural state estimator was proposed by Jazwinski [15], and Kwon and Han [13] developed the general minimum variance FIR (MVF) filter, which is bounded-input bounded-output stable assuming unbiased constraints. Followed by [13], Ahn and Shmaliy developed several solutions for uncertain linear models [14], [16]- [23]. Furthermore, the iterative unbiased FIR (UFIR) filter [24] was derived by Shmaliy for a discrete time-varying linear system, and the robustness was proved by applying it to the GPS-based steering of oscillator frequency in [25]. Beyond the linear estimator, for a nonlinear dynamic system, Pak designed the nonlinear extension of a general MVF filter called the extended MVF filter and combined it with horizon group shift (HGS), which is a method for adjusting the horizon size. The combined filter, called the HGS-FIR filter [26], was realized to compute all the estimates of the extended FIR filter taking different horizon sizes. The HGS-FIR filter was proven its performance through comparison with the EKF, PF, and UKF; however, it has a fatal drawback of taking 60 times longer to compute estimates compared with the UKF. In addition to the computational burden, the HGS method cannot be the solution preventing the extended FIR filter from experiencing unexpected severe disturbance or noise.
Furthermore, research on fusion estimation or information fusion has been actively carried out beyond the single filter. Sun and Deng [27] proposed a new fusion criterion for multi-sensor optimal information fusion using the decentralized KF. Hao et al. [28] proposed a weighted measurement fusion (WMF) algorithm that showed asymptotic optimality using the UKF for a nonlinear system with multiple sensors. Hao et al. [29] proposed a distributed fusion CKF weighted by matrices (MW-CKF) based on the KF framework and spherical cubature rule for multisensory nonlinear systems. Sun et al. [30] reviewed various distributed fusion estimation (DFE) algorithms for multisensor networked systems comparing performance. Most of the existing fusion estimation algorithms are proposed for multi-sensors and based on the IIR structured filter. Therefore, research on the fusion algorithm for a filter that can combine the advantages of two different types of filters is required. In particular, research on a fusion algorithm for the fusion of IIR and FIR filters and application in nonlinear systems is notably insufficient.
In this paper, a new FIR filter, called the TLNF filter, is proposed to overcome the drawback of the HGS-FIR filter. The TLNF filter is developed based on measurements that are redefined by weighting the estimated states obtained from the cost function based on the Frobenius norm. In the first layer, the estimated states are obtained to stabilize the augmented measurement stack. The weighted estimated states are combined with the measurement stack to acquire the filter gain of the second layer. Then, an iterative form of the TLNF filter is developed. By stabilizing the measurement stack in the second layer, the TLNF filter can be highly robust compared with the existing FIR filter [26] especially in circumstances where the unexpected noise would contaminate the measurements or time-varying noise would exist.
Furthermore, we propose a new fusion TLNF/UK filter. The fusion TLNF/UK filter is developed to take advantage of both the TLNF filter and the UKF so that it can obtain highly accurate states under practical conditions. The fusion estimates are computed through the process of initialization, prediction, and updating for estimates, error covariance and weights. The estimates and the error covariance of the TLNF filter are computed in an iterative form. The probabilistic weights for combining two filters are calculated using the Markov transition matrix based on Bayes' rule and the likelihood of each filter. The performance of the fusion TLNF/UK filter is verified by applying it to the mobile robot indoor localization system, in which noisy measurements must be filtered to estimate the mobile robot's position accurately. The nonholonomic mobile robot kinematic model is considered in this paper. The fusion TLNF/UK filter is presented based on simulation first; finally, experiments are conducted to show its performance.
The structure of this paper is as follows. The UKF is introduced for preliminaries in Section II and the details of the TLNF filter and the fusion TLNF/UK filter are developed in Section III. In Section IV, a demonstrative simulation is given, and experimental results follow to present the performance of the fusion TLNF/UK filter in Section V. Finally, Section VI presents the conclusion.
Throughout this paper, I n is the n-dimensional identity matrix, k is the k-dimensional Euclidean space; diag(e 1 · · · e m ) is a diagonal matrix with elements e 1 , · · · , e m ; E{·} denotes the statistical averaging; and (·) T denotes the transpose matrix operator.

II. UNSCENTED KALMAN FILTER
We first recall the general UKF [2]. We consider a nonlinear dynamic system represented in discrete time as: where f k and h k are smooth nonlinear functions, k is the discrete time index, and x k ∈ p and y k ∈ q are the state vector and the observation vector, respectively. The process noise vector w k ∈ p ∼ N(0, Q k ) and measurement noise vector v k ∈ q ∼ N(0, R k ) are assumed to be zero-mean, white Gaussian and mutually uncorrelated.
The general UKF estimatex k can be obtained by carrying out the following steps, recursively.
A set of deterministic sample points with associated weights are chosen aŝ wherex (i) k and w i represent sigma points and corresponding weights, respectively; ( √ P k−1 ) i denotes the ith row of the matrix square root; L is the dimension of x k ; γ = α 2 (L +κ)− L is a scaling parameter; κ, which is a secondary parameter, is usually selected as 0; and β is used for the prior distribution of x k .
Step 2. Time Update. The predicted mean and covariance are calculated using the sample points which are propagated through the nonlinear equations as follows: Step 3. Measurement Update. The state and covariance are updated using the Kalman gain.
Given the initial state x 0 and initial covariance P 0 , the general UKF procedure can be iteratively conducted.

III. TWO-LAYER FIR FILTER AND UKF FUSION A. TWO-LAYER NONLINEAR FIR (TLNF) FILTER
The TLNF filter estimate starts from the linearization of state equations (1) and (2). The equation of the linearization using Taylor series expansions at the estimated statex k is where k means a linearization error. Since it can increase filter estimation errors, we useũ k as a pseudo-control input for the TLNF filter. The linearization equation of the measurement equation is where We also deal with the linearization error in the measurement equation by defining the auxiliary measurement signal Linearized systems (16) and (18) can be reformulated in a batch form on the recent N number of time intervals, , called the horizon. To derive the TLNF filter, the batch form is expressed as follows: · · ·ũ T n ] T . Note that m denotes the initial point, k − N , and n denotes the final point, k − 1 on the horizon.
The first layer of the TLNF filter is designed to obtain prior estimated statex k|Y N which is obtained from measurement stack Y N as follows: where L N andL N are the gains for the first layer. VOLUME 8, 2020 Considering the unbiased condition, (22) can be expressed as follows after taking the expectations of (19) and (21) which substituted (20) in Y N : From (23), the condition to satisfy the unbiased condition becomes Now, we define cost function J L N which is based on the Frobenius norm for the filter gain function as follows [18], [19]: where · F denotes the Frobenius norm. To gain a solution for L N that satisfies constraint (24) and minimize cost function (26), the Lagrange function can be written as follows: where is a Lagrange multiplier. The partial derivative of (27) with respect to L N gives An analytical solution for (28) can be found as Then,L N can be easily obtained by substituting (29) into (25). Furthermore, usingx k|Y N obtained in (21), the second layer of the TLNF filter is designed to enhance the robustness against unexpected measurement noise. The TLNF filter redefines the measurement stackỸÑ as follows: where α is a weight parameter andÑ is a newly defined horizon size. α is a number between 0 and 1 that is determined experimentally. According to the horizon sizeÑ ,m is also redefined as k −Ñ .
The TLNF filter is designed similar to (21) aŝ whereLÑ andLÑ are gain matrices for the TLNF filter. By substituting (30) into (31) and applying the unbiased However, by using (24) andĤÑ F n,m =H n,m ,LÑ is gain that following condition: Finally,LÑ can be solved the same as L N shown in (26)-(29) by substitutingÑ instead of N .

B. ITERATIVE FORM OF TLNF FILTER
Now, to derive the iterative form of the TLNF filter, let k be an iterative variable t, and rewrite (31) aŝ whereÑ denotes the horizon size of the interval [m, t].
We define X t −1 as and following [24], (34) can be expanded as Using Sherman-Morrison-Woodbury formula [31], and X t is derived from (35) as follows: Now, letH T t,mỸÑ be expanded as and at t − 1, (33) can be rewritten as follows: Here,ỸÑ (t − 1) denotes the redefined measurement vector at t − 1.
From (34) and (35), we can express (42) Finally, after routine transformation, (33) can be represented as the iterative form as follows: and P t can be obtained iteratively as The iterative form starts at t =m + n − 1 where n is the number of states. The final estimatex k|ỸÑ is produced at t = k. By deriving the TLNF filter in an iterative form, the likelihood utilized in the fusion process can be computed.

C. FUSION TLNF/UK FILTER
This section presents the process of the fusion algorithm. The fusion algorithm which is based on the estimates of the UKF and TLNF filter, undergoes four steps: initialization, time update, measurement update and fusion. Through this paper, we assign W k and W k|ỸÑ as the weights of the UKF and TLNF filter,x k and P k as the estimate and error covariance of the UKF, andx k|ỸÑ and P k|ỸÑ as the TLNF filter estimates and error covariance, respectively.

1) INITIALIZATION
Provided the horizon sizeÑ , the TLNF filter produces the first estimate at time stepÑ and cannot provide the estimates beforeÑ . Therefore, while k <Ñ , we use the estimates of the UKF for that of the fusion algorithm. For k ≥Ñ , the Markov transition probability matrix is defined as follows: = π 11 π 12 π 21 π 22 , where the notation π ij , a design parameter that can be set arbitrarily, denotes the transition probability between the UKF and the TLNF filter. Matrix determines which filter is to be a dominant subfilter. In case the UKF is selected as a dominant subfilter, π 11 and π 21 are larger than π 12 and π 22 . The opposite case leads π 12 and π 22 to be larger. is determined experimentally, and the constraint for is that 2 j=1 π ij = 1, where i = 1, 2. Matrix should be designed appropriately in this manners, but if no prior information is available, all the elements can be set to 0.5.
For the UKF, the initial statex k and initial covarianceP k can be obtained by weighting each subfilter estimate and covariance as follows [32]: where P (1) In contrast to the UKF, the TLNF filter does not require a previous estimate and covariance. Instead, the iterative TLNF filter starts with the initial valuesx s andP s = E(x s −x s )(· · · ) T .

2) TIME UPDATE
For the UKF, the traditional UKF prediction step is conducted to compute the predicted state and variance. However, in the fusion TLNF/UK filter, the prior state and covariance of the UKF becomex k andP k .
For the TLNF filter, the prediction of the state is conducted iteratively fromx s .
The prediction of the upperbound (UB) covariance is computed as follows: whereP k−1 is computed through the following equation iteratively starting from i = s + 1 until i = k − 1: P UB k|ỸÑ can be obtained by computing using (51) iteratively until i becomes k.
The prediction of weights is calculated based on Bayes' rule as follows:

3) MEASUREMENT UPDATE
The measurement update process for the UKF is as follows: where K k = P xy P −1 yy .
The likelihood of the UKF can be computed as follows: (57) VOLUME 8, 2020 For the TLNF filter, the state and covariance are updated as follows: The likelihood of the TLNF filter is computed by Using the likelihood of each set of two filters, the weights W − k and W − k|ỸÑ are updated as follows [32]: (61)

4) FUSION
Finally, the fusion filter estimate can be obtained by combiningx k andx k|ỸÑ appropriately with the updated weights W k and W k|ỸÑ as follows:

IV. SIMULATION FOR MOBILE ROBOT LOCALIZATION
In this section, the fusion TLNF/UK filter is verified via a simulation example for mobile robot localization. Simulations are carried out for two cases: the first for the case of abrupt change in noise statistics and the second for the case of the existence of temporary model errors. These simulations are performed in MATLAB 2019a. We consider the scenario of two-dimensional localization. Four anchors are installed; their locations are (−20, −5), (20, −5), (−5, 40) and (20,40), respectively. It is assumed that the locations of the anchors are exactly known. The starting position of a mobile robot is chosen as (0, 0), and it is asked to move along a circular trajectory with the appropriate control input.
The kinematic model of the wheeled mobile robot under nonholonomic constraints is as follows [33]: where the triple (x k , y k , θ k ) are the x-position (m), y-position (m), and orientation (radian) of a mobile robot with respect to the two-dimensional world coordinate. d is the distance between the center of mass and the center of axis of the rear wheels. v k and ω k which denote the linear and angular velocity with respect to the world coordinate OXY, respectively, are the control inputs of the mobile robot. T is a sampling interval. w k is the process noise vector, which is assumed to be white Gaussian noise with zero mean. The schematic description of the kinematic model is shown in Fig. 1. The measurement equation is as follows [34]: where z l,k for l = 1, 2, 3, 4 is the measurement from the lth anchor. x l and y l are the coordinates where the anchors are located. n l,k is the lth measurement noise vector and assumed to be white Gaussian noise with zero mean.

A. CASE IN TIME-VARYING NOISE STATISTICS
In practical conditions, noise statistics are usually timevarying. We thus consider the time-varying noise in which the variance takes different values at certain intervals. Here, sampling interval T is set to 0.01 and the total number of range measurements is set to 2000. We assume that the noise variance has values of σ 2 w1 = σ 2 w2 = 0.1, σ 2 w3 = 0.05, and σ 2 v1 = σ 2 v2 = σ 2 v3 = σ 2 v4 = 0.1 in the normal situation, and changes to σ 2 w1 = σ 2 w2 = 0.3, σ 2 w3 = 0.1, and σ 2 v1 = σ 2 v2 = σ 2 v3 = σ 2 v4 = 3 when 100 ≤ k ≤ 300 and 1000 ≤ k ≤ 1300. To prove the performance of the fusion TLNF/UK filter, we test four nonlinear filters, which are the UKF, HGS-FIR, TLNF, and the fusion TLNF/UK filters. For the UKF and HGS-FIR filters, noise covariance design parameters are set according to the initially set noise value. The design parameters for the HGS-FIR filter are taken as j = 3, d = 1, N min = 3, and N max = 30; for the TLNF filter, they are taken as α = 0.8, N = 12, andÑ = 7. The horizon size for the TLNF filter is determined by references [26], [35], [36]. The initial state and covariance are set tox 0 = 0 0 0 T and P 0 = I 3 , respectively.
Markov transition matrix is designed as follows: = 0.5 0.5 0.5 0.5 As can be seen in Fig. 2, the UKF, the HGS-FIR, TLNF, and fusion TLNF/UK filters are applied for tracking the circular trajectory of the mobile robot. We assume that there are two intervals at which measurements are severely contaminated by unexpected noise in which the variance is very high compared with the normal situation. The simulation results are shown as the estimation errors of each state in Fig. 3. While the UKF gives accurate estimates at the interval where noise is not severe, the TLNF filter gives less accurate estimates. However, at the interval where the noise statistic changes, the error of the UKF is significantly higher than that of the TLNF filter. The accuracy of the fusion TLNF/UK filter is higher and stable both in the normal situation and at the interval where the noise statistics change because the fusion TLNF/UK filter follows the filter for which the error is relatively low. The result of the HGS-FIR filter is also presented and shows a similar performance to that of the TLNF filter; however, its computational cost is about three times higher than that of the TLNF and the fusion TLNF/UK filters, as shown in Table 1. The RMSEs of four filters can be clearly seen in Table 2.

B. CASE IN TEMPORARY MODEL ERRORS
Now we verify the performance of the fusion TLNF/UK filter in a situation with temporary model errors. Here, T is set to 0.01 and the total number of range measurements is set to 2000. We assume that the kinematic model undergoes unpredictable model uncertainty in parameter d when 100 ≤ k ≤ 400 and 1000 ≤ k ≤ 1300. However, the filters applied are still designed using the kinematic information in a normal situation. We assume that the noise variance has values of For the UKF and HGS-FIR filters, the design parameters of noise covariance are set according to the initially set noise value. The design parameters for the HGS-FIR filter are taken as j = 3, d = 1, N min = 3, and N max = 30; for the TLNF filter, they are taken as α = 0.7, N = 12, andÑ = 7. The initial state and covariance are set tõ  Table 2 in relation to each of the VOLUME 8, 2020  two cases. It can be observed that the estimation errors of the fusion TLNF/UK filter are always lower than those of the UKF and TLNF filters.

V. EXPERIMENTS FOR MOBILE ROBOT LOCALIZATION
In this section, the fusion TLNF/UK filter is tested in real-world applications. We demonstrate two examples of its practical applications for a mobile robot localization system. In the first experiment, the mobile robot moves along a circular trajectory, and in the second experiment, it moves from the starting point, to the final point, avoiding obstacles. The ultra-wideband, which is the wireless sensor network, is used for the mobile robot localization system. and (5m, 5m) in two-dimensional XY coordinates.

A. CIRCULAR TRAJECTORY
The measurements are sampled with T = 0.4s. Because the noise in the localization system is not known exactly, we analyze the process to some extent and assign σ 2 w1 = σ 2 w2 = 0.2, σ 2 w3 = 0.1, and σ 2 n1 = σ 2 n2 = σ 2 n3 = σ 2 n4 = 0.1 to the filters. The Markov transition matrix is the same as that in Section IV. The horizon size of the HGS-FIR and TLNF filters are both set to 7. α is determined as 0.7 experimentally. We apply filters to the mobile robot which moves along a circular trajectory. The circular trajectory experiment results are shown in Fig. 6.   the figure, the UKF shows the worst performance in the practical condition. The absolute estimation error of the HGS-FIR filter is slightly higher than the TLNF filter, which shows the most accurate performance. Because the fusion TLNF/UK filter is designed to follow the estimate of the most accurate filter, the fusion TLNF/UK filter shows similar results to the TLNF filter.

B. OBSTACLE AVOIDANCE TRAJECTORY
In this experiment, we apply filters to the mobile robot which moves from the starting point to the set point avoiding obstacles. Measurements are sampled with T = 0.4s. We assigned noise statistic values of σ 2 w1 = σ 2 w2 = 0.1, σ 2 w3 = 0.05, and σ 2 n1 = σ 2 n2 = σ 2 n3 = σ 2 n4 = 0.1 to the filters. All the elements of the Markov transition matrix are set to 0.5, as in the case of circular motion. The horizon sizes of the HGS-FIR and TLNF filters are both set to 7. α is determined as 0.8 experimentally.
The trajectory of experiment results is shown in Fig. 8. Three cuboid-shaped obstacles are installed and represented by black rectangles in the XY coordinates. Fig. 9 shows the absolute estimation error of the mobile robot's x,y position obtained using four filters: the UKF, HGS-FIR, TLNF, and fusion TLNF/UK filters. According to the figure, the UKF shows the worst performance in the practical condition.
In the real mobile robot localization experiments, the TLNF filter shows the most accurate performance so that the fusion TLNF/UK filter follows the results of the estimated states of the TLNF filter. This is because the noise statistics are unknown and model uncertainty always exists in the real world. However, depending on the various situations of localization and the tuning of the noise statistics values,  the fusion TLNF/UK filter may follow the estimated states of the UKF.

VI. CONCLUSION
In this paper, we have proposed a fusion TLNF/UK filter based on the TLNF and UKF for nonlinear state estimation. The TLNF filter is proposed for an alternative nonlinear FIR filter that shows robust performance and has a lower computational burden compared with the HGS-FIR filter. The fusion TLNF/UK filter is designed to show accurate performance in well-specified environments while also remaining robust in uncertain conditions for the discrete-time nonlinear system. The simulation results show that the fusion TLNF/UK filter guarantees reliable estimation performance compared with each of the nonlinear filters. A practical example of indoor localization of a mobile robot, whose measurements are contaminated by unexpected noise, has demonstrated good correspondence with the theory. VOLUME