Cooperative Localization and Time Synchronization Based on M-VMP Method

Abstract

Localization estimation and clock synchronization are important research directions in the application of wireless sensor networks. Aiming at the problems of low positioning accuracy and slow convergence speed in localization estimation methods based on message passing, this paper proposes a low-complexity distributed cooperative joint estimation method suitable for dynamic networks called multi-Gaussian variational message passing (M-VMP). The proposed method constrains the message to be a multi-Gaussian function superposition form to reduce the information loss in the variational message passing algorithm (VMP). Only the mean, covariance and weight of each message need to be transmitted in the network, which reduces the computational complexity while ensuring the information completeness. The simulation results show that the proposed method is superior to the VMP algorithm in terms of position accuracy and convergence speed and is close to the sum-product algorithm over a wireless network (SPAWN) based on non-parametric belief propagation, but the computational complexity and communication load are significantly reduced.

1. Introduction

In recent years, wireless sensor network has been widely used in agriculture, warehousing, production safety, emergency rescue and other fields [1]. The information which sensors collected and transmitted is valuable when combined with the sensors’ location [2,3]. Therefore, location awareness of wireless sensor networks has become one of the most important directions in the development of wireless sensor networks. Global Navigation Satellite System (GNSS) is able to provide location information for sensor nodes, but it is difficult to apply on sensor networks due to high power consumption and high cost [2,3,4]. Furthermore, the poor signal penetration capabilities of GNSS lead to inadequate location information in indoor scenes. Cooperative localization can overcome these problems through ranging and exchanging locations between neighbor nodes [5]. Over the past decade, cooperative localization in wireless sensor networks has drawn considerable attention.

In the recent ten years, cooperative positioning in wireless sensor networks has been widely concerned. The multidimensional scaling (MDS) algorithm [6,7] calculates the shortest path distance between nodes according to the connectivity of the network or distance measurement. Then, the relative coordinate diagram of all nodes is constructed by using the multidimension scale algorithm. Finally, the relative coordinate diagram is transformed into an absolute coordinate graph according to the coordinate of the anchor node. The semi-definite programming (SDP) algorithm [8,9] represents geometric constraints among nodes as a set of linear matrix inequalities, then combines the inequality into a semi-definite programming problem and obtains the location of each node by global optimization. The distance-vector (DV) hop algorithm [10,11] firstly measures the minimum number of hops between the nodes to be located and each anchor node. Then, the average distance of each hop is determined according to the number and coordinate between anchor nodes. After the jump number is converted into distance, the position of the node to be located is obtained according to the trilateral measurement method. The Approximate Perfect Point-In-Triangulation (APIT) algorithm [12,13] firstly selects three anchor nodes connected with the node to be located, and then judges whether they are in the triangle composed of anchor nodes according to the pit test, and then repeats the pit test with a different anchor node combination. Finally, the center of mass position of these triangles is taken as the coordinates of the nodes to be located. The SPAWN algorithm [14] decomposes the posterior probability density function of position variables and represents it as a factor graph. Then, the message is transmitted by particles on the factor graph to calculate the probability of edge distribution of each variable. Finally, position estimation is carried out according to minimum mean square error (MMSE) or map criterion. Although the above cooperative positioning algorithm has high positioning accuracy, it has high computational complexity and high communication cost, which seriously restricts the practical application.

For the above problems, a collaborative location algorithm based on factor graph and VMP is proposed in Reference [15], which has simple message form and small computation. However, Kullback-Leibler (KL) divergence is used to Gauss the non-Gaussian confidence of the nonlinear range measurement model. The first kind of convergence hypergeometric function minimization problem is introduced, which makes the calculation complexity very high. In Reference [16], a hard decision-based cooperative algorithm is proposed to alleviate the effect of the outliers. The geometric relationship between agent position and distance is used to avoid a large deviation coursed by geometric constraints. The authors of Reference [17] propose the distributed particle filtering evolved variational message passing (DPF-E-VMP) algorithm, which improves the convergence speed of positioning estimation by using distributed particle filtering (DPF), but this performance improvement is often accompanied by greater computational consumption. By combining the average consensus method and VMP algorithm, a joint self-localization tracking algorithm called cooperative localization with outlier constraints (CLOC) is proposed in Reference [18], which has better location performance than the separate self-localization algorithm. In Reference [19], KL divergence is minimized by the Newton conjugate gradient method, but the computational complexity is still high. But, the information loss caused by VMP parameterization will have a certain influence on the positioning accuracy. At the same time, the algorithm has been reduced in accuracy and convergence speed due to the clock synchronization between the nodes to be located.

In this paper, a VMP distributed cooperative localization algorithm based on multi-Gaussian is proposed. Based on the non-line of sight (NLOS) environment ranging model, the VMP message passing strategy based on multi-Gaussian is innovated, and the second-order Taylor expansion form of position and time synchronization joint estimation is derived. Its computational complexity is far less than the approximate solution based on KL divergence. The time complexity and communication cost are connected with the traditional VMP algorithm. However, the positioning accuracy and iteration speed have been greatly improved.

The remainder of this paper will be organized as follows: A two-dimensional (2D) wireless network is first established as a system model. Then, a traditional cooperative localization algorithm is introduced and leads to the method proposed in this paper. After that, simulation performance of the proposed method is investigated. Finally, concluding remarks are presented. A list of symbols that are used in the paper is given in

Table 1. List of symbols.

