Localization based on two-stage treatment for dealing with noisy and biased distance measurements

Abstract

Localization can be performed by trilateration in which the coordinates of a target are calculated by using the coordinates of reference points and the distances between each reference point and the target. Because the distances are measured on the basis of the time-of-flight of various kinds of signals, they contain errors which are the noise and bias. The presence of bias can become a major problem because its magnitude is generally unknown. In this article, we propose an algorithm that combines the Kalman filter (KF) and the least square (LS) algorithm to treat noisy and biased distances measured by chirp spread spectrum ranging defined in IEEE 802.15.4a. By using the KF, we remove the noise in the measured distance; hence, the noise-eliminated distance, which still contains bias, is obtained. The next step consists of the calculation of the target coordinates by using the weighted LS algorithm. This algorithm uses the noise-eliminated distance obtained by using the KF, and the weighting parameters of the algorithm are determined to reduce the effects of bias. To confirm the accuracy of the proposed algorithm, we present the results of indoor localization experiments.

Introduction

Trilateration is a method used to calculate the coordinates of a target[1]. For two-dimensional localization, it requires at least three reference points whose coordinates are known in advance and involves the determination of the distance between the target and the individual reference points. Geometrically, there is only one point of intersection of all the circles whose radius and center coordinates are equal to the distances and the coordinates of the reference points, respectively. The coordinates of the intersection are those of the target.

In practice, since all the distances are mostly measured on the basis of the time-of-flight of various kinds of signals, namely, laser, ultrasonic and electromagnetic signals, these are only approximately equal to the true distance.[2–5]. Furthermore, it is often the case that not all circles intersect with each other because of the errors in the measured distances. As a consequence, the least square (LS) algorithm and the extended Kalman filter (EKF) have been applied to the trilateration method[6–9].

In the vast majority of studies, it is assumed that errors in the measured distance are caused by zero-mean white Gaussian noise. In this case, the LS algorithm and EKF gives the optimal solution[10]. However, in actual situations, the measured distances are not only corrupted by noise but also deteriorated by bias, which is caused by non-line-of-sight and multiple paths of ranging signal[11]. When only zero-mean noise is added to the measured distance, the expected value of the measured distance is equal to the true distance. On the other hand, when bias exists, the expected value will not be equal to the true distance. The combination of noise and bias can be regarded as nonzero mean noise with unknown mean value. Therefore, the bias can result in errors, even if the LS algorithm and EKF are used.

The use of the Kalman filter (KF) for the bias has been extensively studied[12, 13]. In these studies, the bias is regarded as additional states to be estimated and the augmented state vector is defined as the combination of the original states and the bias. The general KF estimates the augmented state vector[13]. The two-stage KF, which consists of a bias-free and a bias filter, separately estimates the augmented state vector[13]. The estimates of the two-stage KF is the same as those of the general KF, but the former has a numerical advantage[13]. Furthermore, a two-stage EKF, which is a KF used for estimating the state vector of nonlinear state space (SS) equations, has been developed[14, 15].

However, the EKF cannot deal with the bias of the trilateration model due to the observability problem. When the SS equations which model the trilateration are unobservable, the estimated value does not converge to a meaningful solution due to lack of measurement information[16]. The observability of the equations can be investigated by using the Fisher information matrix[17, 18]. According to our study the equations are unobservable, and it means that most conventional approaches such as the EKF cannot accurately estimate all of the original states and the biases.

In this article, therefore, we propose an algorithm for treating both noise and bias in distances measured by chirp spread spectrum (CSS) ranging. The CSS is defined in IEEE 802.15.4a as an alternative physical layer, and we can measure distances by the CSS ranging[8, 9, 19–21]. However, the CSS ranging is biased, so a localization algorithm to solve the bias problem has been proposed[9]. The algorithm uses an approximated bias model of CSS ranging, and applies the model to the EKF[9]. Therefore when the model is modified, the equations of the algorithm have to be changed. On the other hand, the proposed algorithm is divided into two stages. In the first stage, the noise in the distance measurement is eliminated by using the KF. In the second stage, the target coordinates are calculated on the basis of the noise-eliminated distances obtained in the first stage. For this calculation, we use a weighted least square (WLS) algorithm and the weighting parameters have an important rule to reducing the effect of bias. Therefore, when the characteristic of the bias is reconsidered, we only change the equation for determining weighting parameters. To confirm the accuracy of the proposed algorithm, we present the results of indoor localization experiments, which are based on CSS ranging.

This article is organized as follows. In Section “Conventional algorithms for trilateration”, we provide an introduction to trilateration and conventional methods used for solving trilateration problems. In Section “Biased distance model and its observability”, we describe the EKF algorithm used for biased measurements. We also argue that the EKF algorithm is not suitable for treating the bias in the case of trilateration because of the observability problem. In Section “Proposed algorithm”, we present the proposed algorithm. In Section “Experimental results”, we present the results of indoor localization experiments based on the CSS ranging. Lastly, in Section “Conclusion”, we conclude our study.

Conventional algorithms for trilateration

Mathematical expression for trilateration

On a 2D-plane, four reference points are symmetrically located as shown in Figure1. The coordinates of reference points 1, 2, 3, and 4 are [A A]^T, [−A A]^T, [−A−A]^T, and [A−A]^Tin vector form, respectively. At the time instant t, when a target is positioned at x(t) = [x₁(t) x₂(t)]^T, the distances between this target and each reference point denoted as y₁(t), y₂(t), y₃(t), and y₄(t) are given by the following expressions:

$$\begin{array}{l}{y}_1\left(t\right)=\sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}+v_1\left(t\right)\end{array}$$

(1)

$$\begin{array}{l}{y}_2\left(t\right)=\sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}+v_2\left(t\right)\end{array}$$

(2)

$$\begin{array}{l}{y}_3\left(t\right)=\sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}+v_3\left(t\right)\end{array}$$

(3)

$$\begin{array}{l}{y}_4\left(t\right)=\sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}+v_4\left(t\right),\end{array}$$

(4)

where v₁(t), v₂(t), v₃(t), and v₄(t) are the noises in the measurements of these four distances which are generally assumed to be zero-mean white Gaussian. The first terms on the right side of each equation correspond to the true distances. These equations can be more simply expressed as a vector form,

$$\mathbf{y}\left(t\right)=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{v}\left(t\right),$$

(5)

where

$$\begin{array}{l}\mathbf{y}\left(t\right)={\left[y_1\left(t\right)y_2\left(t\right)y_3\left(t\right)y_4\left(t\right)\right]}^T\end{array}$$

(6)

Figure 1

Geometrical representation of the two-dimensional vector of the true distance between the target and the four reference points.

