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

Abstract

For hybrid positioning systems (HPSs), the estimator design is a crucial and important problem. In this paper, a finite-element-method- (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.

1. 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 location-based service areas because of the huge commercial market. So studies are rapidly increasing and more practically viable than ever [1, 3–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–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–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.

2. 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 𝑑𝑥_𝑡=𝑓𝑥_𝑡,𝑡𝑑𝑡+𝑔𝑥_𝑡,𝑡𝑑𝛽_𝑡,𝑡≥𝑡₀,𝑧_𝑘=ℎ𝑥_𝑡𝑘,𝑡_𝑘+𝑒_𝑘 (1) is considered, where 𝑥_𝑡∈ℝ^𝑛 is a state vector and 𝑧_𝑘∈ℝ^𝑚 is an output vector. 𝑓(𝑥_𝑡,𝑡)∶ℝ^𝑛×ℝ→ℝ^𝑛 and ℎ(𝑥_𝑡𝑘,𝑡_𝑘)∶ℝ^𝑛×ℝ→ℝ^𝑚 denote system and measurement equations, while 𝑔(𝑥_𝑡,𝑡)∶ℝ^𝑛×ℝ→ℝ^𝑛×𝑑 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 𝐸[𝑒^𝑇_𝑘𝑒_𝑘]=𝑅(𝑘),𝑅(𝑘)∈ℝ^𝑚×𝑚.

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 𝑝(𝑥₀,𝑡₀∣𝑧₀), 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 𝜕𝑝=1𝜕𝑡2⋅𝜕²𝜎²𝑦,𝑡𝑝𝜕𝑦²−𝜕𝜇𝑦,𝑡𝑝.𝜕𝑦 (2) 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 𝐿𝑝=𝑝̇=12^𝑛_𝑟,𝑠=1𝜕²𝑔(𝑥,𝑡)𝑄(𝑡)𝑔^𝑇(𝑥,𝑡)⋅𝑝𝜕𝑥_𝑟𝜕𝑥_𝑠−^𝑛_𝑟=1𝜕𝑓(𝑥,𝑡)⋅𝑝𝜕𝑥_𝑟, (3) where 𝑝≜𝑝(𝑥,𝑡∣𝑧_𝑡) is called the weak solution of the stochastic differential model. Consider 𝑝𝑥₀,𝑡₀∣𝑧₀=𝑝₀. (4) 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 ℝ^𝑛, and 𝑃∶Ω→ℝ is solution of boundary value problem.

3. 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–26].

Figure 1 

Flowchart of an estimator based on finite element method.

For any state vector 𝑥(𝑥_𝑖∈𝑅), the probability density function of state can be calculated as 𝑝𝑡𝑥,𝑧_𝑘∶𝑅^𝑛×𝑡₀,𝑡_𝑒⟶𝑅², (5) with a fixed time 𝑡, where 𝑝(𝑥,𝑡) is a member of the Hilbert space that is consisted with 𝑓∶𝑅^𝑛→𝑅², so it holds that 𝑝𝑡𝑥,𝑧_𝑘≜𝑝(𝑥,𝑡). (6)

3.1. Biquadratic Interpolation Method

In a two-dimensional space, according to the mean value 𝐸_𝑘 and variance 𝜎_𝑘 with the previous moment 𝑡_𝑘, given an interval {(𝑥₁,𝑥₂)∣𝑎≤𝑥≤𝑏,𝑐≤𝑦≤𝑑}, we can get a rectangular area with {(𝑥₁,𝑥₂)∣𝑎_𝑖−1<𝑥₁<𝑎_𝑖,𝑏_𝑗−1<𝑥₂<𝑏_𝑗,𝑖=1,…,𝑛₁,𝑗=1,…,𝑛₂} as shown in Figure 2.

(a)

(b)

Figure 2 

(a) A parent element of FEM. (b) Subunits of FEM.

In Figure 2(a), for any quadrilateral unit, it can be expressed as (𝜉_𝑖,𝜁_𝑖),𝑖=1,…,4, respectively, whose function values are given as 𝑝₁, 𝑝₂, 𝑝₃, and 𝑝₄. Assuming that only four corner nodes are established at the beginning, so the corresponding shape function by Lagrange polynomial can be obtained as 𝑁_𝑖=1+𝜉_𝑖⋅𝑥₁⋅1+𝜁_𝑖⋅𝑥₂4,𝑖=1,…,4. (7) The shape function of each node should satisfy the conditions (8) 𝑁_𝑖𝑗=1𝑖=𝑗,0𝑖≠𝑗, (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 𝑥₁(or𝑥₂) direction and a first degree polynomials in 𝑥₂(or𝑥₁) direction. Thus, the shape function of the fifth adding node can be described as 𝑁₅=1−𝑥²₂⋅1−𝑥₁2. (9)

For the previous five nodes, obviously, 𝑁₅(𝑗) 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, 𝑁₁, 𝑁₂ should be modified as follows: 𝑁₁=𝑁₁−𝑁₅2,𝑁₂=𝑁₂−𝑁₅2, (10) where the coefficient 1/2 means the value at the node 5 with 𝑁₁, 𝑁₂.

Similarly, we can add the other new nodes, so all of nodes’ interpolation shaper functions can be obtained as follows: 𝑁₁=𝑁₁−𝑁₅2−𝑁₈2,𝑁₂=𝑁₂−𝑁₅2−𝑁₆2,𝑁₃=𝑁₃−𝑁₆2−𝑁₇2,𝑁₄=𝑁₄−𝑁₇2−𝑁₈2,𝑁₅=1−𝑥²₂⋅1−𝑥₁2,𝑁₆=1−𝑥²₁⋅1−𝑥₂2,𝑁₇=1−𝑥²₂⋅1+𝑥₁2,𝑁₈=1−𝑥²₁⋅1+𝑥₂2. (11) In addition, 𝑁₁, 𝑁₂, 𝑁₃, 𝑁₄ can be simplified as 𝑁₁=1+𝑥₂⋅1−𝑥₁⋅𝑥₂−𝑥₁−14,𝑁₂=𝑥₂−1⋅1−𝑥₁⋅𝑥₂+𝑥₁+14,𝑁₃=1−𝑥₂⋅1+𝑥₁⋅𝑥₁−𝑥₂−14,𝑁₄=𝑥₂+1⋅1+𝑥₁⋅𝑥₂+𝑥₁−14. (12) Furthermore, the previous shape functions can be expanded as 𝑁_𝑖=𝑎_𝑖1+𝑎_𝑖2⋅𝑥₁+𝑎_𝑖3⋅𝑥₂+𝑎_𝑖4⋅𝑥²₁+𝑎_𝑖5⋅𝑥₁⋅𝑥₂+𝑎_𝑖6⋅𝑥²₂+𝑎_𝑖7⋅𝑥²₁⋅𝑥₂+𝑎_𝑖8⋅𝑥₁⋅𝑥²₂,𝑖=1,…,8. (13) So the coefficients can be obtained by solving these shape function equations.

In Figure 2(b), the subunit coordinates of any quadrilateral are expressed by (𝑥₁,𝑦₁),…,(𝑥₈,𝑦₈), respectively, and their probability density values are 𝑝₁,𝑝₂,…,𝑝₈, so we can get the values of any subunit 𝑝,𝑥, and 𝑦 as follows: 𝑝=⁸_𝑖=1𝑁_𝑖⋅𝑝_𝑖,𝑥=⁸_𝑖=1𝑁_𝑖⋅𝑥_𝑖,𝑦=⁸_𝑖=1𝑁_𝑖⋅𝑦_𝑖 (14)

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.

3.2. Construction of Interpolation Points

Suppose that a Kolmogorov’s forward equation is defined as follows: 𝐿𝑝=𝜕𝑝=1𝜕𝑡2^𝑛_𝑟,𝑠=1𝜕²𝑔(𝑥,𝑡)𝑄(𝑡)𝑔^𝑇(𝑥,𝑡)⋅𝑝𝜕𝑥_𝑟𝜕𝑥_𝑠−^𝑛_𝑟=1𝜕𝑓(𝑥,𝑡)⋅𝑝𝜕𝑥_𝑟. (15) Let 𝑔(𝑥,𝑡)≜𝑔, 𝑄(𝑡)≜𝑄, 𝑓(𝑥,𝑡)≜𝑓, and due to 𝜕𝑓⋅𝑝𝜕𝑥_𝑟=𝜕𝑓𝜕𝑥_𝑟𝜕𝑝⋅𝑝+𝑓⋅𝜕𝑥_𝑟,𝜕𝑔𝑄𝑔^𝑇⋅𝑝𝜕𝑥_𝑠=𝜕𝑔𝑄𝑔^𝑇𝜕𝑥_𝑠⋅𝑝+𝑔𝑄𝑔^𝑇⋅𝜕𝑝𝜕𝑥_𝑠,𝜕²𝑔𝑄𝑔^𝑇⋅𝑝𝜕𝑥_𝑟𝜕𝑥_𝑠=𝜕²𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟𝜕𝑥_𝑠𝜕⋅𝑝+𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟⋅𝜕𝑝𝜕𝑥_𝑠+𝑔𝑄𝑔^𝑇⋅𝜕²𝑝𝜕𝑥_𝑟𝜕𝑥_𝑠+𝜕𝑔𝑄𝑔^𝑇𝜕𝑥_𝑠⋅𝜕𝑝𝜕𝑥_𝑟, (16) we can get 𝐿𝑝(𝑥)=𝜕𝑝=1𝜕𝑡2^𝑛_𝑟,𝑠=1𝜕²𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟𝜕𝑥_𝑠𝜕⋅𝑝+2⋅𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟⋅𝜕𝑝𝜕𝑥_𝑠+𝑔𝑄𝑔^𝑇⋅𝜕²𝑝𝜕𝑥_𝑟𝜕𝑥_𝑠−^𝑛_𝑟=1𝜕𝑓𝜕𝑥_𝑟𝜕𝑝⋅𝑝+𝑓⋅𝜕𝑥_𝑟. (17) Thus, 12^𝑛_𝑟,𝑠=1𝜕²𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟𝜕𝑥_𝑠−^𝑛_𝑟=1𝜕𝑓𝜕𝑥_𝑟≜Δ₁,_𝑛𝑟=1𝜕𝑔𝑄𝑔^𝑇𝜕𝑥_𝑟−𝑓≜Δ₂,𝑔𝑄𝑔^𝑇≜Δ₃. (18) If 𝑔, 𝑄, 𝑓 are independent of time 𝑡, so Δ₁, Δ₂, Δ₃ are independent of 𝑡 as well. When 𝑛=2, assume that a two-dimensional vector is defined as 𝑥=(𝑥₁,𝑥₂), and the following equation can hold: ^𝑡𝑘+1_𝑡𝑘𝐿𝑝(𝑥)=^𝑡𝑘+1_𝑡𝑘𝜕𝑝=𝜕𝑡𝑑𝑡_{𝑡𝑘+1𝑡𝑘}Δ₁𝑝+Δ₂²_𝑖=1𝜕𝑝𝜕𝑥_𝑖+Δ₃²_𝑖,𝑗=1𝜕²𝑝𝜕𝑥_𝑖𝜕𝑥_𝑗𝑑𝑡, (19) where (𝑡_𝑘+1−𝑡_𝑘)≜Δ𝑡, Δ₁𝑝+Δ₂∑²_𝑖=1(𝜕𝑝/𝜕𝑥_𝑖)+Δ₃∑²_𝑖,𝑗=1(𝜕²(𝑝)/𝜕𝑥_𝑖𝜕𝑥_𝑗)≜𝐹(𝑝,𝑡).

By using the Runge-Kutta method, the probability density can be calculated as 𝑝^𝑡𝑘+1(𝑥)=𝑝^𝑡𝑘(𝑥)+Δ𝑡6⋅𝐾₁+2𝐾₂+2𝐾₃+𝐾₄, (20) where 𝐾₁=𝐹𝑝,𝑡_𝑘,𝐾₂=𝐹𝑝,𝑡_𝑘+Δ𝑡2⋅𝐾₁,𝐾₃=𝐹𝑝,𝑡_𝑘+Δ𝑡2⋅𝐾₂,𝐾₄=𝐹𝑝,𝑡_𝑘+Δ𝑡⋅𝐾₃. (21) Thus, by substituting 𝐾_𝑖 into (20), we can get the probability density of unit point 𝑝^𝑡_𝑖𝑖^𝑘+1(𝑋_𝑖𝑗) at time 𝑡_𝑘+1 in any rectangular area.

3.3. Bayesian Estimation

Suppose that 𝑝^𝑡𝑘+1≜𝑝_𝑖^𝑘+1, so the probability density of subunit area can be formulated as 𝑝_𝑖𝑗=⁸_𝑘=1𝑁_𝑘⋅𝑝^𝑘_𝑖𝑗, (22) 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 𝑝(𝑥,𝑡)=^𝑛2𝑛_𝑗=1¹_𝑗=2𝑝_𝑖𝑗. (23)

Due to the Bayesian formula 𝑝𝑡𝑥,^𝑘+1𝑧_𝑘+1=𝑝𝑧_𝑘+1/𝑥𝑝𝑥,𝑡⁻_𝑘+1/𝑧_𝑘∬_Ω𝑝𝑧_𝑘+1/𝑥𝑝𝑥,𝑡⁻_𝑘+1/𝑧_𝑘,𝑑𝑥𝑑𝑦 (24)

the likelihood probability density is defined as 𝑝𝑧_𝑘+1𝑥=exp−(1/2)𝑧_𝑘−ℎ𝑥,𝑡_𝑘^𝑇𝑅_𝑘⁻¹𝑧_𝑘−ℎ𝑥,𝑡_𝑘(2𝜋)^𝑚det𝑅_𝑘, (25) 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 𝑝_𝑖𝑗𝑡𝑥,^𝑘+1𝑧_𝑘+1=𝑝𝑧_𝑘+1/𝑥𝑝(𝑥,𝑡)∬_Ω𝑝𝑧_𝑘+1𝑝()𝑥/𝑥𝑥,𝑡𝑑𝑥𝑑𝑦,𝑥∈_𝑖−1,𝑥_𝑖,𝑦∈𝑦_𝑗−1,𝑦_𝑗𝑖=1,2,⋯,𝑛₁,𝑗=1,2,…,𝑛₂, (26) 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 Ε𝑥𝑧_𝑘+1=_Ω𝑝𝑡𝑥,^𝑘+1𝑧_𝑘+1𝑑𝑥₁𝑑𝑥₂,𝜎𝑥𝑧_𝑘+1=_Ω𝑥𝑥−Ε𝑧_𝑘+1²𝑝𝑡𝑥,^𝑘+1𝑧_𝑘+1𝑑𝑥₁𝑑𝑥₂. (27)

4. 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 Ω={(𝑥₁,𝑥₂)∣𝑎≤𝑥≤𝑏,𝑐≤𝑦≤𝑑} is a closed and bounded subset of 𝑅², and 𝑃(𝑥,𝑡) represents any continuous functions in Ω. Suppose that the prior probability density function in each subunit is continuous. For any two adjacent subintervals {(𝑥₁,𝑥₂)∣𝑎_𝑖−1<𝑥₁<𝑎_𝑖,𝑏_𝑖−1<𝑥₂<𝑏_𝑖}, {(𝑥₁,𝑥₂)∣𝑎_𝑖<𝑥₁<𝑎_𝑖+1,𝑏_𝑗<𝑥₂<𝑏_𝑗+1}, in the first unit {(𝑥₁,𝑥₂)∣𝑎_𝑖−1<𝑥₁<𝑎_𝑖,𝑏_𝑗−1<𝑥₂<𝑏_𝑗}, the left limit of the interpolation point (𝑎_𝑖,𝑏_𝑗) will be expressed as lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}⁸_𝑗=1𝑁_𝑗⋅𝑝^𝑘_𝑖𝑗. (28)

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_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=𝑁_𝑘lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝^𝑘_𝑖𝑗=lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝^𝑘_𝑖𝑗. (29) By being calculated with (19) and (20), the interpolation point 𝑝^𝑘_𝑖𝑗 at 𝑡_𝑘+1 can be obtained as 𝑝^𝑡𝑘+1(𝑥)=𝑝^𝑡𝑘(𝑥)+Δ𝑡6⋅𝐾₁+2𝐾₂+2𝐾₃+𝐾₄=𝑝^𝑡𝑘(𝑥)()+𝐹Δ𝑡+Δ𝑡²3+(Δ𝑡)³6+(Δ𝑡)⁴.24 (30) Then, 𝑝^𝑡𝑘+1(𝑥)=𝑝^𝑡𝑘(𝑥)+Δ𝑡6⋅𝐾₁+2𝐾₂+2𝐾₃+𝐾₄=𝑝^𝑡𝑘(𝑥)()+𝐹Δ𝑡+Δ𝑡²3+(Δ𝑡)³6+(Δ𝑡)⁴24=𝑝^𝑡𝑘(𝑥)+Δ₁𝑝+Δ₂²_𝑖=1𝜕𝑝𝜕𝑥_𝑖+Δ₃²_𝑖,𝑗=1𝜕²𝑝𝜕𝑥_𝑖𝜕𝑥_𝑗×()Δ𝑡+Δ𝑡²3+(Δ𝑡)³6+(Δ𝑡)⁴,24 (31) where Δ₁, Δ₂, Δ₃ are only relative to the interpolation point in 𝐹 and independent with the shape function. Thus, for all (𝑥₁,𝑥₂)∈Ω, there exists lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝^𝑘_𝑖𝑗=𝑝^𝑘_𝑖𝑗. (32) Therefore, lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=𝑝^𝑘_𝑖𝑗. (33)

In the same way, by choosing the subunit {(𝑥₁,𝑥₂)∣𝑎_𝑖−1<𝑥₁<𝑎_𝑖,𝑏_𝑗−1<𝑥₂<𝑏_𝑗}, we can get lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=𝑝^𝑘_𝑖𝑗. (34) By taking (33) and (34), then lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=lim_{𝑥1→𝑎𝑖𝑥2→𝑏𝑗}𝑝(𝑥,𝑡)=𝑝^𝑘_𝑖𝑗. (35) 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 𝑝₁(𝑥,𝑡), 𝑝₂(𝑥,𝑡)∈𝑃(𝑥,𝑡), and it holds that 𝑑𝑝₁,𝑝₂=max_{𝑎≤𝑥1≤𝑏𝑐≤𝑥2≤𝑑}𝑝₁(𝑥,𝑡)−𝑝₂(𝑥,𝑡). (36) Therefore,(i)for all 𝑝₁(𝑥,𝑡),𝑝₂(𝑥,𝑡)∈𝑃(𝑥,𝑡), 𝑑(𝑝₁,𝑝₂)=max_{𝑎≤𝑥1≤𝑏,𝑐≤𝑥2≤𝑑}|𝑝₁(𝑥,𝑡)−𝑝₂(𝑥,𝑡)|≥0, if and only if 𝑝₁=𝑝₂, 𝑑(𝑝₁,𝑝₂)=0;(ii)𝑑(𝑝₁,𝑝₂)=max_{𝑎≤𝑥1≤𝑏,𝑐≤𝑥2≤𝑑}|𝑝₁(𝑥,𝑡)−𝑝₂(𝑥,𝑡)|=max_{𝑎≤𝑥1≤𝑏,𝑐≤𝑥2≤𝑑}|𝑝₂(𝑥,𝑡)−𝑝₁(𝑥,𝑡)|=𝑑(𝑝₂,𝑝₁);(iii)for all 𝑝₁(𝑥,𝑡), 𝑝₂(𝑥,𝑡), 𝑝₃(𝑥,𝑡)∈𝑃(𝑥,𝑡),   𝑑𝑝₁,𝑝₂=max_{𝑎≤𝑥1≤𝑏𝑐≤𝑥2≤𝑑}𝑝₁(𝑥,𝑡)−𝑝₂(𝑥,𝑡)≤max_{𝑎≤𝑥1≤𝑏𝑐≤𝑥3≤𝑑}𝑝₁(𝑥,𝑡)−𝑝₃(𝑥,𝑡)+max_{𝑎≤𝑥3≤𝑏𝑐≤𝑥2≤𝑑}𝑝₃(𝑥,𝑡)−𝑝₂(𝑝𝑥,𝑡)=𝑑₁,𝑝₃+𝑑𝑝₃,𝑝₂. (37)So (𝑃,𝑑) is in the metric spaces.

Assume that 𝑝_𝑛∈𝑃,𝑛=1,2,…, is a Cauchy sequence in 𝑃; for all 𝜀>0, ∃𝑁 is positive integers, and s.t. for all 𝑚,𝑛>𝑁, there exists max_{𝑎≤𝑥1≤𝑏𝑐≤𝑥2≤𝑑}𝑝_𝑚(𝑥,𝑡)−𝑝_𝑛(𝑥,𝑡)=𝑑𝑝_𝑚,𝑝_𝑛<𝜀. (38) So for all {(𝑥₁,𝑥₂)∣𝑎≤𝑥≤𝑏,𝑐≤𝑦≤𝑑}, one has |𝑝_𝑚(𝑥,𝑡)−𝑝_𝑛(𝑥,𝑡)|<𝜀, when 𝑡 is a constant, and 𝑝_𝑛(𝑥,𝑡), 𝑛=1,2,…, is a Cauchy sequence. Thus, ∃𝑝(𝑥,𝑡),𝑝_𝑛(𝑥,𝑡)→𝑝(𝑥,𝑡),𝑛→∞. By taking the formula (38), when 𝑛→∞, 𝑚>𝑁, we can get max_{𝑎≤𝑥1≤𝑏𝑐≤𝑥2≤𝑑}𝑝_𝑚(𝑥,𝑡)−𝑝(𝑥,𝑡)≤𝜀. (39)

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.

5. Computational Complexity Analysis

According to Section 2, we can draw conclusions that for any given area Ω∈ℜ² in a two-dimensional space, the number of the rectangular area can be calculated as 𝑛₁×𝑛₂. Consider that by taking the probability density of subunit area described in (34), it holds that 𝑝_𝑖𝑗=⁸_𝑘=1𝑁_𝑘⋅𝑝^𝑘_𝑖𝑗𝑥,𝑦∣𝑥_𝑖−1<𝑥<𝑥_𝑖,𝑦_𝑗−1<𝑦<𝑦_𝑗 (40) So the computational complexity of the current interpolation point is 𝑂6𝑛₁×𝑛₂+𝑛₁+1×𝑛₂+1+5𝑛₁+1×𝑛₂+1. (41)

Thus, the computational complexity of 𝑝^𝑘_𝑖𝑗 is 𝑂(𝑛₁×𝑛₂). Because the bi-quadratic interpolation method does not involve matrix inversion, finally, in a Bayesian process, the computational complexity of Posterior probability density is 𝑂(𝑛₁×𝑛₂) as well. Therefore, the computational complexity of proposed estimating method based on FEM is still 𝑂(𝑛₁×𝑛₂).

In addition, we can find that the computational complexity will be growing with a linear speed by the quantity of grid 𝑛₁×𝑛₂. 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.

6. 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: 𝑑𝑥=−𝑥₂0.2𝑥²₁−1𝑥₂+𝑥₁𝑧+𝑑𝛽,_𝑡𝑘=𝑥²₁+𝑥²₂𝑥arctan¹𝑥₂+𝑒_𝑡𝑘, (42) 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.

(a)

(b)

(c)

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.

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.

(a)

(b)

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.

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: ∑𝑅=^{𝑆𝑇𝑖=1}∑^𝑡_𝑘=1𝑆_𝑘−𝑌_𝑘²/𝑡,∑𝑆𝑇𝜎=^{𝑆𝑇𝑘=1}∑^𝑡_𝑖=1Ε𝑌_𝑖−Ε𝑌_𝑖²,𝑡⋅𝑆𝑇 (43) 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.

Table 1 

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

7. 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.

Acknowledgments

This work was supported by the National Natural Science Foundation of China (nos. 60904087 and 51109045) and the Fundamental Research Funds for the Central Universities of China (nos. HEUCF110419 and HEUCFX41302).