Symbol	Meaning	Symbol	Meaning
$S_{i,n}$	Set of neighbor anchor nodes of node $i$ at time $n$	$C_{i,n}$	Set of neighbor nodes to be located of node $i$ at time $n$
${\mathit{x}}_{i,n}$	Position vector of node $i$ at time $n$	${\tilde{t}}_{i,n}$	Measurement value of local clock of note $i$ at time $n$
$T_n$	Real time value at time $n$	$a_{i,n}$	Slope of local clock of node $i$ at time $n$
$a_{ij,n}$	Relative slope of local clock offset between nodes $i,j$	$N_{i,n}$	Set of neighbor nodes of node $i$ at time $n$
${\tilde{\rho }}_{ij,n}$	Range measurement between node $i$,$j$ at time $n$	${\omega }_{ij,n}$	Measurement noise of ${\tilde{\rho }}_{ij,n}$
${\mathsf{\sigma }}_d^2$	Variance of ${\omega }_{ij,n}$	${\zeta }_{ij,n}$	NLOS error of ${\tilde{\rho }}_{ij,n}$
${\Psi }_n$	Set of all communicable node pairs $\left(i,j\right)$ at time $n$	${\Theta }_n$	Set of all node pairs $\left(i,j\right)$ with NLOS error at time $n$
${\mathit{x}}_n$	Position vector of all nodes at time $n$	${\mathit{a}}_n$	Clock offset of all nodes at time $n$
${\mathit{v}}_{i,n}$	Average velocity of node $i$ from time $n-1$ to time $n$	${\omega }_{i,n}$	Measurement noise of ${\mathit{v}}_{i,n}$
${\mathsf{\sigma }}_{i,n}^2$	Variance of ${\omega }_{i,n}$	${𝔃}_{i,n}$	Vector to be estimated of note $i$ at time $n$
${\widehat{𝔃}}_{i,n}$	Estimation result of node $i$ at time $n$	${\tilde{\mathit{\rho }}}_n$	Range measurement in all node pairs $\left(i,j\right)\in V_n$ at time $n$
${\mathit{\zeta }}_n$	NLOS error in all node pairs $\left(i,j\right)\in {\Theta }_n$ at time $n$	${𝓧}_{1:n},{𝓐}_{1:n},{𝓟}_{1:n},{𝓑}_{1:n}$	Vector sets of ${\mathit{x}}_n,{\mathit{a}}_n,{\tilde{\mathit{\rho }}}_n,{\mathit{\zeta }}_n$ from time 1 to time $n$
${\mu }_{f_{ij,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)$	Message send from $f_{ij,n}$ to ${𝔃}_{i,n}$	$b\left({𝔃}_{i,n}\right)$	Belief of variable ${𝔃}_{i,n}$
$F\left(·\right)$	Confluent Hypergeometric Function of the First Type	$E_{{𝔃}_{i,n}}$	Mean of belief $b\left({𝔃}_{i,n}\right)$
$V_{{𝔃}_{i,n}}$	Covariance of belief $b\left({𝔃}_{i,n}\right)$	${\eta }_{M,ij}$	Weight of the $M$-th Gaussian distribution
$\mathit{F}\left({𝔃}_{i,n}\right)$	Fisher Information Matrix (FIM) of ${𝔃}_{i,n}$	$\mathrm{CRLB}\left({𝔃}_{i,n}\right)$	Cramer-Rao Lower Bound of ${𝔃}_{i,n}$

.

2. System Model

The anchor nodes are always deployed at the same height, and high vertical dilution of precision means the system cannot provide reliable vertical positioning results [20], which is usually obtained by other sensors [21]. So, in this paper, a 2D dynamic wireless network is considered, which includes the anchor node with known location and synchronized local time, and the node with inaccurate location information and local time out of sync. The position vector of node $i$ at time $n$ is ${\mathit{x}}_{i,n}={\left[x_{i,n},y_{i,n}\right]}^T$, and the measured value of local clock ${\tilde{t}}_{i,n}=t_i\left(T_n\right)$. The slope of local clock at time $n$ between node $i$ and the external standard clock is $a_{i,n}=\left({\tilde{t}}_{i,n}-{\tilde{t}}_{i,n-1}\right)/\left(T_n-T_{n-1}\right)$. The local clock of all anchor nodes is synchronized with the external reference clock, i.e., $a_{i,n}=1\forall i\in S$. At time $n$, node $i$ has neighbor nodes set as $N_{i,n}$, where neighbor anchor node set is $S_{i,n},$ node set to be located is $C_{i,n}$, and all communicable node pairs $\left(i,j\right)$ constitute communicable node set ${\Psi }_n$.

Considering the local clock drift, node $i$ measures time of arrival (TOA) from neighbor node $j$ at time $n$ as follows:

$${\tilde{\rho }}_{ij,n}=∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥+{\zeta }_{ij,n}+cT{a}_{ij,n}+{\omega }_{ij,n},$$

(1)

where $∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥$ is the Euclidean distance, $d_{ij,n},$ between nodes $i, j$, $T=T_n-T_{n-1}$, ${\omega }_{ij,n}$ is the observed measurement noise, assuming that it obeys the Gaussian distribution, i.e., ${\omega }_{ij,n}\sim 𝒩\left(0,{\mathsf{\sigma }}_d^2\right)$, and ${\zeta }_{ij,n}$ is NLOS error, which is expressed as follows:

$${\zeta }_{ij,n}=\left\{\begin{array}{c}0,\left(i,j\right)\notin {\Theta }_n\\ \lambda e^{-\lambda b_{ij,n}},\left(i,j\right)\in {\Theta }_n\end{array}\right.,$$

(2)

where $b_{ij,n}>0$ [22], $\lambda$ is a constant and ${\Theta }_n$ is the set of all node pairs $\left(i,j\right)$ with NLOS error at time $n$. Where, $a_{ij,n}$ is the relative slope of local clock offset between nodes $i,j$, defined as follows:

$$a_{ij,n}\triangleq \left\{\begin{array}{c}\frac{a_{i,n}}{a_{j,n}},t_{i,n}>t_{j,n}\\ \frac{a_{j,n}}{a_{i,n}},t_{i,n}>t_{j,n}\end{array}\right.,$$

(3)

Define ${\mathit{x}}_n\triangleq {\left[{\mathit{x}}_{1,n}^T,{\mathit{x}}_{2,n}^T,\dots ,{\mathit{x}}_{S+C,n}^T\right]}^T$ as the position vector of all nodes, ${\mathit{a}}_n\triangleq {\left[a_{1,n},a_{2,n},\dots ,a_{S+C,n}\right]}^{\mathrm{T}}$ as the clock offset slope of all nodes, ${\tilde{\mathit{\rho }}}_n\triangleq {\left[\dots ,{\rho }_{ij,n},\dots \right]}^T$ as the range measurement in all node pairs $\left(i,j\right)\in {\Psi }_n$, and ${\mathit{\zeta }}_n\triangleq {\left[\dots ,{\zeta }_{ij,n},\dots \right]}^T$ as the NLOS error in all node pairs $\left(i,j\right)\in {\Theta }_n$. Moreover, all the vector sets from time 1 to time $n$ are defined as follows: ${𝓧}_{1:n}\triangleq \left\{{\mathit{x}}_1,{\mathit{x}}_2,\dots ,{\mathit{x}}_n\right\}$, ${𝓐}_{1:n}\triangleq \left\{{\mathit{a}}_1,{\mathit{a}}_2,\dots ,{\mathit{a}}_n\right\}, {𝓟}_{1:n}\triangleq \left\{{\mathit{\rho }}_1,{\mathit{\rho }}_2,\dots ,{\mathit{\rho }}_n\right\}, {𝓑}_{1:n}\triangleq \left\{{\mathit{\zeta }}_1,{\mathit{\zeta }}_2,\dots ,{\mathit{\zeta }}_n\right\}$. The goal of the algorithm is to estimate the accurate node location vector ${\mathit{x}}_n$ and the clock slope ${\mathit{a}}_n$ by measuring ${𝓟}_{1:n}$ and the information transmitted between nodes.

3. M-VMP Joint Estimation Algorithm

According to whether the location information is considered as a random variable, the cooperative localization algorithms are divided into two categories: non-Bayesian estimation and Bayesian estimation [5]. In non-Bayesian estimation, the location information is estimated by deterministic methods. The typical algorithms are least square (LS) [23,24] and the maximum likelihood method (ML) [25]. Bayesian estimation is based on the probability model of location information. The typical algorithms include the maximum posterior probability (MAP) estimation [26] and the MMSE method [6]. Location information node $i$ estimates by MMSE criteria. The expression is as follows:

$${\widehat{𝔃}}_{i,n}={\displaystyle \int }{𝔃}_{i,n}p({𝔃}_{i,n}|{𝓟}_{1:n})d{𝔃}_{i,n}$$

(4)

where ${\widehat{𝔃}}_{i,n}$ is the estimation result and ${𝔃}_{i,n}={\left[x_{i,n},y_{i,n},a_{i,n}\right]}^T$ is the vector to be estimated.

3.1. Probability Model

Assume ${\mathit{x}}_{i,n}$ evolve according to a memory-less Gauss–Markov process, then, there are:

$${\mathit{x}}_{i,n}={\mathit{x}}_{i,n-1}+{\mathit{v}}_{i,n}T+{\omega }_{i,n},$$

(5)

where ${\mathit{v}}_{i,n}$ is the average velocity of node $i$ from time $n-1$ to time $n$, which is measured by the sensor inside nodes. ${\omega }_{i,n}$ is Gaussian white noise, and its covariance matrix is $diag\left\{{\mathsf{\sigma }}_{i,n}^2,{\mathsf{\sigma }}_{i,n}^2\right\}$. Since the motions of all nodes are independent of each other, there are:

$$p\left({\mathit{x}}_n|{\mathit{x}}_{n-1}\right)={{\displaystyle \prod }}_{i}p({\mathit{x}}_{i,n}|{\mathit{x}}_{i,n-1})$$

(6)

$$p\left(a_n|a_{n-1}\right)={{\displaystyle \prod }}_{i}p(a_{i,n}|a_{i,n-1}),$$

(7)

$$p\left({𝓧}_{1:n}\right)=p\left({\mathit{x}}_0\right){{\displaystyle \prod }}_{n}p({\mathit{x}}_n|{\mathit{x}}_{n-1}),$$

(8)

$$p\left({𝓐}_{1:n}\right)=p\left(a_0\right){{\displaystyle \prod }}_{i}p(a_n|a_{n-1}),$$

(9)

where $p\left({\mathit{x}}_0\right)$ is the prior distribution of all nodes’ location information at time 0, which is obtained by GNSS, base station or in other ways [27]. According to the Bayesian rule, the edge probability function in (3) is calculated by the following formula:

$$p({𝓧}_{1:n},{𝓐}_{1:n}|{𝓟}_{1:n})\propto p({𝓟}_{1:n}|{𝓧}_{1:n},{𝓐}_{1:n})p\left({𝓧}_{1:n},{𝓐}_{1:n}\right)$$

(10)

The likelihood function $p({𝓟}_{1:n}|{𝓧}_{1:n},{𝓐}_{1:n})$ in (10) is decomposed into the following formula with the independent observations at different times ${\rho }_{ij,n}$:

$$p\left({𝓟}_{1:n}|{𝓧}_{1:n},{𝓐}_{1:n})\propto {{\displaystyle \prod }}_n{{\displaystyle \prod }}_{\left(i,j\right)\notin {\Theta }_n}{p}_{LOS}({\rho }_{ij,n}\right){{\displaystyle \prod }}_{\left(i,j\right)\in {\Theta }_n}{p}_{NLOS}({\rho }_{ij,n}),$$

(11)

$$p_{LOS}\left({\rho }_{ij,n}\right)\propto \exp \left[-\frac{{\left({\rho }_{ij,n}-∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥-cT{a}_{ij,n}\right)}^2}{2{\mathsf{\sigma }}_d^2}\right],$$

(12)

$$p_{NLOS}\left({\rho }_{ij,n}\right)\propto \exp \left[-\frac{{\left({\rho }_{ij,n}-∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥-cT{a}_{ij,n}-{\zeta }_{ij,n}\right)}^2}{2{\mathsf{\sigma }}_d^2}\right],$$

(13)

According to the above derivation, (10) is expanded to the following formula:

$$\begin{array}{cc}\hfill p({𝓧}_{1:n},{𝓐}_{1:n}|{𝓟}_{1:n})& \\ & \propto {{\displaystyle \prod }}_n{{\displaystyle \prod }}_{\left(i,j\right)\notin {\Theta }_n}{p}_{LOS}({\rho }_{ij,n}){{\displaystyle \prod }}_{\left(i,j\right)\in {\Theta }_n}{p}_{NLOS}({\rho }_{ij,n})\hfill \\ & \cdot {{\displaystyle \prod }}_{n}p\left({\mathit{x}}_{0,n}\right)p\left(a_{0,n}\right){{\displaystyle \prod }}_{i}p({\mathit{x}}_{i,n}|{\mathit{x}}_{i,n-1})p(a_{i,n}|a_{i,n-1}),\hfill \end{array}$$

(14)

3.2. Factor Graph Model

Factor graphs intuitively reflect the spatiotemporal relationship between variables [5]. The factor graph is a dichotomous graph, which contains two kinds of nodes: factor node and variable node. Variable node represents the information to be evaluated, and factor node represents the messages passed between variable nodes. The schematic diagram of cooperative localization is shown in Figure 1.

Factor nodes are further assumed as follows:

$$f_{i,0}=p\left({𝔃}_{i,0}\right),f_{i,n}=p({𝔃}_{i,n}|{𝔃}_{i,n-1}),$$

(15)

$$f_{ij,n}=\{\begin{array}{c}{p}_{LOS}\left({\rho }_{ij,n}\right),\left(i,j\right)\notin {\Theta }_n\\ p_{NLOS}\left({\rho }_{ij,n}\right),\left(i,j\right)\in {\Theta }_n\end{array},$$

(16)

$${\zeta }_{ij,n}=p\left(b_{ij,n}\right),$$

(17)

According to the above definition, edge probability of localization is clearly calculated in the factor graph. Next, a localization algorithm based on the VMP will be introduced.

3.3. VMP-Based Localization Algorithm

VMP uses the exponential model to deliver messages, which greatly reduces communication consumption. According to the VMP message rules based on the factor graph proposed in Reference [28] and the assumptions in Section 3.2, the message delivered from factor node to parameter node at time n is as follows:

$${\mu }_{f_{ij,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)=\exp \left\{{\displaystyle \int } {\mu }_{b_{ij,n}\to f_{ij,n}}\left(b_{ij,n}\right){\mu }_{{𝔃}_{j,n}\to f_{ij,n}}\left({𝔃}_{j,n}\right)\ln f_{ij,n}\left({𝔃}_{i,n},{𝔃}_{j,n}\right)d{𝔃}_{j,n}\right\},$$

(18)

$${\mu }_{f_{i,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)=p\left({𝔃}_{i,n}|{𝔃}_{i,n-1}\right),$$

(19)

$${\mu }_{f_{ij,n}\to b_{ij,n}}\left(b_{ij,n}\right)=\exp \left\{{\displaystyle \int }{\mu }_{{𝔃}_{i,n}\to f_{ij,n}}\left({𝔃}_{i,n}\right){\mu }_{{𝔃}_{j,n}\to f_{ij,n}}\left({𝔃}_{j,n}\right)\ln f_{ij,n}\left({𝔃}_{i,n},{𝔃}_{j,n}\right)d{𝔃}_{j,n}\right\},$$

(20)

where the cooperative messages ${\mu }_{{𝔃}_{j,n}\to f_{ij,n}}\left({𝔃}_{j,n}\right)$ are divided into two categories: one is related to the neighbor anchor node $j\in S_{i,n}$, and the other is related to the neighbor node $k\in C_{i,n}$ to be located. The calculations are performed below:

$${\mu }_{f_{ij,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)\propto 𝒩\left({\rho }_{ij,n}|∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥,{\mathsf{\sigma }}_d^2\right)$$

(21)

$${\mu }_{f_{ij,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)=\text{exp}\{{\mathbb{E}}_{b\left(x_k\right)}[𝒩({\rho }_{ik,n}|∥{\mathit{x}}_{i,n}-{\mathit{x}}_{k,n}∥,{\mathsf{\sigma }}_d^2)]\},$$

(22)

where ${\mathbb{E}}_{f(\cdot )}\left[g(\cdot )\right]$ means the mean of $g(\cdot )$ with respect to $f(\cdot )$. The belief of ${𝔃}_{i,n}$ is obtained as follows:

$$\begin{array}{ll}b\left({𝔃}_{i,n}\right)& =\frac{1}{Z}{\mu }_{f_{i,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right){{\displaystyle \prod }}_{j\in S_{i,n}}{\mu }_{f_{ij,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right){{\displaystyle \prod }}_{k\in C_{i,n}}{\mu }_{f_{ik,n}\to {𝔃}_{i,n}}\left({𝔃}_{i,n}\right)\\ & \propto \frac{1}{Z}𝒩\left({𝔃}_{i,n}|{𝔃}_{i,n-1}+v_{i,n}T, {\mathsf{\sigma }}_{i,n}^2\right){{\displaystyle \prod }}_{j\in S_{i,n}}𝒩\left({\rho }_{ij,n}|∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥,{\mathsf{\sigma }}_d^2\right)\\ & \times {{\displaystyle \prod }}_{k\in C_{i,n}}\exp \left\{{\mathbb{E}}_{b\left(x_k\right)}\left[𝒩\left({\rho }_{ik,n}|∥{\mathit{x}}_{i,n}-{\mathit{x}}_{k,n}∥,{\mathsf{\sigma }}_d^2\right)\right]\right\}\end{array}$$

(23)

where $Z$ is the normalization constant. Obviously, it can be seen that $b\left({𝔃}_{i,n}\right)$ is not a Gaussian function about ${𝔃}_{i,n}$, and it is difficult to transmit directly between nodes. To reduce communication overhead, Reference [19] approximates $b\left({𝔃}_{i,n}\right)$ by minimizing KL divergence:

$$b\left({𝔃}_{i,n}\right)=\underset{q\left({𝔃}_{i,n}\right)\in 𝒢}{\mathbf{a}\mathbf{r}\mathbf{g}\mathbf{m}\mathbf{i}\mathbf{n}}\mathrm{KLD}\left(q^{\left(m\right)}\left({𝔃}_{i,n}\right)|b^{\left(m\right)}\left({𝔃}_{i,n}\right)\right)$$

(24)

where $𝒢$ represents a Gaussian function, and $\mathrm{KLD}\left(q\left({𝔃}_{i,n}\right)|\mathrm{b}\left({𝔃}_{i,n}\right)\right)$ is the KL divergence between Gaussian function $q\left({𝔃}_{i,n}\right)$ and belief $b\left({𝔃}_{i,n}\right)$, the calculation formula is as follows:

$$\begin{array}{l}\mathrm{KLD}\left(q\left({𝔃}_{i,n}\right)|b\left({𝔃}_{i,n}\right)\right)\\ =\frac{{∥{\mathit{x}}_{i,n}-{\mathit{x}}_{i,0}∥}^2+2{\omega }_{i,n}^2}{2{\omega }_{i,0}^2}-\ln {\omega }_{i,n}^2\\ +{{\displaystyle \sum }}_{j\in S_{i,n}}\ln \frac{-2\left({\tilde{\rho }}_{ij,n}-cT{a}_{ij,n}\right)\sqrt{\frac{\pi }{2}{-}_{i,n}^2}F\left(-\frac{1}{2},1,\frac{{∥{\mathit{x}}_{i,n}-{\mathit{x}}_{i,0}∥}^2}{2{\mathsf{\sigma }}_d^2}\right)+∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥+2{\mathsf{\sigma }}_d^2}{2{\mathsf{\sigma }}_d^2}\\ +{{\displaystyle \sum }}_{k\in C_{i,n}}\ln \frac{-2\left({\tilde{\rho }}_{ik,n}-cT{a}_{ik,n}\right)\sqrt{\frac{\pi }{2}{-}_{i,n}^2}F\left(-\frac{1}{2},1,\frac{{∥{\mathit{x}}_{i,n}-{\mathit{x}}_{i,0}∥}^2}{2{\mathsf{\sigma }}_d^2}\right)+∥x_{i,n}-x_{k,n}∥+2{\mathsf{\sigma }}_d^2}{2{\mathsf{\sigma }}_d^2}\end{array}$$

(25)

3.4. M-VMP-Based Joint Estimation Algorithm

In Section 3.3, the clock drift of node or model loss with single Gaussian distribution are not considered, which affects localization accuracy [25]. In this paper, a joint estimation algorithm of time synchronization and localization based on multi-Gaussian distribution VMP is proposed, that the belief $b\left({𝔃}_{i,n}\right)$ of the variable ${𝔃}_{i,n}$ to be estimated is approximately a multi-Gaussian function $\widehat{b}\left({𝔃}_{i,n}\right)\propto {\displaystyle \sum }{\eta }_{M,ij}𝒩\left({𝔃}_{k.i,n}|E_{{𝔃}_{i,n}},V_{{𝔃}_{i,n}}\right)$, and the belief of ${\zeta }_{ij,n}$ is $\widehat{b}\left({\zeta }_{ij,n}\right)\propto 𝒩\left({\zeta }_{ij,n}|{\tilde{\rho }}_{ij,n}-cT{a}_{ij,n}-∥{\mathit{x}}_{i,n}-{\mathit{x}}_{j,n}∥,{\sigma }_d^2\right)$. The factor graph between nodes $i\mathrm{and}j$ at the time $n$ is shown in Figure 2.

In order to approximate $b\left({𝔃}_{i,n}\right)$ to a Gaussian function, Formula (23) is rewritten as follows:

$$b\left({𝔃}_{i,n}\right)\propto \left[f_{i,n}\left({𝔃}_{i,n}|{𝔃}_{i,n-1}\right)+{{\displaystyle \sum }}_{j\in S_{i,n}}{f}_j\left({𝔃}_{i,n}\right)+{{\displaystyle \sum }}_{k\in C_{i,n}}{f}_k\left({𝔃}_{i,n}\right)\right],$$

(26)

where:

$$f_{i,n}\left({𝔃}_{i,n}|{𝔃}_{i,n-1}\right)\triangleq {{\displaystyle \sum }}_M\frac{-{\eta }_{M,i}}{2}{\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1,M}}\right)}^T{V}_{i,n-1,M}^{-1}\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1,M}}\right)$$

(27)

$$f_j\left({𝔃}_{i,n}\right)\triangleq {{\displaystyle \sum }}_M-{\eta }_{M,ij}\frac{{\left({\tilde{\rho }}_{ij,n}-∥{\mathit{x}}_{i,n}-{\mathit{x}}_j∥-cT{E}_{a_{ij,n-1,M}}-b_{ij,n}\right)}^2}{2{\mathsf{\sigma }}_{{𝔃}_{ij,n}}^2},$$

(28)

$$f_k\left({𝔃}_{i,n}\right)\triangleq {{\displaystyle \sum }}_M-{\eta }_{M,ik}{\displaystyle \int } b\left({𝔃}_{k,n}\right)\frac{{\left({\tilde{\rho }}_{ij,n}-∥{\mathit{x}}_{i,n}-{\mathit{x}}_{k,n}∥-cT{E}_{a_{ik,n-1,M}}-b_{ik,n}\right)}^2}{2{\mathsf{\sigma }}_{{𝔃}_{ik,n}}^2}d{𝔃}_{k,n},$$

(29)

Because the existence of two non-linear terms, $∥{\mathit{x}}_{i,n}-{\mathit{x}}_j∥$ in $f_j\left({𝔃}_{i,n}\right)$ and $∥{\mathit{x}}_{i,n}-E_{{\mathit{x}}_{k,n-1,M}}∥$ in $f_k\left({𝔃}_{i,n}\right)$, results in non-Gaussian of $b\left({𝔃}_{i,n}\right)$, the two non-linear terms are expanded by a second-order Taylor expansion to obtain the mean and covariance of $b\left({𝔃}_{i,n}\right)$. For discussion purposes, define $g_j\left({𝔃}_{i,n}\right)\triangleq ∥{\mathit{x}}_{i,n}-{\mathit{x}}_j∥-cT{E}_{a_{ij,n-1,M}}$ and $g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)\triangleq ∥{\mathit{x}}_{i,n}-{\mathit{x}}_{k,n}∥-cT{E}_{a_{ik,n-1,M}}$.

a.

Second-order Taylor series expansion for nonlinear term $g_j\left({𝔃}_{i,n}\right):$$g_j\left({𝔃}_{i,n}\right)$ is expanded at ${𝔃}_{i,n}=E_{{𝔃}_{i,n-1}}$ by a second-order Taylor to get:

$$\begin{array}{ll}{g}_j\left({𝔃}_{i,n}\right)& =∥{\mathit{x}}_{i,n}-{\mathit{x}}_j∥-cT{E}_{a_{ij,n-1,M}}\\ & \approx g_j\left(E_{{𝔃}_{i,n-1}}\right)+{\nabla }^T{g}_j\left(E_{{𝔃}_{i,n-1}}\right)\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\right)\\ & +\frac{1}{2}{\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\right)}^T{\nabla }^2{g}_j\left(E_{{𝔃}_{i,n-1}}\right)\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\right),\end{array}$$