$$\begin{array}{l}\mathbf{v}\left(t\right)={\left[v_1\left(t\right)v_2\left(t\right)v_3\left(t\right)v_4\left(t\right)\right]}^T\end{array}$$

(7)

and

$$\mathbf{h}\left(\mathbf{x}\left(t\right)\right)=\left[\begin{array}{c}\sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}\\ \sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}\\ \sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}\\ \sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}\end{array}\right].$$

(8)

Here, y(t), v(t), and h(x(t)) are the vectors of the measured distances, the measurement noises, and the true distances. The true distance vector h(x(t)) is a function of the vector x(t) of the target coordinates.

LS algorithms for trilateration

The ordinary LS algorithm is one of methods for calculating the coordinates of a specific target by using equations of (1)–(4). To transform the equations into linear equations, the terms corresponding to the measurement noises are neglected and the equations are rearranged, then we obtain[7, 22]:

$$\begin{array}{l}{y}_1^2\left(t\right)=x_1^2\left(t\right)+x_2^2\left(t\right)-2A{x}_1\left(t\right)-2A{x}_2\left(t\right)+2A^2\end{array}$$

(9)

$$\begin{array}{l}{y}_2^2\left(t\right)=x_1^2\left(t\right)+x_2^2\left(t\right)+2A{x}_1\left(t\right)-2A{x}_2\left(t\right)+2A^2\end{array}$$

(10)

$$\begin{array}{l}{y}_3^2\left(t\right)=x_1^2\left(t\right)+x_2^2\left(t\right)+2A{x}_1\left(t\right)+2A{x}_2\left(t\right)+2A^2\end{array}$$

(11)

$$\begin{array}{l}{y}_4^2\left(t\right)=x_1^2\left(t\right)+x_2^2\left(t\right)-2A{x}_1\left(t\right)+2A{x}_2\left(t\right)+2A^2.\end{array}$$

(12)

After subtracting (9)–(11) from (12), we obtain:

$$\begin{array}{l}{y}_1^2\left(t\right)-y_4^2\left(t\right)\\ =-2A{x}_1\left(t\right)+2A{x}_1\left(t\right)-2A{x}_2\left(t\right)-2A{x}_2\left(t\right)\end{array}$$

(13)

$$\begin{array}{l}{y}_2^2\left(t\right)-y_4^2\left(t\right)\\ =2A{x}_1\left(t\right)+2A{x}_1\left(t\right)-2A{x}_2\left(t\right)-2A{x}_2\left(t\right)\end{array}$$

(14)

$$\begin{array}{l}{y}_3^2\left(t\right)-y_4^2\left(t\right)\\ =2A{x}_1\left(t\right)+2A{x}_1\left(t\right)+2A{x}_2\left(t\right)-2A{x}_2\left(t\right).\end{array}$$

(15)

These equations can be rewritten in matrix and vector form as follows:

$$\mathbf{\Phi }\mathbf{x}\left(t\right)=\theta \left(t\right),$$

(16)

where

$$\mathbf{\Phi }=\left[\begin{array}{cc}0& -4A\\ 4A& -4A\\ 4A& 0\end{array}\right]$$

(17)

and

$$\theta \left(t\right)=\left[\begin{array}{l}{y}_1^2\left(t\right)-y_4^2\left(t\right)\\ y_2^2\left(t\right)-y_4^2\left(t\right)\\ y_3^2\left(t\right)-y_4^2\left(t\right)\end{array}\right].$$

(18)

The exact solution of the linear Equation (16), i.e. x(t) does not exist because of the presence of noise. Instead of an exact solution, the LS solution$\widehat{\mathbf{x}}\left(t\right)$ is calculated by using the ordinary LS algorithm[7, 22]:

$$\widehat{\mathbf{x}}\left(t\right)={\left({\mathbf{\Phi }}^T\mathbf{\Phi }\right)}^{-1}{\mathbf{\Phi }}^T\theta \left(t\right).$$

(19)

As a different approach, the nonlinear LS algorithm can be utilized. The nonlinear LS algorithm does not need the rearrangement of the Equation (5) and is based on the following minimization criterion[23]:

$$\widehat{\mathbf{x}}\left(t\right)=\text{arg}\underset{\mathbf{x}\left(t\right)}{\text{min}}{\mathbf{r}}^T\left(\mathbf{x}\left(t\right)\right)\mathbf{r}\left(\mathbf{x}\left(t\right)\right),$$

(20)

where r(x(t)) = y(t)−h(x(t))which is the residual vector. To obtain the solution that satisfies this minimization criterion, the nonlinear LS problem needs to be solved using the Gauss–Newton method, which is a recursive algorithm used to solve nonlinear LS problems[23]. The Gauss–Newton method is expressed based on the given vector y(t) of the measured distances and the initial value of${\widehat{\mathbf{x}}}_0\left(t\right)$,

$${\widehat{\mathbf{x}}}_i\left(t\right)={\widehat{\mathbf{x}}}_{i-1}\left(t\right)+\alpha {\mathbf{p}}_i\left(t\right)\mathbf{r}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right),$$

(21)

where

$$\begin{array}{l}\mathbf{r}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right)=\mathbf{y}\left(t\right)-\mathbf{h}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right)\end{array}$$

(22)

$$\begin{array}{l}{\mathbf{p}}_i\left(t\right)=-{\left({\mathbf{J}}_i^T\left(t\right){\mathbf{J}}_i\left(t\right)\right)}^{-1}{\mathbf{J}}_i^T\left(t\right),\end{array}$$

(23)

and α is step-size and meets the condition 0 < α ≤ 1. The vector${\widehat{\mathbf{x}}}_i\left(t\right)$ is the nonlinear LS solution of x(t) at i-th iteration. Here, the matrix J _i (t) is the Jacobian matrix of h(x) defined as

$${\mathbf{J}}_i\left(t\right)=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{|}_{\mathbf{x}={\widehat{\mathbf{x}}}_{i-1}\left(t\right)}.$$

(24)

EKF for trilateration

The EKF is a state estimator of nonlinear SS equations. When the measured distance vector y(t) is successively measured over time, the EKF can be applied to solve the trilateration problem. For the EKF, the nonlinear SS equations for trilateration model are defined as follows:

$$\begin{array}{l}\mathbf{x}(t+1)=\mathbf{x}\left(t\right)\end{array}$$

(25)

$$\begin{array}{l}\mathbf{y}\left(t\right)=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{v}\left(t\right),\end{array}$$

(26)

