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

Authors: Yu-Xin Zhao; Li-Juan Chen; Yan Ma
Journal: Mathematical Problems in Engineering
 (2013)

---

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


---

## Body

## 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. Section3 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)dxt=f(xt,t)dt+g(xt,t)dβt,t≥t0,zk=h(xtk,tk)+ek
is considered, where xt∈ℝn is a state vector and zk∈ℝm is an output vector. f(xt,t):ℝn×ℝ→ℝn and h(xtk,tk):ℝn×ℝ→ℝm denote system and measurement equations, while g(xt,t):ℝn×ℝ→ℝn×d is diffusion coefficient. Here βt is d-dimensional Brownian motion vector with the covariance matrix E[dβtdβtT]=Q(t)dt, and ek is an m-dimensional white Gaussian variable independent of dβt with covariance matrix R(k),k≥1, where E[ekTek]=R(k),R(k)∈ℝm×m.LetZk={zk,tk≤t} denote the observations set accumulated up to k, so the problem is evolved into how to seek for the conditional density p(x,t∣zt). Because the statistical information of the system is contained in the measurements zk and the initial condition p(x0,t0∣z0), the conditional mean and variance can be computed from p(x,t∣zt), which generally depend on all of the higher order moments for any nonlinear system. Theoretically, with the observations between time tk and tk+1, the conditional density p≜p(x,t∣zt) diffuses according to Kolmogorov’s forward equation
(2)∂p∂t=12·∂2(σ2(y,t)p)∂y2-∂(μ(y,t)p)∂y.
Usually, a weak solution of stochastic differential equation could be decided by transferring function, so the conditional probability density p(x,t∣zt) of system will satisfy Kolmogorov’s forward equation
(3)L(p)=p˙=12∑r,s=1n∂2(g(x,t)Q(t)gT(x,t)·p)∂xr∂xs-∑r=1n∂(f(x,t)·p)∂xr,
where p≜p(x,t∣zt) is called the weak solution of the stochastic differential model. Consider
(4)p(x0,t0∣z0)=p0.
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 ℝn, and P:Ω→ℝ 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 vectorx(xi∈R), the probability density function of state can be calculated as
(5)p(x,tzk):Rn×[t0,te)⟶R2,
with a fixed time t, where p(x,t) is a member of the Hilbert space that is consisted with f:Rn→R2, so it holds that
(6)p(x,tzk)≜p(x,t).


### 3.1. Biquadratic Interpolation Method