(30)

where $\nabla g_j\left(E_{{𝔃}_{i,n-1}}\right)$ and ${\nabla }^2{g}_j\left(E_{{𝔃}_{i,n-1}}\right)$ are the first and second steps of $g_j\left({𝔃}_{i,n}\right)$ at $E_{{𝔃}_{i,n-1}}$.

b.

Second-order Taylor series expansion for nonlinear term $g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)$: $g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)$ is expanded at ${𝔃}_{i,n}=E_{{𝔃}_{i,n-1}}$ and ${𝔃}_{k,n}=E_{{𝔃}_{k,n-1}}$ by a second-order Taylor to get:

$$\begin{array}{cc}\hfill g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)& =∥{\mathit{x}}_{i,n}-{\mathit{x}}_{k,n}∥-cT{E}_{a_{ik,n-1,M}}\hfill \\ \hfill & \approx g_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)+\frac{\partial g_k\left(E_{{𝔃}_{i,n-1},n_{{𝔃}_{k,n-1}}}\right)}{\partial {𝔃}_{i,n}}\left({𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\right)\hfill \\ \hfill & +\frac{{\partial }^T{g}_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)}{\partial {𝔃}_{k,n}}\left({𝔃}_{k,n}-E_{{𝔃}_{k,n-1}}\right)\hfill \\ \hfill & +\frac{1}{2}{\left(\begin{array}{c}{𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\\ {𝔃}_{k,n}-E_{{𝔃}_{k,n-1}}\end{array}\right)}^T{\nabla }^2{g}_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)\left(\begin{array}{c}{𝔃}_{i,n}-E_{{𝔃}_{i,n-1}}\\ {𝔃}_{k,n}-E_{{𝔃}_{k,n-1}}\end{array}\right),\hfill \end{array}$$