The measurement Equation (26) is the same as (5). In this article, we deal with two-dimensional localization of a fixed object. As shown in the process Equation (25), the coordinate vector x(t) is not changed over time which means that the target object is stationary. Also, in the SS equations, the coordinate vector is called the state vector. The correlation matrix of v(t) is defined as R(t) and is assumed as known. With initial values of$\widehat{\mathbf{x}}\left(0\right)$ and P(0), the EKF is given by the following equations:

$$\begin{array}{l}\mathbf{e}\left(t\right)=\mathbf{y}\left(t\right)-\mathbf{h}\left(\widehat{\mathbf{x}}(t-1)\right)\end{array}$$

(27)

$$\begin{array}{l}\mathbf{K}\left(t\right)=\mathbf{P}(t-1){\mathbf{H}}^T\left(t\right){\left[\mathbf{H}\left(t\right)\mathbf{P}(t-1){\mathbf{H}}^T\left(t\right)+\mathbf{R}\left(t\right)\right]}^{-1}\end{array}$$

(28)

$$\begin{array}{l}\widehat{\mathbf{x}}\left(t\right)=\widehat{\mathbf{x}}(t-1)+\mathbf{K}\left(t\right)\mathbf{e}\left(t\right)\end{array}$$

(29)

$$\begin{array}{l}\mathbf{P}\left(t\right)=\left[\mathbf{I}-\mathbf{K}\left(t\right)\mathbf{H}\left(t\right)\right]\mathbf{P}(t-1),\end{array}$$

(30)

where

$$\mathbf{H}\left(t\right)=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{|}_{\mathbf{x}=\widehat{\mathbf{x}}(t-1)}.$$

(31)

Here, I is the identity matrix with appropriate dimensions, and P(t) is the estimation error covariance matrix. The estimated state vector at time instant t is denoted as$\widehat{\mathbf{x}}\left(t\right)$ that is the estimator based on the measured distance vectors y(1),y(2),…,y(t). With the application of the EKF algorithm, a new y(t) is continuously required at every time instant t, whereas the LS algorithms require a measured distance vector y(t) for obtaining a LS solution.

Biased distance model and its observability

Biased model

In (5), the noises are assumed as the zero-mean, but they are not always zero-mean especially in actual situations. In this article, the nonzero-mean value of the noises are denoted as the bias. The biased model is expressed as follows:

$$\mathbf{y}\left(t\right)=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{b}\left(t\right)+\mathbf{v}\left(t\right),$$

(32)

where b(t) = b₁(i)b₂(i)b₃(i)b₄(i)]^T is the bias vector that is added to the vector of the true distances similarly to the vector of the measurement noise. However the bias vector is a deterministic error source, whereas the noise vector is stochastic[10]. Also the bias can be assumed as constant, because the variation of the bias is much slower than the random variation of the noise[10]. If the bias vector is known in advance, the effect caused by the bias vector can be easily eliminated by subtracting the bias vector from the measured distance vector. However the bias vector is unknown in practical situations, so it becomes a major problem.

EKF for biased distance model

In this section, we describe the SS equations of the biased distances. First, we can rewrite the nonlinear SS equations of trilateration with the biased distance measurements (32) as:

$$\begin{array}{l}\mathbf{x}(t+1)=\mathbf{x}\left(t\right)\end{array}$$

(33)

$$\begin{array}{l}\mathbf{y}\left(t\right)=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{b}\left(t\right)+\mathbf{v}\left(t\right).\end{array}$$

(34)

As the bias vector must be estimated in order to eliminate its effects, an augmented state vector is defined as follows:

$$\mathbf{z}\left(t\right)={\left[\mathbf{x}{\left(t\right)}^T\mathbf{b}{\left(t\right)}^T\right]}^T.$$

(35)

In this definition, the biases are regarded as states, as well as the original state vector x(t). Subsequently, the nonlinear SS equations can be rewritten using z(t) as:

$$\begin{array}{l}\mathbf{z}(t+1)=\mathbf{z}\left(t\right)\end{array}$$

(36)

$$\begin{array}{l}\mathbf{y}\left(t\right)=\mathbf{g}\left(\mathbf{z}\left(t\right)\right)+\mathbf{v}\left(t\right),\end{array}$$

(37)

where

