An FEM-Based State Estimation Approach to Nonlinear Hybrid Positioning Systems

For hybrid positioning systems (HPSs), the estimator design is a crucial and important problem. In this paper, a finite-elementmethod(FEM-) based state estimation approach is proposed to HPS. As the weak solution of hybrid stochastic differential model is denoted by the Kolmogorov’s forward equation, this paper constructs its interpolating point through the classical fourth-order Runge-Kutta method.Then, it approaches the solution with biquadratic interpolation function to obtain a prior probability density function of the state. A posterior probability density function is gained through Bayesian formula finally. In theory, the proposed scheme has more advantages in the performance of complexity and convergence for low-dimensional systems. By taking an illustrative example, numerical experiment results show that the new state estimator is feasible and has good performance than PF and UKF.


Introduction
In recent years, a new concept called hybrid positioning systems (HPSs) is more and more popular in navigation and location-based services research community [1].HPSs are systems for finding the location of a mobile device by using several different positioning technologies in indoor environments.Mostly, GPS is one major component of such systems, aided by others such as tower signals, wireless internet signals, Indoor Messaging System (IMES), bluetooth sensors, or other local positioning systems [1,2].So far many different hybrid positioning systems are being explored and gradually applied in services from Google Maps for Mobile, Combain Mobile, SkyHook, openBmap, Navizon, PlaceEngine, Xtify, and so forth.
Obviously, HPS will be well developed in urban locationbased service areas because of the huge commercial market.So studies are rapidly increasing and more practically viable than ever [1,[3][4][5][6].But in these literatures, researchers paid more attention to the architecture of HPS, or how to choose and integrate different sensors or signals reliably [1,5,6].Actually, for getting reasonable accuracy and precision, position estimator is one of the key factors in the hybrid positioning systems, which will directly affect the quality of system performance.As we all know, for an HPS use in real environments, there is hardly enough measuring information for a unique position because of the reflection conditions of the environment, which will influence the WLAN or the other radio signals.On the other hand, the measurement functions are nonlinear functions of the motion state, so we have to think about the estimation accuracy the variations because the sensors' signals may significantly change depending on the motion of vehicle (or person) and slight differences of the positions at which the signals are measured [7].For example, due to the influence of the transmission distance, WLAN or other mobile phone signals with the nonlinearity can have a significant effect on the estimation errors than GPS measurements.
Since state estimation of a stochastic dynamic system from noisy observations is a crucial and important problem in hybrid positioning applications, the motivation for this paper is what we can do with the performance of position estimator.In [8] three Kalman Filter Extensions in hybrid navigation are evaluated, respectively.It is not difficult to find that the estimates tend to become biased and inconsistent when the linearization errors are coming large.Subsequently, a robust Extended Kalman Filtering modeled in [9] shows that it outperforms EKF and EKF2 in cases where there is blunder measurement or considerable linearization errors present only in simulation cases.Although EKF is popular in many navigation applications [10][11][12][13][14][15], it must satisfy three assumptions for using EKF and its variants.(1) The deviations of the reference state trajectory should be small.(2) The mathematical description for state and observation of the dynamics system should be accurate.(3) The conditional density function of the state should be Gaussian.Otherwise, the performance of EKF will become unstable and poorly.Therefore, this is not a recommended approach for HPS estimating problems.
Particle Filter (PF) is another popular estimating algorithm in HPS [1,6], which is also known as a sophisticated technique based on Sequential Monte Carlo (SMC) method, started by Gordon et al. in 1993 [16].Roughly speaking, the power of PF is that it can approximate the posterior probability density of a complex system by using a kind of stochastic sampling particles with different weights and update the posterior by involving the new observations according to the Bayesian principle.In the past twenty years, a lot of work has been done to develop the performance of particle filters.Especially like Auxiliary Particle Filter [17], Unscented Particle Filter [18], and Gaussian Particle Filtering [19] et al. improve the importance sampling effectively.In [20], a so-called Rao-Blackwellised Particle Filtering can exploit the structure of the dynamic Bayesian networks to increase the efficiency of particle filtering.For preventing the sample impoverishment problem, some heuristic algorithms like genetic algorithm (GA) are incorporated into a particle filter to overcome this drawback of the filter innovatively [21].With the development of computing technology, PF is now in a golden age as it could deal with nonlinear, non-Gaussian, non-steady-state recursive estimation problem.However, in fact, it still has some disadvantages in practice, such as the fast growing computational complexity and the sample impoverishment.
Theoretically speaking, a typical hybrid positioning system should be described by a partial differential equation (Kolmogorov's forward equation) and a difference equation separately, while the former reflects how the conditional density of a dynamic system evolves, and the latter means how it is works by the new measuring information [22].For solving the problem, finite element method (FEM), a classic procedure for approximating solutions of partial differential equations (PDEs), is introduced in this paper.FEM assumes that the exact solution to a PDE could be expanded with a sum of local basis functions.Inspired by [23][24][25][26], an approximate solution is found by combining interpolation points and shape function.Essentially, FEM has more advantages than PF or other classic filters like EKF and UKF.It outperforms others in case of model mismatches, large state variations, and arbitrary initial conditions.This paper is organized as follows.The next section consists of the abstractive description of the hybrid positioning problem in a Bayesian framework.The measurement model and the state model are described as continuous nonlinear dynamics and discrete nonlinear observations separately.Section 3 gives the procedure of FEM and shows how make it works to solve estimating problems.In Sections 4 and 5, we analyze the algorithm performance of convergence and computational complexity mathematically.Then, an illustrative example and its simulation results are given in Section 6.Finally, we conclude this paper in Section 7.

Problem Description and Preliminaries
Actually, most hybrid positioning systems that we have in real applications may be reasonably described in continuous dynamics and discrete measurements.So an abstractive model denoted by a partial differential equation and a difference equation separately as is considered, where   ∈ R  is a state vector and   ∈ R  is an output vector.(  , ) : R  ×R → R  and ℎ(   ,   ) : R  × R → R  denote system and measurement equations, while (  , ) : R  × R → R × is diffusion coefficient.Here   is -dimensional Brownian motion vector with the covariance matrix [     ] = (), and   is an -dimensional white Gaussian variable independent of   with covariance matrix (),  ≥ 1, where [     ] = (), () ∈ R × .Let   = {  ,   ≤ } denote the observations set accumulated up to , so the problem is evolved into how to seek for the conditional density (,  |   ).Because the statistical information of the system is contained in the measurements   and the initial condition ( 0 ,  0 |  0 ), the conditional mean and variance can be computed from (,  |   ), which generally depend on all of the higher order moments for any nonlinear system.Theoretically, with the observations between time   and  +1 , the conditional density  ≜ (,  |   ) diffuses according to Kolmogorov's forward equation Usually, a weak solution of stochastic differential equation could be decided by transferring function, so the conditional probability density (,  |   ) of system will satisfy Kolmogorov's forward equation where  ≜ (,  |   ) is called the weak solution of the stochastic differential model.Consider Thus, Kolmogorov's forward equation ( 2) and its initial boundary conditions (3) have been differential equations boundary value problems, where Ω is smooth boundaries area in R  , and  : Ω → R is solution of boundary value problem.

An Estimator Design Based on Finite Element Method
In fact, with the recursive Bayesian estimation framework, Particle Filter is a method that piecewise approximates probability density function of a state in function space, while finite element method (FEM) approximates for state distribution function in the state space.When we approximate the solution of Kolmogorov's forward equation by FEM, it could be conducted into a problem to solve the ordinary differential equations with coefficients in piecewise function.
In this section, firstly, we use FEM to simplify formula (2) from a function of space and time to a function of time only, and a linear ordinary differential equation is obtained, which is easy to solve numerically.Certainly, FEM is a powerful tool to solve Partial Differential Equations (PDEs) problem, which is discussed in most textbooks; therefore, an estimator design flowchart based on FEM is shown in Figure 1, reference to the literature [24][25][26].
For any state vector (  ∈ ), the probability density function of state can be calculated as with a fixed time , where (, ) is a member of the Hilbert space that is consisted with  :   → In Figure 2(a), for any quadrilateral unit, it can be expressed as (  ,   ),  = 1, . . ., 4, respectively, whose function values are given as  1 ,  2 ,  3 , and  4 .Assuming that only four corner nodes are established at the beginning, so the corresponding shape function by Lagrange polynomial can be obtained as The shape function of each node should satisfy the conditions (8) where  represents the number of nodes.
If the edge node is added, the corresponding interpolation function can be directly represented as the Lagrange polynomial product of a quadratic in  1 (or  2 ) direction and a first degree polynomials in  2 (or  1 ) direction.Thus, the shape function of the fifth adding node can be described as For the previous five nodes, obviously,  5 () satisfies the requirements of the shape function, but node 1 and node 2 cannot meet the requirements as formula (8).In order to make them work properly, Ñ1 , Ñ2 should be modified as follows: where the coefficient 1/2 means the value at the node 5 with Ñ1 , Ñ2 .Similarly, we can add the other new nodes, so all of nodes' interpolation shaper functions can be obtained as follows: In addition,  1 ,  2 ,  3 ,  4 can be simplified as Furthermore, the previous shape functions can be expanded as So the coefficients can be obtained by solving these shape function equations.
In Figure 2  their probability density values are  1 ,  2 , . . .,  8 , so we can get the values of any subunit , , and  as follows: Contrast with the traditional generalized coordinate finite element models, the biquadratic interpolation algorithm is more simple and effective.None else by seeking for the value of the interpolation points, it can build arbitrary quadrilateral element interpolation function with the coordinate system of rectangular unit type.It not only avoids the matrix inverse calculation, the integral of the cell matrix can also be used in a standardized manner in the specification domain in a Bayesian formula.

Construction of Interpolation
Points.Suppose that a Kolmogorov's forward equation is defined as follows: 2 ( (, )  ()   (, ) ⋅ ) we can get Thus, If , ,  are independent of time , so Δ 1 , Δ 2 , Δ 3 are independent of  as well.When  = 2, assume that a two-dimensional vector is defined as  = ( 1 ,  2 ), and the following equation can hold: where ( 2 ()/    ) ≜ (, ).By using the Runge-Kutta method, the probability density can be calculated as where Thus, by substituting   into (20), we can get the probability density of unit point    +1 (  ) at time  +1 in any rectangular area.

Bayesian Estimation. Suppose that 𝑝 𝑡 𝑘+1 ≜ 𝑝 𝑘+1
, so the probability density of subunit area can be formulated as where    represents the probability density value of node  at time  +1 in a rectangular area.
So the prior probability density expression (, ) can be obtained by using formula (22) as Due to the Bayesian formula the likelihood probability density is defined as where ℎ(,   ) is a two-dimensional measurement function,   represents the measurement in a two-dimensional space, and   = ( Var( 1 ) Cov( 1 , 2 ) Cov( 2 , 1 ) Var( 2 ) ) is a covariance matrix of the posterior probability density.
By substituting the prior probability density into Bayesian formula, the posterior probability density can hold as where (,  +1 / +1 ) is a piecewise function as well.
According to the posterior probability density, the conditional mean Ε(/ +1 ) ≜ Ε +1 and the variance (/ +1 ) ≜  +1 of a system can be obtained by the following equations

Convergence Analysis
In this section, we will give the analysis of the proposed estimating method based on FEM.To make the convergence arguments transparent, some special notations are introduced in the process.Assume that Ω = {( 1 ,  2 ) |  ≤  ≤ ,  ≤  ≤ } is a closed and bounded subset of  2 , and (, ) represents any continuous functions in Ω. Suppose that the prior probability density function in each subunit is continuous.For any two adjacent subintervals 2 <   }, the left limit of the interpolation point (  ,   ) will be expressed as lim From the shape function described by formula ( 8), we can get that interpolation point (  ,   ) corresponds to a shape function   .Obviously, the value of shape function is one, while the remaining values are zero, so lim By being calculated with ( 19) and ( 20), the interpolation point    at  +1 can be obtained as ) . ( Then, =    () where Δ 1 , Δ 2 , Δ 3 are only relative to the interpolation point in  and independent with the shape function.Thus, for all ( 1 ,  2 ) ∈ Ω, there exists lim Therefore, lim In the same way, by choosing the subunit By taking (33) and (34), then lim Clearly, (, ) is continuous in Ω.
To analyze the convergence of FEM, we shall give the following proof.Suppose that (, ) represents all of continuous functions in Ω, for all  1 (, ),  2 (, ) ∈ (, ), and it holds that Above all, for any time ,   (, ) is convergent with (, ) uniformly for that (, ) is continuous.For all (, ) ∈ ,  is completeness, so that (, ) is a complete function space, and the prior probability density is convergent.

Computational Complexity Analysis
According to Section 2, we can draw conclusions that for any given area Ω ∈ R 2 in a two-dimensional space, the number of the rectangular area can be calculated as  1 × 2 .Consider that by taking the probability density of subunit area described in (34), it holds that So the computational complexity of the current interpolation point is Thus, the computational complexity of    is ( 1 ×  2 ).Because the bi-quadratic interpolation method does not involve matrix inversion, finally, in a Bayesian process, the computational complexity of Posterior probability density is ( 1 × 2 ) as well.Therefore, the computational complexity of proposed estimating method based on FEM is still ( 1 × 2 ).
In addition, we can find that the computational complexity will be growing with a linear speed by the quantity of grid  1 ×  2 .Compared with Particle Filters, assuming that the number of particles is defined as  in a two-dimensional state space and the recursive call was adopted in the process of particle resampling, so the computational complexity is calculated as (!).The more number of samples required, the higher computational complexity does PF.Therefore, the computational complexity of the new method based on FEM is simpler than PF.

Illustrative Example and Simulations
In this section, consider a reference system in a two-dimensional state space given by [27], which is described as follows: where the covariance of  is diag(10, 0.1), and the covariance of measurement noise    is diag(0.1,1).For any time    , it is a two-dimensional white Gaussian sequence independent of .Here, for FEM, the interval of time is chosen as 1 s, the quantity of grid is chosen as 20, and the initial state is [−0.6 −1].For PF, the number of particles is chosen as 200.
The performance output of FEM is shown in Figures 3(a), 3(b), and 3(c).The conditional density functions are plotted with discrete times.After a few measurements, the predicted density function is bimodal with modes approximately centered at plus and minus the absolute value of the actual state, which is what we expect.Numerical experiment results show that the feasibility of the proposed stochastic estimating method based on FEM is confirmed.
The true state trajectory and the conditional means of the trajectories executed by FEM, UKF, and PF are given in Figures 4(a) and 4(b).Obviously, for a nonlinear non-Gaussian system, UKF estimating output will lose stableness after a few measurement updates.Because the measurement function of UKF should be linearized with the current mean estimate at each measuring step and the second moment of probability distribution of system state also has to be obtained, but the real distribution of probability may not be normal distribution, the actual state is outside of the region where the linearization is valid.The result is that the UKF estimate jumps outside of the region of attraction.Meanwhile, the FEM performs reasonably well by giving the limited information available from the measurements and does not fail because of the nonlinearities in the system and measurement characteristics.
As the real point in these experiments is generated by the random number, every simulation result will be different, therefore; the estimation precision will be measured by using the root mean square error (RMSE) and standard deviation of state, which is defined as follows:  where ,  are the root mean square error and standard deviation, and   ,   are the real position and estimated position.The experiments will be executed 20 times repeatedly.Clearly, we can see that the performance of FEM is better than PF and UKF shown in Table 1.

Conclusion
An FEM-based state estimation approach to nonlinear hybrid positioning systems has been investigated in this paper.It was shown that the new filter can be converged to the true density function in a two-dimensional state space.Furthermore, a detailed demonstration for how to use the FEM efficiently is demonstrated, and with a simple example, we can find that FEM outperforms the UKF and the PF reasonably.It is envisioned that the filter could be used to process data from sensors with severely nonlinear output characteristics, and how to implement FEM algorithm in parallel, especially for solving high dimension problem.These considerations should be further extended in our future work.

Figure 1 :
Figure 1: Flowchart of an estimator based on finite element method.

Figure 3 :
Figure 3: (a) Approximate density function and the true state at the measurement updates at 1 second.(b) Approximate density function and true state after the measurement updates at 12 seconds.(c) Approximate density function and true state after the measurement updates at 28 seconds.

Figure 4 :
Figure 4: (a) Real state and estimated state trajectories by FEM, PF and UKF.(b) The estimated standard deviation of east components by FEM, UKF, and PF.

Table 1 :
The root-mean-square error and standard deviation of the state.