(31)

where $\frac{\partial g_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)}{\partial {𝔃}_{i,n}}$ and $\frac{\partial g_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)}{\partial {𝔃}_{k,n}}$ are the first-order partial derivatives of $g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)$ with respect to ${𝔃}_{i,n}$ and ${𝔃}_{k,n}$ at ${𝔃}_{i,n}=E_{{𝔃}_{i,n-1}}$, and ${𝔃}_{k,n}=E_{{𝔃}_{k,n-1}}$, ${\nabla }^2{g}_k\left(E_{{𝔃}_{i,n-1}},E_{{𝔃}_{k,n-1}}\right)$ is the Hessian matrix of $g_k\left({𝔃}_{i,n},{𝔃}_{k,n}\right)$ at ${𝔃}_{i,n}=E_{{𝔃}_{i,n-1}}$ and ${𝔃}_{k,n}=E_{{𝔃}_{k,n-1}}$.

After calculation, we can get the mean $E_{{𝔃}_{i,n}}$ and covariance $V_{{𝔃}_{i,n}}$ of $b\left({𝔃}_{i,n}\right)$:

$$\begin{array}{cc}\hfill V_{z_{i,n}}=\left[V_{z_{i,n-1}}^{-1}+\right.& {\sum }_{j\in S_{i,n}}{\sum }_{M_1}\frac{{\eta }_{M_1,ij}}{{\mathsf{\sigma }}_d^2}\left(I-\left({\tilde{\rho }}_{ij,n}-cT{E}_{a_{ij,n-1}}-{\zeta }_{ij,n}\right){\nabla }^2{g}_j\left(E_{z_{i,n-1}}\right)\right)\hfill \\ \hfill & +{\sum }_{k\in C_{i,n}}{\sum }_{M_2}\frac{{\eta }_{M_2,ik}}{{\mathsf{\sigma }}_d^2}(I\hfill \\ \hfill & {\left.\left.-\left({\tilde{\rho }}_{ik,n}-cT{E}_{a_{ik,n-1}}-{\zeta }_{ik,n}\right){\nabla }^2{g}_k\left(E_{z_{i,n-1}},E_{z_{k,n-1}}\right)\right)\right]}^{-1},\hfill \end{array}$$