$$\begin{array}{l}\mathbf{g}\left(\mathbf{z}\left(t\right)\right)=\left[\begin{array}{c}\sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}+b_1\left(t\right)\\ \sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)-A\right)}^2}+b_2\left(t\right)\\ \sqrt{{\left(x_1\left(t\right)+A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}+b_3\left(t\right)\\ \sqrt{{\left(x_1\left(t\right)-A\right)}^2+{\left(x_2\left(t\right)+A\right)}^2}+b_4\left(t\right)\end{array}\right]\end{array}$$

(38)

$$\begin{array}{l}=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{b}\left(t\right).\end{array}$$

(39)

As mentioned, the bias vector b(t) is assumed as constant, so we can express that b(t + 1) = b(t). Therefore the augmented state vector z(t) is not changed over time in (36). Also, g(z(t)) is defined as the summation of the true distance vector and the bias vector, and is referred to as the biased true distance vector.

In general, the augmented state vector can be estimated using various algorithms including the two-stage EKF algorithms[14, 15]. However, these approaches are not applicable in case of trilateration because of the observability problem.

Observability of biased distance model

When SS equations are unobservable, the meaningful estimation cannot be derived due to lack of measurement information[16]. In this section, we will investigate the observability of the SS Equations (36) and (37) by using the Fisher information matrix (FIM). In the SS equations, which are the trilateration model with biases, only y(t) is the given measurement information available for estimating z(t), because the process Equation (36) does not contain any measurement information. Therefore, based on (37), the FIM of the trilateration model is given by[18]

$$\mathbf{F}={\nabla }_{\mathbf{z}}^T\mathbf{g}\left(\mathbf{z}\right){\mathbf{R}}^{-1}{\nabla }_{\mathbf{z}}\mathbf{g}\left(\mathbf{z}\right),$$

(40)

where

$${\nabla }_{\mathbf{z}}\mathbf{g}\left(\mathbf{z}\right)=\frac{\partial \mathbf{g}\left(\mathbf{z}\right)}{\partial \mathbf{z}}={\left[\frac{\partial g_j\left(\mathbf{z}\right)}{\partial z_k}\right]}_{j,k}$$

(41)

for 1 ≤ j ≤ 4 and 1 ≤ k ≤ 6. Here, the FIM is denoted as F, and the covariance matrix of the measurement noise v(t) is written as R. For convenience, we abbreviate the time instant t such as z(t)= z and R(t) = R. The inverse of the FIM means the Cramér-Rao lower bound of any unbiased estimator[17]. Further if the FIM is singular, i.e. the FIM is not invertible, the model is unobservable[18].

Based on this knowledge we can judge that the SS model is unobservable if F is singular matrix. To test the singularity of F, we investigate the rank of F. If the rank of F is not full rank, the matrix is not invertible that means the matrix is singular. Through (35) and (38), we can know that

$$\begin{array}{l}\mathbf{z}={\left[z_1{z}_2{z}_3{z}_4{z}_5{z}_6\right]}^T\end{array}$$

(42)

$$\begin{array}{l}={\left[x_1{x}_2{b}_1{b}_2{b}_3{b}_4\right]}^T\end{array}$$

(43)

and

$$\begin{array}{l}{g}_1\left(\mathbf{z}\right)=\sqrt{{\left(x_1-A_{11}\right)}^2+{\left(x_2-A_{12}\right)}^2}+b_1\end{array}$$

(44)

$$\begin{array}{l}{g}_2\left(\mathbf{z}\right)=\sqrt{{\left(x_1-A_{21}\right)}^2+{\left(x_2-A_{22}\right)}^2}+b_2\end{array}$$

(45)

$$\begin{array}{l}{g}_3\left(\mathbf{z}\right)=\sqrt{{\left(x_1-A_{31}\right)}^2+{\left(x_2-A_{32}\right)}^2}+b_3\end{array}$$

(46)

$$\begin{array}{l}{g}_4\left(\mathbf{z}\right)=\sqrt{{\left(x_1-A_{41}\right)}^2+{\left(x_2-A_{42}\right)}^2}+b_4.\end{array}$$

(47)

Here, we use non-symmetrical reference points for the generality of our derivation. The coordinates of them are denoted as A _pq for 1 ≤ p ≤ 4 and 1 ≤ q ≤ 2. By the definitions, ∇ _z g(z) is given by

$$\begin{array}{l}{\nabla }_{\mathbf{z}}\mathbf{g}\left(\mathbf{z}\right)=\frac{\partial \mathbf{g}\left(\mathbf{z}\right)}{\partial \mathbf{z}}\end{array}$$

(48)

$$\begin{array}{l}=\left[\begin{array}{llllll}\frac{x_1-A_{11}}{{\Sigma }_1}& \frac{x_2-A_{21}}{{\Sigma }_1}& 1& 0& 0& 0\\ \frac{x_1-A_{21}}{{\Sigma }_2}& \frac{x_2-A_{22}}{{\Sigma }_2}& 0& 1& 0& 0\\ \frac{x_1-A_{31}}{{\Sigma }_3}& \frac{x_2-A_{32}}{{\Sigma }_3}& 0& 0& 1& 0\\ \frac{x_1-A_{41}}{{\Sigma }_4}& \frac{x_2-A_{42}}{{\Sigma }_4}& 0& 0& 0& 1\end{array}\right]\end{array}$$

(49)

$$\begin{array}{l}=\left[\begin{array}{llllll}{a}_{11}& a_{12}& 1& 0& 0& 0\\ a_{21}& a_{22}& 0& 1& 0& 0\\ a_{31}& a_{32}& 0& 0& 1& 0\\ a_{41}& a_{42}& 0& 0& 0& 1\end{array}\right],\end{array}$$

(50)

where

$$\begin{array}{l}{\Sigma }_1=\sqrt{{\left(x_1-A_{11}\right)}^2+{\left(x_2-A_{12}\right)}^2}\end{array}$$

(51)

$$\begin{array}{l}{\Sigma }_2=\sqrt{{\left(x_1-A_{21}\right)}^2+{\left(x_2-A_{22}\right)}^2}\end{array}$$

(52)

$$\begin{array}{l}{\Sigma }_3=\sqrt{{\left(x_1-A_{31}\right)}^2+{\left(x_2-A_{32}\right)}^2}\end{array}$$

(53)

$$\begin{array}{l}{\Sigma }_4=\sqrt{{\left(x_1-A_{41}\right)}^2+{\left(x_2-A_{42}\right)}^2}.\end{array}$$

(54)

In (50), the parameters a _mn for m = 1,2,3,4 and n = 1,2 are used for simplifying (49). If we suppose$\mathbf{R}={\sigma }_v^2\mathbf{I}$ under the assumption that v(t) is white, the FIM is expressed as (55).

$$\mathbf{F}={\sigma }_v^{-2}\left[\begin{array}{cccccc}{a}_{11}^2+a_{21}^2+a_{31}^2+a_{41}^2& a_{11}{a}_{12}+a_{21}{a}_{22}+a_{31}{a}_{32}+a_{41}{a}_{42}& a_{11}& a_{21}& a_{31}& a_{41}\\ a_{11}{a}_{12}+a_{21}{a}_{22}+a_{31}{a}_{32}+a_{41}{a}_{42}& a_{12}^2+a_{22}^2+a_{32}^2+a_{42}^2& a_{12}& a_{22}& a_{32}& a_{42}\\ a_{11}& a_{12}& 1& 0& 0& 0\\ a_{21}& a_{22}& 0& 1& 0& 0\\ a_{31}& a_{32}& 0& 0& 1& 0\\ a_{41}& a_{42}& 0& 0& 0& 1\end{array}\right].$$

(55)

Subsequently, we execute the following row reductions:

• (1) Multiply each of 3, 4, 5, and 6-th rows of F by a ₁₁, a ₂₁, a ₃₁, and a ₄₁, respectively, and then subtract each of them from the first row.

• (2) Multiply each of 3, 4, 5, and 6-th rows of F by a ₁₂, a ₂₂, a ₃₂, and a ₄₂, respectively, and then subtract each of them from the second row.

Through these successive row reductions, we can obtain the row-reduced matrix (56) and know that the rank of F is at most 4 regardless of z. This means that F is not full rank matrix, because the dimension of F is 6×6. Hence F is singular and the SS model is unobservable[18]. To conclude, we cannot guarantee the accuracy of the estimated state vector by the EKF.

$$\text{Row-reduced}\mathbf{F}={\sigma }_v^{-2}\left[\begin{array}{llllll}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ a_{11}& a_{12}& 1& 0& 0& 0\\ a_{21}& a_{22}& 0& 1& 0& 0\\ a_{31}& a_{32}& 0& 0& 1& 0\\ a_{41}& a_{42}& 0& 0& 0& 1\end{array}\right].$$

(56)

Proposed algorithm

Concept of separation for treatments of noise and bias

In this section, we explain the causes of deterioration of the distance measurements, namely of the noise and bias. The noise is generally assumed to be zero-mean white Gaussian, and the bias is an unknown quantity which remains approximately constant. However, both bias and noise cannot be treated by using only the EKF because of because of the observability problem. Therefore, in this article, we algorithm which combines the KF with the WLS algorithm for estimating the position of a specific target when the distance measurements are noisy and biased.

The proposed method is divided into two stages: noise elimination and bias reduction. In the first stage, the noise in the distance measurements is removed with the use of the KF, and then noise-eliminated distances are obtained. In the second stage, the coordinates of the target are calculated by using the WLS algorithm. For this purpose, we use the noise-eliminated distances obtained in the first stage. Subsequently, we propose a method to derive the weighting parameters for the WLS algorithm. As the noise has already been removed in the first stage, these parameters are determined based only on the characteristics of the bias. It should be noted that the weighting parameters affect the estimation of the target coordinates with the use of the WLS algorithm.

In the following sections, we explain the proposed algorithm in three steps: noise elimination, target coordinates calculation, and determination of the weighting parameters.

Measurement noise elimination by KF

To remove the noise from the distance measurements, we define new SS equations as

$$\begin{array}{l}\mathbf{g}(t+1)=\mathbf{g}\left(t\right)\end{array}$$

(57)

$$\begin{array}{l}\mathbf{y}\left(t\right)=\mathbf{g}\left(t\right)+\mathbf{v}\left(t\right),\end{array}$$

(58)

where g(t) is equal to the biased true distance vector g(z(t)) in (37). In this SS equations, however, we regard g(t) as a state vector, because the purpose of the first stage is not estimating z(t), but is estimating the biased true distance vector g(t) itself. Also, we can know that the expression of constant g(t) in (57) is valid, since g(t) only depend on z(t) which is constant as shown in (36).

The SS Equations (57) and (58) are linear, so estimating g(t) is readily executed by the KF, and not by the EKF. The KF as a constant parameter estimator is ideal for the noise elimination, because the KF is optimal estimator and robust against the noise. With initial value of$\widehat{\mathbf{g}}\left(0\right)$ and P(0), the KF for estimating g(t) are given by the following equations:

$$\begin{array}{l}\mathbf{e}\left(t\right)=\mathbf{y}\left(t\right)-\widehat{\mathbf{g}}(t-1)\end{array}$$

(59)

$$\begin{array}{l}\mathbf{K}\left(t\right)=\mathbf{P}(t-1){\left[\mathbf{P}(t-1)+\mathbf{R}\left(t\right)\right]}^{-1}\end{array}$$

(60)

$$\begin{array}{l}\widehat{\mathbf{g}}\left(t\right)=\widehat{\mathbf{g}}(t-1)+\mathbf{K}\left(t\right)\mathbf{e}\left(t\right)\end{array}$$

(61)

$$\begin{array}{l}\mathbf{P}\left(t\right)=\left[\mathbf{I}-\mathbf{K}\left(t\right)\right]\mathbf{P}(t-1),\end{array}$$

(62)

where$\widehat{\mathbf{g}}\left(t\right)$ is the estimated biased true distance vector. In other word,$\widehat{\mathbf{g}}\left(t\right)$ is the estimated noise-eliminated distance vector. If we have no a priori information on initialization, P(0) can be set to infinite matrix. In this case, the KF becomes[24]

$$\widehat{\mathbf{g}}\left(t\right)=\widehat{\mathbf{g}}(t-1)+\frac{1}{t}\left(\mathbf{y}\left(t\right)-\widehat{\mathbf{g}}(t-1)\right).$$

(63)

Equation (63) is the same as the recursive average, and$\widehat{\mathbf{g}}\left(t\right)$ can be estimated by using (63). The vector$\widehat{\mathbf{g}}\left(t\right)$ is used for the WLS algorithm in the next stage of calculating the coordinates of a specific target.

Target coordinate calculation by using nonlinear WLS algorithm

In the second stage, the coordinates of a target are calculated based on the noise-eliminated distances obtained with the use of the KF in the first stage. Based on the relationship in (39), the nonlinear equation for the second stage is given by

$$\mathbf{g}\left(t\right)=\mathbf{h}\left(\mathbf{x}\left(t\right)\right)+\mathbf{b}\left(t\right)$$

(64)

where b(t) is regarded as the error source. In (64), the true value of g(t) is actually unknown. Instead of using g(t), therefore, the estimated value obtained in the first stage is used, and then we define the residual vector as

$$\mathbf{r}\left(\mathbf{x}\left(t\right)\right)=\widehat{\mathbf{g}}\left(t\right)-\mathbf{h}\left(\mathbf{x}\left(t\right)\right).$$

(65)

By using (65), the criterion of the WLS algorithm is given by

$$\widehat{\mathbf{x}}\left(t\right)=\arg \underset{\mathbf{x}\left(t\right)}{\text{min}}{\mathbf{r}}^T\left(\mathbf{x}\left(t\right)\right)\mathbf{W}\left(t\right)\mathbf{r}\left(\mathbf{x}\left(t\right)\right),$$

(66)

where W(t) is the matrix of the weighting parameters or differently, the weight matrix. Unlike the conventional LS problem, the bias vector is not a zero-mean noise, so the weight matrix must be selected differently. We will consider the assignment of the weight matrix in the following section.

The solution of (66) is obtained by using the Gauss–Newton method which has already been mentioned in Section “Conventional algorithms for trilateration”. With the given noise-eliminated distance vector$\widehat{\mathbf{g}}\left(t\right)$ and initial value of${\widehat{\mathbf{x}}}_0\left(t\right)$, the algorithm is expressed as the following recursive equations:

$${\widehat{\mathbf{x}}}_i\left(t\right)={\widehat{\mathbf{x}}}_{i-1}\left(t\right)+\alpha {\mathbf{p}}_i\left(t\right)\mathbf{r}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right),$$