In a two-dimensional space, according to the mean valueEk and variance σk with the previous moment tk, given an interval {(x1,x2)∣a≤x≤b,c≤y≤d}, we can get a rectangular area with {(x1,x2)∣ai-1<x1<ai,bj-1<x2<bj,i=1,…,n1,j=1,…,n2} as shown in Figure 2.(a) A parent element of FEM. (b) Subunits of FEM.
(a)
(b)In Figure2(a), for any quadrilateral unit, it can be expressed as (ξi,ζi),i=1,…,4, respectively, whose function values are given as p1, p2, p3, and p4. Assuming that only four corner nodes are established at the beginning, so the corresponding shape function by Lagrange polynomial can be obtained as
(7)Ni~=(1+ξi·x1)·(1+ζi·x2)4,i=1,…,4.
The shape function of each node should satisfy the conditions (8)
(8)Ni(j)={1i=j,0i≠j,
where j 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 inx1(orx2) direction and a first degree polynomials in x2(orx1) direction. Thus, the shape function of the fifth adding node can be described as
(9)N5=(1-x22)·(1-x1)2.For the previous five nodes, obviously,N5(j) 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, N1~, N2~ should be modified as follows:
(10)N1=N1~-N52,N2=N2~-N52,
where the coefficient 1/2 means the value at the node 5 with N1~, N2~.Similarly, we can add the other new nodes, so all of nodes’ interpolation shaper functions can be obtained as follows:(11)N1=N1~-N52-N82,N2=N2~-N52-N62,N3=N3~-N62-N72,N4=N4~-N72-N82,N5=(1-x22)·(1-x1)2,N6=(1-x12)·(1-x2)2,N7=(1-x22)·(1+x1)2,N8=(1-x12)·(1+x2)2.
In addition, N1, N2, N3, N4 can be simplified as
(12)N1=(1+x2)·(1-x1)·(x2-x1-1)4,N2=(x2-1)·(1-x1)·(x2+x1+1)4,N3=(1-x2)·(1+x1)·(x1-x2-1)4,N4=(x2+1)·(1+x1)·(x2+x1-1)4.
Furthermore, the previous shape functions can be expanded as
(13)Ni=ai1+ai2·x1+ai3·x2+ai4·x12+ai5·x1·x2+ai6·x22+ai7·x12·x2+ai8·x1·x22,i=1,…,8.
So the coefficients can be obtained by solving these shape function equations.In Figure2(b), the subunit coordinates of any quadrilateral are expressed by (x1,y1),…,(x8,y8), respectively, and their probability density values are p1,p2,…,p8, so we can get the values of any subunit p,x, and y as follows:
(14)p=∑i=18Ni·pi,x=∑i=18Ni·xi,y=∑i=18Ni·yiContrast 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:(15)L(p)=∂p∂t=12∑r,s=1n∂2(g(x,t)Q(t)gT(x,t)·p)∂xr∂xs-∑r=1n∂(f(x,t)·p)∂xr.
Let g(x,t)≜g, Q(t)≜Q, f(x,t)≜f, and due to
(16)∂(f·p)∂xr=∂(f)∂xr·p+f·∂(p)∂xr,∂(gQgT·p)∂xs=∂(gQgT)∂xs·p+gQgT·∂(p)∂xs,∂2(gQgT·p)∂xr∂xs=∂2(gQgT)∂xr∂xs·p+∂(gQgT)∂xr·∂(p)∂xs+gQgT·∂2(p)∂xr∂xs+∂(gQgT)∂xs·∂(p)∂xr,
we can get
(17)L(p(x))=∂p∂t=12∑r,s=1n[∑r,s=1n[∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs+gQgT·∂2(p)∂xr∂xs∑r,s=1n[∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs]-∑r=1n[∂(f)∂xr·p+f·∂(p)∂xr].
Thus,
(18)12∑r,s=1n∂2(gQgT)∂xr∂xs-∑r=1n∂(f)∂xr≜Δ1,∑r=1n∂(gQgT)∂xr-f≜Δ2,gQgT≜Δ3.
If g, Q, f are independent of time t, so Δ1, Δ2, Δ3 are independent of t as well. When n=2, assume that a two-dimensional vector is defined as x=(x1,x2), and the following equation can hold:
(19)∫tktk+1L(p(x))=∫tktk+1∂p∂tdt=∫tktk+1(Δ1p+Δ2∑i=12∂p∂xi+Δ3∑i,j=12∂2(p)∂xi∂xj)dt,
where (tk+1-tk)≜Δt, Δ1p+Δ2∑i=12(∂p/∂xi)+Δ3∑i,j=12(∂2(p)/∂xi∂xj)≜F(p,t).By using the Runge-Kutta method, the probability density can be calculated as(20)ptk+1(x)=ptk(x)+Δt6·(K1+2K2+2K3+K4),
where
(21)K1=F(p,tk),K2=F(p,tk)+Δt2·K1,K3=F(p,tk)+Δt2·K2,K4=F(p,tk)+Δt·K3.
Thus, by substituting Ki into (20), we can get the probability density of unit point piitk+1(Xij) at time tk+1 in any rectangular area.


### 3.3. Bayesian Estimation

Suppose thatptk+1≜pik+1, so the probability density of subunit area can be formulated as
(22)pij=∑k=18Nk·pijk,
where pijk represents the probability density value of node k at time tk+1 in a rectangular area.So the prior probability density expressionp(x,t) can be obtained by using formula (22) as
(23)p(x,t)=∑j=1n2∑j=2n1pij.Due to the Bayesian formula(24)p(x,tk+1zk+1)=p(zk+1/x)p(x,tk+1-/zk)∬Ωp(zk+1/x)p(x,tk+1-/zk)dxdy,the likelihood probability density is defined as(25)p(zk+1x)=exp{-(1/2)(zk-h(x,tk))TRk-1(zk-h(x,tk))}(2π)mdetRk,
where h(x,tk) is a two-dimensional measurement function, zk represents the measurement in a two-dimensional space, and Rk=(Var(x1)Cov(x1,x2)Cov(x2,x1)Var(x2)) 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(26)pij(x,tk+1zk+1)=p(zk+1/x)p(x,t)∬Ωp(zk+1/x)p(x,t)dxdy,x∈[xi-1,xi],y∈[yj-1,yj](i=1,2,⋯,n1,j=1,2,…,n2),
where p(x,tk+1/zk+1) is a piecewise function as well.According to the posterior probability density, the conditional meanΕ(x/zk+1)≜Εk+1 and the variance σ(x/zk+1)≜σk+1 of a system can be obtained by the following equations
(27)Ε(xzk+1)=∫∫Ωp(x,tk+1zk+1)dx1dx2,σ(xzk+1)=∬Ω(x-Ε(xzk+1))2p(x,tk+1zk+1)dx1dx2.


## 3.1. Biquadratic Interpolation Method

In a two-dimensional space, according to the mean valueEk and variance σk with the previous moment tk, given an interval {(x1,x2)∣a≤x≤b,c≤y≤d}, we can get a rectangular area with {(x1,x2)∣ai-1<x1<ai,bj-1<x2<bj,i=1,…,n1,j=1,…,n2} as shown in Figure 2.(a) A parent element of FEM. (b) Subunits of FEM.
(a)
(b)In Figure2(a), for any quadrilateral unit, it can be expressed as (ξi,ζi),i=1,…,4, respectively, whose function values are given as p1, p2, p3, and p4. Assuming that only four corner nodes are established at the beginning, so the corresponding shape function by Lagrange polynomial can be obtained as
(7)Ni~=(1+ξi·x1)·(1+ζi·x2)4,i=1,…,4.
The shape function of each node should satisfy the conditions (8)
(8)Ni(j)={1i=j,0i≠j,
where j 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 inx1(orx2) direction and a first degree polynomials in x2(orx1) direction. Thus, the shape function of the fifth adding node can be described as
(9)N5=(1-x22)·(1-x1)2.For the previous five nodes, obviously,N5(j) 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, N1~, N2~ should be modified as follows:
(10)N1=N1~-N52,N2=N2~-N52,
where the coefficient 1/2 means the value at the node 5 with N1~, N2~.Similarly, we can add the other new nodes, so all of nodes’ interpolation shaper functions can be obtained as follows:(11)N1=N1~-N52-N82,N2=N2~-N52-N62,N3=N3~-N62-N72,N4=N4~-N72-N82,N5=(1-x22)·(1-x1)2,N6=(1-x12)·(1-x2)2,N7=(1-x22)·(1+x1)2,N8=(1-x12)·(1+x2)2.
In addition, N1, N2, N3, N4 can be simplified as
(12)N1=(1+x2)·(1-x1)·(x2-x1-1)4,N2=(x2-1)·(1-x1)·(x2+x1+1)4,N3=(1-x2)·(1+x1)·(x1-x2-1)4,N4=(x2+1)·(1+x1)·(x2+x1-1)4.
Furthermore, the previous shape functions can be expanded as
(13)Ni=ai1+ai2·x1+ai3·x2+ai4·x12+ai5·x1·x2+ai6·x22+ai7·x12·x2+ai8·x1·x22,i=1,…,8.
So the coefficients can be obtained by solving these shape function equations.In Figure2(b), the subunit coordinates of any quadrilateral are expressed by (x1,y1),…,(x8,y8), respectively, and their probability density values are p1,p2,…,p8, so we can get the values of any subunit p,x, and y as follows:
(14)p=∑i=18Ni·pi,x=∑i=18Ni·xi,y=∑i=18Ni·yiContrast 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:(15)L(p)=∂p∂t=12∑r,s=1n∂2(g(x,t)Q(t)gT(x,t)·p)∂xr∂xs-∑r=1n∂(f(x,t)·p)∂xr.
Let g(x,t)≜g, Q(t)≜Q, f(x,t)≜f, and due to
(16)∂(f·p)∂xr=∂(f)∂xr·p+f·∂(p)∂xr,∂(gQgT·p)∂xs=∂(gQgT)∂xs·p+gQgT·∂(p)∂xs,∂2(gQgT·p)∂xr∂xs=∂2(gQgT)∂xr∂xs·p+∂(gQgT)∂xr·∂(p)∂xs+gQgT·∂2(p)∂xr∂xs+∂(gQgT)∂xs·∂(p)∂xr,
we can get
(17)L(p(x))=∂p∂t=12∑r,s=1n[∑r,s=1n[∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs+gQgT·∂2(p)∂xr∂xs∑r,s=1n[∂2(gQgT)∂xr∂xs·p+2·∂(gQgT)∂xr·∂(p)∂xs]-∑r=1n[∂(f)∂xr·p+f·∂(p)∂xr].
Thus,
(18)12∑r,s=1n∂2(gQgT)∂xr∂xs-∑r=1n∂(f)∂xr≜Δ1,∑r=1n∂(gQgT)∂xr-f≜Δ2,gQgT≜Δ3.
If g, Q, f are independent of time t, so Δ1, Δ2, Δ3 are independent of t as well. When n=2, assume that a two-dimensional vector is defined as x=(x1,x2), and the following equation can hold:
(19)∫tktk+1L(p(x))=∫tktk+1∂p∂tdt=∫tktk+1(Δ1p+Δ2∑i=12∂p∂xi+Δ3∑i,j=12∂2(p)∂xi∂xj)dt,
where (tk+1-tk)≜Δt, Δ1p+Δ2∑i=12(∂p/∂xi)+Δ3∑i,j=12(∂2(p)/∂xi∂xj)≜F(p,t).By using the Runge-Kutta method, the probability density can be calculated as(20)ptk+1(x)=ptk(x)+Δt6·(K1+2K2+2K3+K4),
where
(21)K1=F(p,tk),K2=F(p,tk)+Δt2·K1,K3=F(p,tk)+Δt2·K2,K4=F(p,tk)+Δt·K3.
Thus, by substituting Ki into (20), we can get the probability density of unit point piitk+1(Xij) at time tk+1 in any rectangular area.


## 3.3. Bayesian Estimation

Suppose thatptk+1≜pik+1, so the probability density of subunit area can be formulated as
(22)pij=∑k=18Nk·pijk,
where pijk represents the probability density value of node k at time tk+1 in a rectangular area.So the prior probability density expressionp(x,t) can be obtained by using formula (22) as
(23)p(x,t)=∑j=1n2∑j=2n1pij.Due to the Bayesian formula(24)p(x,tk+1zk+1)=p(zk+1/x)p(x,tk+1-/zk)∬Ωp(zk+1/x)p(x,tk+1-/zk)dxdy,the likelihood probability density is defined as(25)p(zk+1x)=exp{-(1/2)(zk-h(x,tk))TRk-1(zk-h(x,tk))}(2π)mdetRk,
where h(x,tk) is a two-dimensional measurement function, zk represents the measurement in a two-dimensional space, and Rk=(Var(x1)Cov(x1,x2)Cov(x2,x1)Var(x2)) 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(26)pij(x,tk+1zk+1)=p(zk+1/x)p(x,t)∬Ωp(zk+1/x)p(x,t)dxdy,x∈[xi-1,xi],y∈[yj-1,yj](i=1,2,⋯,n1,j=1,2,…,n2),
where p(x,tk+1/zk+1) is a piecewise function as well.According to the posterior probability density, the conditional meanΕ(x/zk+1)≜Εk+1 and the variance σ(x/zk+1)≜σk+1 of a system can be obtained by the following equations
(27)Ε(xzk+1)=∫∫Ωp(x,tk+1zk+1)dx1dx2,σ(xzk+1)=∬Ω(x-Ε(xzk+1))2p(x,tk+1zk+1)dx1dx2.


## 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Ω={(x1,x2)∣a≤x≤b,c≤y≤d} is a closed and bounded subset of R2, and P(x,t) represents any continuous functions in Ω. Suppose that the prior probability density function in each subunit is continuous. For any two adjacent subintervals {(x1,x2)∣ai-1<x1<ai,bi-1<x2<bi}, {(x1,x2)∣ai<x1<ai+1,bj<x2<bj+1}, in the first unit {(x1,x2)∣ai-1<x1<ai,bj-1<x2<bj}, the left limit of the interpolation point (ai,bj) will be expressed as
(28)limx1→aix2→bj¯p(x,t)=limx1→aix2→bj¯∑j=18Nj·pijk.From the shape function described by formula (8), we can get that interpolation point (ai,bi) corresponds to a shape function Nk. Obviously, the value of shape function is one, while the remaining values are zero, so
(29)limx1→aix2→bj¯p(x,t)=Nklimx1→aix2→bj¯pijk=limx1→aix2→bj¯pijk.
By being calculated with (19) and (20), the interpolation point pijk at tk+1 can be obtained as
(30)ptk+1(x)=ptk(x)+Δt6·(K1+2K2+2K3+K4)=ptk(x)+F(Δt+(Δt)23+(Δt)36+(Δt)424).
Then,
(31)ptk+1(x)=ptk(x)+Δt6·(K1+2K2+2K3+K4)=ptk(x)+F(Δt+(Δt)23+(Δt)36+(Δt)424)=ptk(x)+(Δ1p+Δ2∑i=12∂p∂xi+Δ3∑i,j=12∂2(p)∂xi∂xj)×(Δt+(Δt)23+(Δt)36+(Δt)424),
where Δ1, Δ2, Δ3 are only relative to the interpolation point in F and independent with the shape function. Thus, for all (x1,x2)∈Ω, there exists
(32)limx1→aix2→bjpijk=pijk.
Therefore,
(33)limx1→aix2→bj¯p(x,t)=pijk.In the same way, by choosing the subunit{(x1,x2)∣ai-1<x1<ai,bj-1<x2<bj}, we can get
(34)lim_x1→aix2→bjp(x,t)=pijk.
By taking (33) and (34), then
(35)limx1→aix2→bj¯p(x,t)=lim_x1→aix2→bjp(x,t)=pijk.
Clearly, p(x,t) is continuous in Ω.To analyze the convergence of FEM, we shall give the following proof. Suppose thatP(x,t) represents all of continuous functions in Ω, for all p1(x,t), p2(x,t)∈P(x,t), and it holds that
(36)d(p1,p2)=maxa≤x1≤bc≤x2≤d|p1(x,t)-p2(x,t)|.
Therefore,(i)
for allp1(x,t),p2(x,t)∈P(x,t), d(p1,p2)=maxa≤x1≤b,c≤x2≤d|p1(x,t)-p2(x,t)|≥0, if and only if p1=p2, d(p1,p2)=0;(ii)
d(p1,p2)=maxa≤x1≤b,c≤x2≤d|p1(x,t)-p2(x,t)|=maxa≤x1≤b,c≤x2≤d|p2(x,t)-p1(x,t)|=d(p2,p1);(iii)
for allp1(x,t), p2(x,t), p3(x,t)∈P(x,t),
(37)d(p1,p2)=maxa≤x1≤bc≤x2≤d|p1(x,t)-p2(x,t)|≤maxa≤x1≤bc≤x3≤d|p1(x,t)-p3(x,t)|+maxa≤x3≤bc≤x2≤d|p3(x,t)-p2(x,t)|=d(p1,p3)+d(p3,p2).So (P,d) is in the metric spaces.Assume thatpn∈P,n=1,2,…, is a Cauchy sequence in P; for all ε>0, ∃N is positive integers, and s.t. for all m,n>N, there exists
(38)maxa≤x1≤bc≤x2≤d|pm(x,t)-pn(x,t)|=d(pm,pn)<ε.
So for all {(x1,x2)∣a≤x≤b,c≤y≤d}, one has |pm(x,t)-pn(x,t)|<ε, when t is a constant, and pn(x,t), n=1,2,…, is a Cauchy sequence. Thus, ∃p(x,t),pn(x,t)→p(x,t),n→∞. By taking the formula (38), when n→∞, m>N, we can get
(39)maxa≤x1≤bc≤x2≤d|pm(x,t)-p(x,t)|≤ε.Above all, for any timet, pm(x,t) is convergent with p(x,t) uniformly for that p(x,t) is continuous. For all p(x,t)∈P, P is completeness, so that (P,d) is a complete function space, and the prior probability density is convergent.


## 5. Computational Complexity Analysis

According to Section2, we can draw conclusions that for any given area Ω∈ℜ2 in a two-dimensional space, the number of the rectangular area can be calculated as n1×n2. Consider that by taking the probability density of subunit area described in (34), it holds that
(40)pij=∑k=18Nk·pijk{(x,y)∣xi-1<x<xi,yj-1<y<yj}
So the computational complexity of the current interpolation point is
(41)O(6n1×n2+(n1+1)×(n2+1)+5(n1+1)×(n2+1)).Thus, the computational complexity ofpijk is O(n1×n2). Because the bi-quadratic interpolation method does not involve matrix inversion, finally, in a Bayesian process, the computational complexity of Posterior probability density is O(n1×n2) as well. Therefore, the computational complexity of proposed estimating method based on FEM is still O(n1×n2).In addition, we can find that the computational complexity will be growing with a linear speed by the quantity of gridn1×n2. Compared with Particle Filters, assuming that the number of particles is defined as N 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 O(N!). 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:
(42)dx=[-x20.2(x12-1)x2+x1]+dβ,ztk=(x12+x22arctan(x1x2))+etk,
where the covariance of dβ is diag(10,0.1), and the covariance of measurement noise etk is diag(0.1,1). For any time etk, it is a two-dimensional white Gaussian sequence independent of dβ.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 Figures3(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) 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.
(a)
(b)
(c)The true state trajectory and the conditional means of the trajectories executed by FEM, UKF, and PF are given in Figures4(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) Real state and estimated state trajectories by FEM, PF and UKF. (b) The estimated standard deviation of east components by FEM, UKF, and PF.
(a)
(b)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:(43)R=∑i=1ST(∑k=1t(Sk-Yk)2)/tST,σ=∑k=1ST∑i=1tΕ(Yi-ΕYi)2t·ST,
where R,σ are the root mean square error and standard deviation, and Sk,Yk 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 Table1.Table 1
The root-mean-square error and standard deviation of the state.
Algorithm
R
x
R
y
σ
x
σ
y
FE
0.1817
0.2250
0.0956
0.1194
PF
0.2177
0.2444
0.1296
0.1586
UKF
0.5366
0.4965
0.2857
0.3223


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



---

*Source: 175425-2013-06-12.xml*