(32)

$$\begin{array}{c}{\widehat{E}}_{z_{i,n}}=V_{z_{i,n}}\left\{V_{z_{i,n-1}}^{-1}{E}_{z_{i,n-1}}\right.\hfill \\ +\sum _{j\in S_{i,n}}\sum _{M_1}\frac{{\eta }_{M_1,ij}}{{\mathsf{\sigma }}_d^2}\left({\mathit{x}}_j\right.\hfill \\ \left.-\left({\tilde{\rho }}_{ij,n}-cT{E}_{a_{ij,n-1}}-{\zeta }_{ij,n}\right)\left(\nabla g_j\left(E_{z_{i,n-1}}\right)-{\nabla }^2{g}_j\left(E_{z_{i,n-1}}\right)E_{z_{i,n-1}}\right)\right)\hfill \\ +\sum _{k\in C_{i,n}}\sum _{M_2}\frac{{\eta }_{M_2,ik}}{{\mathsf{\sigma }}_d^2}\left({\mathit{x}}_k\right.\hfill \\ -\left({\tilde{\rho }}_{ik,n}-cT{E}_{a_{ik,n-1}}-{\zeta }_{ik,n}\right)\left(\nabla g_j\left(E_{z_{i,n-1}}\right)\right.\hfill \\ \left.\left.-{\nabla }^2{g}_k\left(E_{z_{i,n-1}},E_{z_{k,n-1}}\right)E_{z_{i,n-1}}\right)\right)\},\hfill \end{array}$$