(67)

where

$$\begin{array}{l}\mathbf{r}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right)=\widehat{\mathbf{g}}\left(t\right)-\mathbf{h}\left({\widehat{\mathbf{x}}}_{i-1}\left(t\right)\right)\end{array}$$

(68)

$$\begin{array}{l}{\mathbf{p}}_i\left(t\right)=-{\left({\mathbf{J}}_i^T\left(t\right)\mathbf{W}\left(t\right){\mathbf{J}}_i\left(t\right)\right)}^{-1}{\mathbf{J}}_i^T\left(t\right)\mathbf{W}\left(t\right),\end{array}$$

(69)

0 < α ≤ 1, and ${\widehat{\mathbf{x}}}_i\left(t\right)$ is the nonlinear LS solution of x(t) at i-th iteration. Here, the matrix J _i (t) is Jacobian matrix of h, i.e.

$${\mathbf{J}}_i\left(t\right)=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{}_{\mathbf{x}={\widehat{\mathbf{x}}}_{i-1}\left(t\right)}.$$

(70)

While the Gauss–Newton algorithm is running, the new$\widehat{\mathbf{g}}\left(t\right)$ is being recursively estimated by the KF. In other words, while the Gauss–Newton algorithm is recursively updating${\widehat{\mathbf{x}}}_i\left(t\right)$, the previous noise-eliminated distance vector$\widehat{\mathbf{g}}\left(t\right)$ is continuously replaced by a new one, i.e.$\widehat{\mathbf{g}}(t+1)$. Replacing$\widehat{\mathbf{g}}\left(t\right)$ to$\widehat{\mathbf{g}}(t+1)$ can alter the minimum point of (66), so the Gauss–Newton algorithm tries to track the new minimum point, which is x(t + 1). Therefore the coordinates estimated by the Gauss–Newton algorithm will also vary as a result of the changes that the noise-eliminated distance vector undergoes. However, based on the assumption of the bias remains approximately constant, we can argue that the noise-eliminated distance vector which is estimated by the KF is not changing rapidly. Even though the bias may change slightly, it varies much slower than the Gauss–Newton algorithm converges. This implies that the Gauss–Newton algorithm is not required to converge fast. According to this observation, the Gauss–Newton algorithm can be synchronized with the iteration of the KF. Consequently, we remove the number of iterations i in the Gauss–Newton algorithm and rewrite the algorithm recursive with respect to the time instant t. Hence, with the given noise-eliminated distance vector by the first stage and initial value of$\widehat{\mathbf{x}}\left(0\right)$, the second stage of the proposed algorithm is expressed as