(33)

where ${\eta }_{M,ij}=Z/\left({\rho }_{ij}·tr\left(V_{{𝔃}_{j,n-1}}\right)\right)$, and $Z$ is the normalization constant. Compared with the approximation method based on KL divergence minimization, the proposed method based on multi-Gaussian approximation with second-order Taylor series expansion greatly reduces the complexity of approximate calculation. When $b\left({𝔃}_{i,n}\right)$ is approximated to a Gaussian function $\widehat{b}\left({𝔃}_{i,n}\right)$, each node only needs to send its own position vector and covariance matrix to its neighbor nodes, and the communication overhead is much lower than that of the particle message-based method. In addition, since each node has three parameters to be estimated, the network is required to include at least three anchor nodes. The flow of the M-VMP localization algorithm (Algorithm 1) is as follows:

Algorithm 1: M-VMP joint estimation algorithm

Initialization:

Initialize node location distribution

$f_{i,0}p\left({𝔃}_{i,0}\right)$

Location estimation:

for n = 1 to N (time index) do

Nodes $i\in C$

1) Compute covariance $V_{{𝔃}_{i,n}}$ of belief $\widehat{b}\left({𝔃}_{i,n}\right)$ according to (32)

2) Compute mean ${\widehat{E}}_{z_{i,n}}$ of belief $\widehat{b}\left({𝔃}_{i,n}\right)$ according to (33)

3) Broadcast means and variances of belief $\widehat{b}\left({𝔃}_{i,n}\right)$, and receive the mean

and covariance matrix of location variables of neighbor nodes

4) Estimate location information of node $i$ according to MMSE principle

5) Update location information and form messages ${\mu }_{{𝔃}_{i,n}\to f_{i,n+1}}$

end parallel

end for

4. Simulation Analysis and Test Results

In this section, Cramer-Rao Lower Bound (CRLB) of ${𝔃}_{i,n}$ is derived first, then the performance of the proposed joint algorithms is analyzed through simulation.

4.1. CRLB Lower Bound of ${𝔃}_{i,n}$

For node $i$, the Fisher Information Matrix $\mathit{F}\left({𝔃}_{i,n}\right)$ can be derived as:

$$\mathit{F}\left({𝔃}_{i,n}\right)={\mathbf{A}}_{i}diag\left(\dots ,{\sigma }_d^2,\dots \right){\mathbf{A}}_i^{\mathrm{T}},$$

(34)

with

$${\mathbf{A}}_{i,n}=\left[\dots ,{\mathit{u}}_{ij,n},\dots \right],j\in {\mathit{N}}_{i,n}$$

(35)

$${\mathit{u}}_{ij,n}=\frac{1}{{\rho }_{ij,n}}\left[\begin{array}{c}{x}_{i,n}-x_{j,n}\\ y_{i,n}-y_{j,n}\\ cT{a}_{ij,n}\end{array}\right],$$

(36)

and CRLB of ${𝔃}_{i,n}$ is $\mathrm{CRLB}\left({𝔃}_{i,n}\right)={\mathit{F}}^{-1}\left({𝔃}_{i,n}\right)$.

4.2. Simulation Scenario and Result Analysis

To analyze the performance of the proposed method, we have built a simulation scenario according to the real scene of the zone 1, underground parking lot, Beijing University of Posts and Telecommunications. The real scene is shown in Figure 3a and the top view is shown in Figure 3b.

According to the actual size of zone 1, the simulation scene is a rectangular area of 20 × 24 m. By default, the number of anchor nodes is 4, the number of nodes to be located is 20 and the initial position distribution of nodes in the site conforms to the uniform distribution. In order to limit the communication area of nodes to less than 1/2 of the whole simulation area, the maximum communication distance is set to 10 m. The performance of the proposed method is obtained in the line of sight (LOS) environment, except the last simulation, which shows localization accuracy with different NLOS probability. All simulation results are the average of 1000 independent runs.

The initial position measurement error of the anchor node conforms to the Gaussian distribution with the standard deviation of 0.1 m. The initial position measurement error distribution of the node to be located is composed of Gaussian distributions with the standard deviation of 10 m. According to the performance of the crystal oscillator used in the hardware (TG5032CFN), the clock drift of the node to be located conforms to uniform distribution from [−1 ppm, 1 ppm], that is, the maximum distance measurement error caused by the clock drift between adjacent time (1 s) is 300 m. In the process of simulation, anchor nodes remain stationary, and the velocity of the node to be located is consistent with the uniform distribution of the maximum value of 3 m/s, and the direction is random every time step. The position movement measurement of the node conforms to the Gaussian distribution with the standard deviation of 1 m. The default process is 15 s, 10 iterations per second. Root mean square error (RMSE) and cumulative distribution functions (CDF) are used to measure the performance of algorithms.

In Figure 4a, the relationship between localization RMSE and initial location error is shown. The CLOC method [16] treats the uncertainties of nodes’ positions and clock offsets as measurement noise and thus suffers performance degradation. The SPAWN [26] method is implemented by using 4000 particles to represent the messages on FG. The localization accuracy of the VMP method [17] and the proposed M-VMP method are better than the CLOC method, but slightly worse than the SPAWN method.

The position error of VMP and the proposed M-VMP method are compared in Figure 4b. The cumulative distribution function of the VMP method and the proposed method under different initial position errors is given in Figure 4b. It is found that the probability of position error less than 1.6 m of the two methods under different initial position errors is basically the same, while the proposed method has a better effect of restraining position error. The result is consistent with the conclusion in Figure 4a.