$$\widehat{\mathbf{x}}\left(t\right)=\widehat{\mathbf{x}}(t-1)+\alpha \mathbf{p}\left(t\right)\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right),$$

(71)

where

$$\begin{array}{l}\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)=\widehat{\mathbf{g}}\left(t\right)-\mathbf{h}\left(\widehat{\mathbf{x}}(t-1)\right)\end{array}$$

(72)

$$\begin{array}{l}\mathbf{p}\left(t\right)=-{\left({\mathbf{J}}^T\left(t\right)\mathbf{W}\left(t\right)\mathbf{J}\left(t\right)\right)}^{-1}{\mathbf{J}}^T\left(t\right)\mathbf{W}\left(t\right),\end{array}$$

(73)

0 < α ≤ 1, and $\widehat{\mathbf{x}}\left(t\right)$ is the nonlinear LS solution at time instant t. Here, the matrix J(t) is Jacobian matrix of h,

$$\mathbf{J}\left(t\right)=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{}_{\mathbf{x}=\widehat{\mathbf{x}}(t-1)}.$$

(74)

Determining weight matrix

Unlike the conventional LS problem, the bias vector is not zero-mean noise. Therefore, we must derive a proper weight matrix W for the bias vector, when (64) is applied to the WLS algorithm.

First, we review the weight matrix for the conventional LS problem. Let image the linear equation

$$\mathbf{y}=\mathbf{\Phi }\mathbf{x}+\mathbf{v}.$$

(75)

In the conventional LS algorithm, the cause of error is the zero-mean white Gaussian noise v. In this case, according to the WLS algorithm theory, the correlation matrix of the estimation error is expressed as[25]

$$\begin{array}{l}E\left(\widehat{\mathbf{x}}-\mathbf{x}\right){\left(\widehat{\mathbf{x}}-\mathbf{x}\right)}^T\end{array}$$

(76)

$$\begin{array}{l}={\left({\mathbf{\Phi }}^T\mathbf{W}\mathbf{\Phi }\right)}^{-1}{\mathbf{\Phi }}^T\mathbf{W}{\mathbf{R}}_v\mathbf{W}\mathbf{\Phi }{\left({\mathbf{\Phi }}^T\mathbf{W}\mathbf{\Phi }\right)}^{-1},\end{array}$$

(77)

where

$${\mathbf{R}}_v=E\mathbf{v}{\mathbf{v}}^T.$$

(78)

When$\mathbf{W}={\mathbf{R}}_v^{-1}$, the correlation matrix is minimized, i.e.[25]

$$E\left(\widehat{\mathbf{x}}-\mathbf{x}\right){\left(\widehat{\mathbf{x}}-\mathbf{x}\right)}^T={\left({\mathbf{\Phi }}^T\mathbf{W}\mathbf{\Phi }\right)}^{-1}.$$

(79)

In (64), however, the cause of error is not noise, but bias. In light of this, the weight matrix cannot be determined similarly to other conventional algorithms. It must be assigned according to the property of bias. By using the first-order approximation of the Taylor series, (64) is linearized around x^∗ as follows:

$$\mathbf{g}\left(t\right)\approx \mathbf{h}\left({\mathbf{x}}^{\ast }\right)+\mathbf{H}\left(\mathbf{x}\left(t\right)-{\mathbf{x}}^{\ast }\right)+\mathbf{b}\left(t\right),$$

(80)

where

$$\mathbf{H}=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{}_{\mathbf{x}={\mathbf{x}}^{\ast }}.$$

(81)

This equation can be rearranged as

$$\bar{\mathbf{g}}\left(t\right)=\mathbf{H}\mathbf{x}\left(t\right)+\mathbf{b}\left(t\right),$$

(82)

where

$$\bar{\mathbf{g}}\left(t\right)=\mathbf{g}\left(t\right)-\mathbf{h}\left({\mathbf{x}}^{\ast }\right)+\mathbf{H}{\mathbf{x}}^{\ast }.$$

(83)

The WLS solution of the linearized equation is

$$\widehat{\mathbf{x}}\left(t\right)={\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1}{\mathbf{H}}^T\mathbf{W}\left(t\right)\bar{\mathbf{g}}\left(t\right)$$

(84)

The estimation error, i.e.,$\widehat{\mathbf{x}}\left(t\right)-\mathbf{x}\left(t\right)$, is approximately equal to

$$\begin{array}{l}\widehat{\mathbf{x}}\left(t\right)-\mathbf{x}\left(t\right)\end{array}$$

(85)

$$\begin{array}{l}={\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1}{\mathbf{H}}^T\mathbf{W}\left(t\right)\bar{\mathbf{g}}\left(t\right)-\mathbf{x}\left(t\right)\end{array}$$

(86)

$$\begin{array}{l}={\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1}{\mathbf{H}}^T\mathbf{W}\left(t\right)\left(\mathbf{H}\mathbf{x}\left(t\right)+\mathbf{b}\left(t\right)\right)-\mathbf{x}\left(t\right)\end{array}$$