Figure 5 shows the relationship among the clock drift slope, RMSE of localization accuracy and the number of iterations, respectively. It is seen that three methods converge in the finite iterations, which proves that the algorithms are feasible. Compared with the VMP method [17], the proposed method improves the convergence speed of localization and time synchronization by adding time synchronization parameters into the localization process.

In Figure 6, the CDF of the proposed method under different values of the number of Gaussian distribution M are compared. It is seen that the localization accuracy (3$\mathsf{\sigma }$) of the proposed method is better than that of the VMP method when M is greater than 2. With the increase of M, the localization accuracy is also improved.

Figure 7 reflects the influence of communication distance on the localization accuracy of the proposed method under the same node density. Obviously, localization accuracy increases with the increase of communication distance, which increases the connectivity of nodes and the redundancy of information. At the same time, it can be seen that the improvement of localization accuracy is not obvious when the communication distance is 20 and 30 m. The difference of node connectivity between those two conditions is little, so the localization accuracy cannot be greatly improved. Because the communication distance is proportional to the transmission power of the node, it is necessary to determine the appropriate communication distance in practical application by considering the localization accuracy, node power consumption and other indicators.

In the simulation of Figure 8, in order to amplify the influence of node density on positioning accuracy, we reset some of the simulation parameters, and set the communication distance to 15 m and the variance of the distance observation and the initial position error of the node to be located to 20 m². It can be seen from Figure 8 that as the density of nodes increases, the overall positioning accuracy is decreasing. However, when the density of nodes to be located is too large, the positioning error will increase. At the same time, when there are more nodes to be located, the speed at which the positioning error decreases with more anchor nodes also decreases. This is due to the increase in the density of nodes to be located, and the overall weight of the information contained in neighboring nodes to be located in the proposed method becomes larger. When the number of anchor nodes is insufficient, the increase of neighboring nodes to be located helps to improve the positioning accuracy, but the effect is limited.

In Figure 9, the relationship between the RMSE of localization accuracy and NLOS occurrence probability of the VMP method and the proposed method is compared. Furthermore, it compares with the method that only deals with LOS. It is seen that both the VMP and the proposed method effectively suppress NLOS error. Because the two-way NLOS parameters between nodes are added in the proposed method, the NLOS error is better suppressed.

4.3. Computational Complexity and Communication Overhead Analysis

Because the proposed method in this paper and the comparison methods are distributed algorithms, the computation and communication are done by the nodes independently, so only one node’s time complexity and communication overhead need to be considered in the analysis.

The time complexity is evaluated by the number of operations in local computing, and the communication cost is evaluated by the number of information parameters broadcast by nodes. The time complexity of the CLOC method is related to its neighbor nodes number ${\mathrm{N}}_{\mathrm{inb}}$, and its communication cost is $3·\mathrm{O}\left(1\right)$. The SPAWN method is based on particles. When the number of particles in each message is ${\mathrm{N}}_{\mathrm{p}}$, its time complexity is $\mathrm{O}\left({\mathrm{N}}_{\mathrm{p}}+{\mathrm{N}}_{\mathrm{p}}^2·{\mathrm{N}}_{\mathrm{inb}}\right)$, and its communication cost is $\mathrm{O}\left({\mathrm{N}}_{\mathrm{p}}\right)+2·\mathrm{O}\left(1\right)$. Compared with the VMP method, the number of Gaussian messages in the proposed method is M times, and every message from neighbor nodes needs to be processed once. The time complexity of dimension reduction depends on ${\mathrm{N}}_{\mathrm{inb}}$, so its time complexity is $\mathrm{O}\left(\mathrm{M}·{\mathrm{N}}_{\mathrm{inb}}\right)+\mathrm{O}\left({\mathrm{N}}_{\mathrm{inb}}\right)$. In the proposed method, nodes transmit a localization vector including ${\mathrm{x}}_{\mathrm{i},\mathrm{n}},{\mathrm{y}}_{\mathrm{i},\mathrm{n}},{\mathrm{a}}_{\mathrm{i},\mathrm{n}}$ and a covariance matrix, so the communication cost is $3\mathrm{O}\left(\mathrm{M}\right)+\mathrm{O}\left(1\right)$. The computational complexity, run-time and communication Overhead of the three algorithms is shown in

Table 2. Comparisons of different methods for each node at each iteration.

Method	Computational Complexity	Run-Time	Communication Overhead
M-VMP (proposed method)	$O\left(M·N_{inb}\right)+O\left(N_{inb}\right)$	84.449 ms	$3·O\left(M\right)+O\left(1\right)$
CLOC	$O\left(N_{inb}^2\right)$	288.500 ms	$3·O\left(1\right)$
SPAWN	$O\left(N_p+N_p^2·N_{inb}\right)$	5469.089 ms	$O\left(N_p\right)+2·O\left(1\right)$

.

4.4. Future Research Directions

In the next stage of research, we will mainly focus on the following aspects: First, analyze the performance of the positioning and simultaneous joint estimation problem and its influencing factors in principle. Secondly, expand the positioning scene from 2 dimensions to 3 dimensions, and reduce the problem of positioning accuracy degradation in dense scenes with nodes to be located through methods such as signal quality screening. Finally, the proposed algorithm will be implemented based on the hardware platform, and the measured results will be compared with the simulation results to further improve the performance of the proposed method.

5. Conclusions

This paper presented a M-VMP-based TOA localization and time synchronization joint estimation algorithm for mixed LOS and NLOS environments. Firstly, according to the VMP method, a message propagation model based on a factor graph was constructed for localization and time synchronization. Owing to the existence of nonlinear terms, it is difficult to represent the message in a closed form, so Taylor expansion was used for the linearization of nonlinear terms. Moreover, to reduce the communication cost, all messages were expressed in the form of multi-Gaussian distribution, and only the mean value, covariance and weight of each Gaussian distribution need to be transferred in the message transmission. The simulation results showed that the accuracy and convergence speed of the proposed method were close to that of SPAWN method, and the time complexity and communication cost were greatly reduced: the run-time was only 1.5% that of SPAWN.