(87)

$$\begin{array}{l}={\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1}{\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{b}\left(t\right).\end{array}$$

(88)

Therefore, the correlation matrix for the estimation error is written as follows:

$$\begin{array}{l}E\left(\widehat{\mathbf{x}}\left(t\right)-\mathbf{x}\left(t\right)\right){\left(\widehat{\mathbf{x}}\left(t\right)-\mathbf{x}\left(t\right)\right)}^T\end{array}$$

(89)

$$\begin{array}{l}={\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1}{\mathbf{H}}^T\mathbf{W}\left(t\right){\mathbf{R}}_b\mathbf{W}\left(t\right)\mathbf{H}{\left({\mathbf{H}}^T\mathbf{W}\left(t\right)\mathbf{H}\right)}^{-1},\end{array}$$

(90)

where

$${\mathbf{R}}_b=E\mathbf{b}\left(t\right){\mathbf{b}}^T\left(t\right)\approx \mathbf{b}\left(t\right){\mathbf{b}}^T\left(t\right).$$

(91)

In (91), the approximation is reasonable under the assumption of the constant bias. To minimize the correlation matrix for the estimation error, the weight matrix W(t) has to be determined as the inverse matrix of R _b . However, it is not possible to calculate R _b because the bias vector is still unknown. To solve this problem, we have considered the following situation. Under the assumption that the measurements are carried out in a homogeneous environment, the bias for two pairs of nodes placed at different positions is considered the same as long as the actual distance between each member of the two pairs is identical. In addition, the bias to the distance measured between node pairs is never greater than the bias to the distance measured between two nodes that are placed further apart in the same homogeneous environment. In summary, the bias is proportional to the true distance. Multiple paths and non-line-of-sight of signal propagation can be the cause of the bias, because they make the signal travel longer distance than the signal that passes through the direct path[11]. Therefore the longer length between a node pair is, the greater bias can be caused especially at indoor in which a number of obstacles exist. Based on this observation, we model the bias vector as

$$\mathbf{b}\left(t\right)=\beta \mathbf{h}\left(\mathbf{x}\left(t\right)\right),$$

(92)

where β is the proportionality constant and x(t) is the true coordinate. Therefore, h(x(t)) is the true distance vector. By h(x(t)) = g(t)−b(t), we can rewrite (92)

$$\mathbf{b}\left(t\right)=\gamma \mathbf{g}\left(t\right),$$

(93)

where$\gamma =\frac{\beta }{1+\beta }$. The direction of b(t) is equal to that of g(t) due to the proportionality, so the normalized b(t) and g(t) are the same, i.e.

$$\frac{\mathbf{b}\left(t\right)}{\parallel \mathbf{b}\left(t\right){\parallel }_2}=\frac{\mathbf{g}\left(t\right)}{\parallel \mathbf{g}\left(t\right){\parallel }_2},$$

(94)

where ∥·∥₂ is the 2-norm operator. Through (94), we can know that b(t) can be expressed as

$$\mathbf{b}\left(t\right)=\frac{\parallel \mathbf{b}\left(t\right){\parallel }_2}{\parallel \mathbf{g}\left(t\right){\parallel }_2}\mathbf{g}\left(t\right)$$

(95)

and γ is given by$\frac{\parallel \mathbf{b}\left(t\right){\parallel }_2}{\parallel \mathbf{g}\left(t\right){\parallel }_2}$. To calculate the right hand side of (95), we use$\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)$ and$\widehat{\mathbf{g}}\left(t\right)$ instead of b(t) and g(t), respectively. Hence, the estimated bias vector can be derived as

$$\widehat{\mathbf{b}}\left(t\right)=\frac{\parallel \mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right){\parallel }_2}{\parallel \widehat{\mathbf{g}}\left(t\right){\parallel }_2}\widehat{\mathbf{g}}\left(t\right),$$

(96)

where$\widehat{\mathbf{b}}\left(t\right)$ is the estimated bias vector. If$\widehat{\mathbf{x}}(t-1)$ is equal to the true coordinate, the bias vector is just the same as$\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)$. In actual situations, however,$\widehat{\mathbf{x}}(t-1)$ is not equal to the true coordinate, so we cannot guarantee the equality between$\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)$ and the bias vector. Therefore we only use the magnitude of$\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)$ for calculating the estimated bias vector. By the proportionality assumption, the direction of the estimated bias vector is given by the direction of$\widehat{\mathbf{g}}\left(t\right)$ as shown in (96).

Finally, the weight matrix is estimated as

$$\mathbf{W}\left(t\right)={\left(\widehat{\mathbf{b}}\left(t\right){\widehat{\mathbf{b}}}^T\left(t\right)+\epsilon \mathbf{I}\right)}^{-1},$$

(97)

where ε is a small scalar value which has been inserted to solve mathematical problem of the matrix inversion, because$\widehat{\mathbf{b}}\left(t\right){\widehat{\mathbf{b}}}^T\left(t\right)$ is singular matrix.

The proposed algorithm is summarized in Algorithm 1 and the block diagram of the proposed algorithm is represented in Figure2.

Figure 2

Block diagram of proposed algorithm.

Algorithm 1 Summary of the proposed algorithm

Initial values:$\widehat{\mathbf{x}}\left(0\right)$,$\widehat{\mathbf{g}}\left(0\right)$, and P(0).

Given parameters: ε, α, and R(t).

For t = 1,2,3,…

When y(t) is obtained by ranging

//START First Stage - Noise elimination

$\mathbf{e}\left(t\right)=\mathbf{y}\left(t\right)-\widehat{\mathbf{g}}(t-1)$

K(t)=P(t−1)[P(t−1) + R(t)]⁻¹

$\widehat{\mathbf{g}}\left(t\right)=\widehat{\mathbf{g}}(t-1)+\mathbf{K}\left(t\right)\mathbf{e}\left(t\right)$

P(t) = [I−K(t)]P(t − 1)

//END First Stage - Noise elimination

//START Second Stage

$\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)=\widehat{\mathbf{g}}\left(t\right)-\mathbf{h}\left(\widehat{\mathbf{x}}(t-1)\right)$

//START Determination of the weight matrix

$\widehat{\gamma }=\frac{\parallel \mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right){\parallel }_2}{\parallel \widehat{\mathbf{g}}\left(t\right){\parallel }_2}$

$\widehat{\mathbf{b}}\left(t\right)=\widehat{\gamma }\widehat{\mathbf{g}}\left(t\right)$

$\mathbf{W}\left(t\right)={\left(\widehat{\mathbf{b}}\left(t\right){\widehat{\mathbf{b}}}^T\left(t\right)+\epsilon \mathbf{I}\right)}^{-1}$

//END Determination of the weight matrix

//START Calculation of the target coordinates

$\mathbf{J}\left(t\right)=\frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}{}_{\mathbf{x}=\widehat{\mathbf{x}}(t-1)}$

p(t) = −(J(t)^TW(t)J(t))⁻¹J(t)^TW(t)

$\widehat{\mathbf{x}}\left(t\right)=\widehat{\mathbf{x}}(t-1)+\alpha \mathbf{p}\left(t\right)\mathbf{r}\left(\widehat{\mathbf{x}}(t-1)\right)$

//END Calculation of the target coordinates

//END Second Stage

End

Experimental results

Noise elimination by using KF

First, we present the results obtained with the use of the KF for noise elimination, and show the relationship between the true distance and the bias. Under assumption that the measurement noise is uncorrelated, the off-diagonal terms of R(t) were set to zeros. The diagonal terms of R(t) were set to 0.0346 which was calculated from a number of measurements. Accordingly, R(t) was given by 0.0346 × I. We also set the initial value$\widehat{\mathbf{g}}\left(0\right)$ to the distance vector measured at the first iteration. P(0) was set to the identity matrix. In this experiment, we arranged four pairs of CSS nodes which were labeled pair 1, 2, 3, and 4 at indoor. The actual distances between pair 1, 2, 3, and 4 were set to 2.1, 4.2, 6.3, and 8.4 meters, respectively. We successively measured the distances by using the CSS ranging, and the measured distances were fed into the KF.

The dash-dotted lines in Figures3,4,5, and6 correspond to the successively measured distances between pair 1, 2, 3, and 4, respectively. The measured distances vary considerably because of the presence of noise. The solid lines correspond to the noise-eliminated distances which were calculated with the use of the KF, and they converge to the averages of the measured distances denoted as the dashed lines. The averages of the measured distances were calculated by averaging the 600 measured distances. As result, we can conclude that the KF accurately estimates the biased true distance vector g(t) in (57) and (58). Also the variation of the noise-eliminated distances after removing the noise v(t) by the KF is much smaller than that of the measured distances. By these results we can confirm our assumption of constant bias.

Figure 3

Noise elimination of measured distances between pair 1.

Figure 4

Noise elimination of measured distances between pair 2.

Figure 5

Noise elimination of measured distances between pair 3.

Figure 6

Noise elimination of measured distances between pair 4.

Moreover, the difference between the true distance and average of measured distances can be regarded as the bias. Through the graphes, we can see that the relationship between the bias and the true distance. Although the bias is not perfectly proportional to the true distance, there is a trend that the bias becomes higher when the true distance is long. Therefore the proportionality assumption can model the bias approximately. If the mismatch between the actual and modeled bias becomes considerable, the localization error of the proposed algorithm can become higher. However, the proposed algorithm can reduce the effect of the bias as long as the trend continues.

Coordinates calculation by using LS algorithm

To confirm the accuracy of the target coordinates estimated with the use of the proposed algorithm, our next step was to compare them with the coordinates estimated by using only the EKF. The solely use of the EKF for trilateration has already been discussed in Section “Conventional algorithms for trilateration”. The EKF does not consider the effect of bias.

In our localization experiments, we assigned a two-dimensional Cartesian coordinate system and installed four CSS nodes at [4.5 4.5]^T, [−4.5 4.5]^T, [−4.5 −4.5]^T, and [4.5 −4.5]^Tand numbered as reference position 1, 2, 3, and 4, respectively. They are also the vertices of 9×9 square plane. The unit of measurement of all these values is meters. The target CSS node was fixed at four different positions: these were [2.25 2.25]^T, [−2.25 2.25]^T, [−2.25 −2.25]^T, and [2.25−2.25]^T. The target CSS node begins ranging with each CSS node at the four reference points. After all four distances were measured, the proposed algorithm can calculate the coordinates of the target node. The ranging operations are iteratively performed and the proposed algorithm is executed at every iteration. It should also be noted that the localization experiment at each position was independently executed. The initial value P(0) was set equal to the identity matrix. The initial state vector$\widehat{\mathbf{x}}\left(0\right)$ was set equal to the LS solution of (19) which was obtained by using the firstly measured distances. The initial value$\widehat{\mathbf{g}}\left(0\right)$ equal to the distance vector measured at the first iteration.

With different combinations of parameters α and ε, the errors in the estimation of the target coordinates are shown in Figures7,8,9, and10. These errors are the calculated Euclidean distance between the true position and the estimated position. In the graphs below, all of results obtained with the used of the proposed algorithm are more accurate than the results of the EKF. When both α and ε are set to 0.1, the algorithm proposed in this article ensures errors in the estimation of the target position less than about 0.2 m. On the other hand, the errors in the estimation of the target position with the use of the EKF seem to differ from case to case.

Figure 7

Position estimation error estimated with the proposed algorithm and compared to the result obtained using only the EKF when the target is positioned at [2.25 2.25] ^T .

Figure 8

Position estimation error estimated with the proposed algorithm and compared to the result obtained using only the EKF when the target is positioned at [−2.25 2.25] ^T .

Figure 9

Position estimation error estimated with the proposed algorithm and compared to the result obtained using only the EKF when the target is positioned at [−2.25−2.25] ^T .

Figure 10

Position estimation error estimated with the proposed algorithm and compared to the result obtained using only the EKF when the target is positioned at [2.25−2.25] ^T .

In the proposed algorithm, parameter α affects the convergence rate of the algorithm, because α plays a role of step-size. If α decreases to zero, the convergence rate of the proposed algorithm decreases. Reversely, if α increases to one, the convergence rate of the proposed algorithm increases. Another parameter ε is originally adopted for only solving the mathematical problem of matrix inversion, but it also affects the accuracy of the proposed algorithm in the steady state. To minimize its effect, parameter ε must be selected as small as possible. In some cases, however, a large ε could give better results, because the parameter ε accidentally corrects the error of the estimated bias.

According to experimental results, we can suppose that the proposed algorithm can be used for ubiquitous applications such as navigation, context-aware services, warehouse management, and mobile robots, because the position is significant information for implementing these ubiquitous applications[8].

Conclusion

In this article, we proposed a localization algorithm by using the KF and the WLS algorithm. The proposed algorithm consists of two stages: noise elimination by using the KF and calculation of the target coordinates by the WLS algorithm. More specifically, the KF estimates a noise-eliminated distance vector, which is subsequently used for the calculation of the target coordinates as well as for the determination of the weight matrix. The results of localization experiments demonstrate the accuracy of the proposed algorithm.