Fusion Localization Algorithm Based on Robust IMM Model Combined with Semi-Definite Programming

Abstract

With the continuous development of wireless sensor network (WSN) technology, WSN has gradually become one of the key technologies of the Internet, and is widely used in indoor target location technology. However, the obstacles will have a great influence on the distance measurement, and it will result in a large positioning error. Therefore, how to deal with the non-line-of-sight (NLOS) error becomes an important problem. In this paper, Interacting Multiple Model (IMM) was used to identify NOLS/LOS. The NLOS probability was calculated by Markov transform probability, and the likelihood function was calculated by extended Kalman filter (EKF). The NLOS probability was compared with the LOS probability to judge whether the measurement contained the NLOS error. A robust algorithm combining IMM model with semidefinite programming (IMM-SDP) was proposed. The improved convex programming algorithm was proposed to reduce the NLOS error. Simulation and experimental results showed that the proposed algorithm can effectively reduce the influence of NLOS error and achieve higher positioning accuracy compared with the existing positioning methods.

1. Introduction

A wireless sensor network (WSN) consists of a large number of sensor nodes, network gateways, and clients. The sensor node could communicate with each other and share information in a network. Since GPS navigation systems are very expensive and not suitable for indoor positioning, we need to use other positioning methods. Wireless sensor positioning technology is a kind of bidirectional wireless technology with a short distance, low power consumption, low data transmission rate, and low cost. WSN is a revolutionary way of information collection in an information communication system. Therefore, it is widely applied in indoor positioning.

According to anchor node and unknown node, there are two localization methods based on WSN: one is the location method with a known distance, and the other is the location method with an unknown distance. This paper adopts the distance-based localization method. Kalman filter is used to process the data and get the correct location. However, in the process of positioning, many obstacles will have a great influence on the distance measurement, resulting in a large positioning error, so how to correctly deal with the NLOS error in the process of positioning has always been the focus and difficulty. Distance-based WSN localization mainly adopts time difference of arrival (TDOA), time of arrival (TOA), angle of arrival (AOA), and received signal strength (RSSI) systems, which have been widely studied and used to identify and mitigate NLOS errors. An IMM algorithm based on an extended Kalman filter is proposed to reduce the impact of simulation results error [1]. A new semidefinite programming algorithm is proposed [2], which transforms the TDOA model into the TOA model and adds new variables and constraints to improve positioning accuracy and reduce positioning errors.

In this paper, an improved IMM model is used to identify NLOS/LOS errors, and two convex programming methods are improved to deal with NLOS and LOS errors respectively. The algorithm has the following advantages:

(1)

In order to solve the problem of NLOS error, the semi-definite programming algorithm of the TOA/TDOA model transformation is improved in this paper. It can effectively alleviate the influence of NLOS error, and improve the positioning accuracy.

(2)

The proposed convex programming algorithm is more accurate than maximum likelihood estimation and residual estimation.

The structure of this article is as follows. Section 2 introduces relevant work and summarizes existing techniques from the literature. The signal model is introduced in Section 3. The proposed algorithm is analyzed in Section 4. The simulation and the experimental results are analyzed in Section 5. The conclusion is drawn in Section 6.

2. Related Work

Because NLOS error is the main influencing factor, the influence of NLOS error can be dealt with by suppressing the NLOS error algorithm, or directly using the filtering algorithm to filter the NLOS error. These algorithms may lose some data more or less and may not have good positioning accuracy and robustness. Therefore, through continuous research and development. Many ways of dealing with various problems have been put forward. A rough position estimation algorithm for TDOA positioning based on a non-linear least squares algorithm is proposed [3]. This algorithm is a rough position estimation algorithm based on scaling. The algorithm is robust to the initial position by optimizing the complex location function of arrival time difference. In the system [4], A moving target location tracking algorithm based on TOA measurement in a non-line-of-sight environment is proposed. In the NLOS model, the EKF standard algorithm is transformed into a linear regression model by the REKF algorithm, and the Robust technology in Hammes and Zoubir is used to solve the model. In the algorithm [5], an iterative minimum residual algorithm combined with AOA/TOA is proposed to improve the estimation performance through sequential detection and elimination of non-LOS nodes. In [6], a smoothing algorithm based on an extended Kalman smoothing filter (EK-IMM) is proposed, which combines the time of arrival (TOA) and received signal strength (RSS) algorithm for the target location. The extended Kalman filter and IMM scheme are combined to reduce the influence of NLOS on the measurement distance error. In [7], a robust NLOS bias mitigation algorithm for target location based on RSS-TOA is proposed. The algorithm transforms the location algorithm into a generalized trust region subproblem, which is solved only by dichotomy. Literature [8] proposed a low-complexity NLOS suppression method, which uses a sparse pseudo-input Gaussian process to directly eliminate LOS bias and NLOS conditions without non-line-of-sight recognition. In [9], Estimate the expected value of the NLOS error to correct the measurement. The NLOS error is eliminated by subtracting the expectation of the NLOS error. The classical multidimensional scale (MDS) algorithm is redesigned for non-LOS localization problems. In [10], the general equality constraint of spatial correlation of NLOS error is captured by establishing a maximum posterior (MAP) estimate, and the algorithm is optimized by using the specific constraint function of Gaussian process (GP) regression.

In [11], A random variable is introduced into the RSS model to simulate the path loss data, and a new robust algorithm is proposed to improve the positioning accuracy of the measurement model under unknown parameters. The effect of NLOS error can be effectively alleviated by effectively recognizing LOS distance and NLOS distance. In [12], an improved joint Probabilistic Data Association location (MJPDA) algorithm is proposed. An improved JPDA algorithm based on EKF is proposed to identify LOS and NLOS. In the case of NLOS. The improved JPDA is used to deal with the data association between virtual points and granules. Experiments show that the proposed method has better positioning accuracy and robustness when the NLOS error is large. In [13], the author proposed an NLOS recognition method based on a multidimensional scale and quasi-accurate detection. NLOS awakening can be identified effectively by mapping the NLOS awakening problem to the coarse error problem by building a model. A new NLOS recognition method based on a convolution algorithm is proposed [14]. In [15], model probability and hypothesis testing are used to identify NLOS, and then the innovation covariance matrix is used to identify NLOS for the second time in hypothesis testing Location estimates polluted by NLOS errors are dumped. The correct position estimate is aggravated by correlation probability. In [16], an NLOS classifier based on AdaBoost is proposed to identify NLOS and be used for wireless sensor positioning.

Convex programming is also used to solve NLOS problems [17]. A semi-static location method based on TDOA in a non-LOS environment is proposed, and a statistical location method based on empirical location data in an NLOS environment is proposed. Error model [18]. A soft minimum method based on convex optimization to alleviate TOA localization is proposed to solve the infeasible problem and improve the localization accuracy of NLOS. Literature [19] proposed a robust convex approximation method based on TDOA localization in an NLOS environment. Two convex relaxation approximations are used to transform robust least squares from a non-convex set to a convex set. Literature [20] proposed a new robust second-order cone relaxation method, and the NLOS error did not have much influence on this method. This method does not need to consider the prior information of NLOS error, but only needs to consider the upper and lower limits of NLOS error. In [21], two low-complexity heuristic algorithms, namely optimal selection filling (BOF) algorithm and iterative commutative greed (ISG) algorithm, are proposed to deal with non-convex problems and transform them into semidefinite programming (SDP). The problem is TDOA localization in NLOS conditions. In [22], in order to alleviate the NLOS problem, an “equilibrium parameter” related to the NLOS error is introduced, and a new robust weighted least squares problem is constructed with source position and NLOS equilibrium parameters as estimation variables, and transformed into a semidefinite programming problem. In [23], the authors use the sparsity of LOS/ NLOS measurement bias to mitigate NLOS

3. System Model UNITS

When a mobile node is moving, the information received by the base station will be interfered with noise. This interference is constantly changing between LOS and NLOS, so we need to identify whether the state of interference is LOS. This process is called As a Markov transformation process, we need to build a model to distinguish between LOS and NLOS.

We set up N base stations to detect the signals of mobile nodes, We use $\left(x_k,y_k\right)$, k = 1, 2,…N. Use $\left(x_0,y_0\right)$ to represent the mobile node, so $d_k\left(n\right)$ represents the real distance between the k-th base station and the mobile node at moment n, where k = 1, 2,…N. Since the received signal will have two situations, LOS and NLOS, we can model the true distance of the beacon node as:

$$d_k=\sqrt{{(x_k-x_0)}^2+{(y_k-y_0)}^2}$$

(1)

$$r_k=d_k+m_k$$

(2)

where, $d_k$ is expressed as the measured distance between the k base station and the mobile node, and $m_k$ is the measurement noise.

The size of the noise is divided into two cases, LOS and NLOS, when it is in the case of NLOS:

$$m_k=b_k+h_k$$

(3)

where, $b_k$ is expressed as Gaussian white noise with mean $\mu$ and variance ${\sigma }_p^2$. Thus, when the NLOS error is 0, $b_k=0$. $h_k$ is expressed as Gaussian white noise with a mean value of 0 and a variance of ${\sigma }_n^2$, which is:

$$m_k=h_k$$

(4)

According to the above distance measurement model, we build a state space model, so the state model based on all beacon nodes is expressed as:

$$\theta =\left[d_k(n) {\stackrel{.}{d}}_k(n)\right]$$

(5)

where, ${\dot{d}}_k\left(n\right)$ represents the speed at which a node is moved, so the non-linear state space model of the mobile node can be obtained as:

$${\theta }_k(n+1)=F{\theta }_k(n)+C{v}_k(n),k=1,2,\dots N$$

(6)

$$F=\left[\begin{array}{l}1 T_i\\ 0 1\end{array}\right],C=\left[\begin{array}{l}{T}_i^2/2\\ T_i\end{array}\right]$$

(7)

where, $T_{i }$ is the sampling period, and $v_k\left(n\right)$ is the noise of the mobile node during the movement [23].

4. The Proposed Algorithm

In Figure 1, the initial states of state estimation and skew variance are known, and the transformation problem of LOS/NLOS is modeled as a Markov transformation problem by means of set model probability and transition matrix. We use two parallel extended Kalman filters (LOS and NLOS) for synchronous processing to achieve model matching. By calculating the probability of LOS and NLOS and the probability of the initial state estimation of the next iteration, we judge by comparing the probability of the two models (LOS/NLOS). If the probability of NLOS is greater than the probability of LOS, the input signal is considered to be the NLOS signal; otherwise, it is the LOS signal. The two kinds of data are respectively processed by two different convex optimizations. Finally, the data after the two convex optimization processing are fused to get the final result. As shown in Figure 1, it is mainly composed of four aspects: likelihood function update, NLOS/LOS probability, semidefinite programming, and data fusion. Figure 2 shows the Process of Markov transformation.

4.1. Markov Transformation Process

The conversion probability of Markov’s model is [6]:

$$pai=\left[\begin{array}{l}0.995 0.005\\ 0.005 0.995\end{array}\right]$$

(8)

4.2. The Initial Input of the MODEL

It is known that $B{S}_k$ can be expressed as a base station, k = 1…N. $i,j\in$ [1 for the LOS smoother, 2 for the NLOS smoother], We know that the prior probability at time n − 1 is ${\mu }_{k,i}\left(n-1\right)$, and the Markov transition probability from i to mode j in $B{S}_k$ is $P_{k,ij}$. Therefore, the mixed probability from mode i to mode j can be obtained as:

$${\mu }_{k,i|j}(n-1|n-1)=(1/{\stackrel{-}{c}}_{k,j})P_{k,ij}{\mu }_{k,i}(n-1)$$

(9)

where, ${\overline{c}}_{k,j}$ is the normalization factor, which can be expressed as:

$${\stackrel{-}{c}}_{k,j}={\displaystyle \sum _i{P}_{k,ij}{\mu }_{k,i}(n-1)}$$

(10)

4.3. Interaction

The mixed state estimation in j [LOS smoother, NLOS smoother] mode is ${\stackrel{\wedge }{\theta }}_{k,0j}(n-1|n-1)$, and the covariance matrix at time n-1 in j [LOS smoother, NLOS smoother] mode is $P_{k,0j}(n-1|n-1)$, and we can obtain from the above:

$${\stackrel{\wedge }{\theta }}_{k,0j}(n-1|n-1)={\displaystyle \sum _i{\stackrel{\wedge }{\theta }}_{k,i}(n-1|n-1)}{\mu }_{k,i|j}(n-1|n-1)$$

(11)

$$\begin{array}{l}{P}_{k,0j}(n-1|n-1)={\displaystyle \sum _i\{P_{k,i}(n-1|n-1)}+\\ \left[{\stackrel{\wedge }{\theta }}_{k,i}(n-1|n-1)-{\stackrel{\wedge }{\theta }}_{k,0i}(n-1|n-1)\right]\times \\ {\left[{\stackrel{\wedge }{\theta }}_{k,i}(n-1|n-1)-{\stackrel{\wedge }{\theta }}_{k,0i}(n-1|n-1)\right]}^T\}\ast {\mu }_{k,ij}(n-1)\end{array}$$

(12)

Among them ${\stackrel{\wedge }{\theta }}_{k,i}(n-1|n-1)$, $P_{k,i}(n-1|n-1)$ are the state estimation and covariance matrix of each beacon node in i mode.

4.4. Kalman Filter Algorithm

So we introduce the above-mentioned nonlinear state space model into the scalable Kalman filter algorithm:

$${\stackrel{\wedge }{\theta }}_{k,j}(n|n-1)=F{\stackrel{\wedge }{\theta }}_{k,0j}(n-1|n-1),\forall \in j[1,2]$$

(13)

$${\stackrel{\wedge }{\theta }}_{k,j}(n|n)={\stackrel{\wedge }{\theta }}_{k,j}(n|n-1)+K_{k,j}(n)e_{k.j}(n)$$

(14)

$$e_{k,j}(n)=r_{k,j}-h_{k,j}(n){\stackrel{\wedge }{\theta }}_{k,j}(n|n-1),j=1,2$$

(15)

where, $K_{k,j}\left(n\right)$ is the alman gain, $e_{k,j}\left(n\right)$ is the deviation of the observed estimated value, the residual covariance of state prediction was calculated by the deviation value of available observation estimation.

From the above formula, the extended Kalman gain and error covariance matrix can be obtained as:

$$P_{k,j}(n|n-1)=F{P}_{k,0j}(n-1|n-1)F^T+C{v}_k^2{C}^T$$

(16)

$$K_{k,j}(n)=F{P}_{k,j}(n|n-1)H_{k,j}\times H_{k,j}{\left[P_{k,j}(n|n-1)H_{k,j}{}^T+v_k^2\right]}^{-1}$$

(17)

$$P_{k,j}(n|n)=\left[I-K_{k,j}(n)H_{k,j}\right]P_{k,j}(n|n-1)$$

(18)

$$H_{k,j}=\frac{\partial h_{k,j}(\theta )}{\partial \theta },\theta ={\stackrel{\wedge }{\theta }}_{k,j}(n|n-1),j=1,2$$

(19)

$$S_{k,j}(n)=H_{k,j}{P}_{k,j}(n|n-1)H_{k,j}{}^T+v_k^2$$

(20)

$H_{k,j}=\left[1, 0\right],j=1,2$. $v_k^2$ are the noise generated during the movement, $S_{k,j}\left(n\right)$ is the covariance of the state prediction residuals.

So we can predict the covariance of the state as:

$$S_{k,j}(n)=E\{e_{k,j}(n)e_{k,j}^T(n)\}$$

(21)

We can get that the likelihood function and model probability of the Kalman filter of the beacon node in j mode are:

$${\mathsf{\Lambda }}_{k,j}(n)=N(e_{k,j}(n);0,S_{k,j}(n))$$

(22)

$${\mu }_{k,j}(n)=(1/c_k)\ast {\mathsf{\Lambda }}_{k,j}(n){\stackrel{-}{c}}_{k,j}$$

(23)

We use the obtained NLOS/LOS probability to judge whether the input signal is LOS or NLOS, so when ${\mu }_{k,1}\left(n\right)>{\mu }_{k,2}\left(n\right)$, we think that the possibility of LOS is high, and we input the output data into the SDP algorithm that processes LOS data, on the contrary, we think it is more likely to be NLOS.

4.5. NLOS Situation

The system model can be described as:

$$r_k-b_0=d_k+b_k+h_k,k=1,2,\dots N$$

(24)

where, $b_0$ is the length of the sensor itself, which is a very small number. Squaring both sides of it can get:

$$r_k^2+b_0^2-2r_k{b}_0=d_k^2+b_k^2+2d_k{b}_k+h_k^2+2h_k(d_k+b_k)$$

(25)

$$r_k^2+b_0^2-2r_k{b}_0-d_k^2-b_k^2-2d_k{b}_k-h_k^2=2h_k(d_k+b_k)$$

(26)

Since $h_k^2$ is small, we discard it, so we make:

$$r_k^2+b_0^2-2r_k{b}_0-d_k^2-b_k^2-2d_k{b}_k=2h_k(d_k+b_k)$$

(27)

We set new variable parameters:

$$z=b_0^2$$

(28)

$$p_k=b_k^2+2d_k{b}_k>=0$$

(29)

We can get the nonlinear least squares estimate of the unknown $\theta ,z,p_k,d_k$:

$$\underset{\theta ,z,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2+z-2r_k{b}_0-d_k^2-p_k{)}^2$$

(30a)

$$s.t.$$

$$z=b_0^2$$

(30b)

$$p_k=b_k^2+2d_k{b}_k>=0$$

(30c)

It can be seen that $j\left(k\right)$ is the weight value, which is generally taken as 1, and the θ value is the position estimation of the mobile node. We obtain the final location of the mobile node by calculating the value of θ. From the above model, we can see that $\left(x_k,y_k\right)$ is the coordinates of the base station, and ${\zeta }_k$ is the coordinates, so we can get:

$$c_k=d_k^2=||{\zeta }_k-\theta |{|}^2$$

(31)

Convert it to matrix vector mode to get:

$$c_k={\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)}^T\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)$$

(32)

$$\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)>=0$$

(33)

Therefore, by increasing the variable l, more constraints are added to make the final positioning more accurate. Because of variable $c_k>=0$, the semi-definite condition is satisfied, So I can be solved by convex optimization:

$$\underset{\theta ,z,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2+z-2r_k{b}_0-c_k-p_k{)}^2+L{\displaystyle \sum _k{c}_k^2}+L{\displaystyle \sum _k{p}_k^2}+k\ast L{\displaystyle \sum _k{z}^2}$$

(34)

$$s.t. \mathrm{Equations} (27)-(32)$$

where, L is a penalty factor, which is a small number, $c_k$ is a newly added variable.

It can be known from the geometric relationship that the sum of both sides of a triangle is greater than the sum of the third side:

$$d_k+d_n>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(35)

$$(r_k-b_0)+(r_n-b_0)>=||{\zeta }_k-{\zeta }_n||,k\ne n$$

(36)

$$r_k>b_0$$

(37)

From the above description, we can get:

$$c_k^(1/2)+c_n^(1/2)>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(38)

It can be seen from the above that in the NLOS environment, the noise is very large, and the noise is generally positive, so you can get:

$$r_k-b_0>=d_k,{(r_k-b_0)}^2>=d_k^2$$

(39)

So we can introduce a new variable $g_k$, which satisfies the condition of $g_k>0$, so change (38) to:

$${(r_k-b_0)}^2-g_k=d_k^2$$

(40)

$$g_k>=0$$

(41)

A new variable $g_k$ is added, which makes the constraint relaxed, so it is necessary to add the newly added variable to the objective function $L{{\displaystyle \sum }}_k{g}_k^2$, increase L(>0). The value of a determines the result of the objective function, which can be obtained by calculation. Constrained by the above constraint conditions, combined with the new objective function to make it more accurate.

The objective function of the new semi-definite programming is:

$$\underset{\theta ,z,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2+z-2r_k{b}_0-c_k-p_k{)}^2+L{\displaystyle \sum _k{c}_k^2}+L{\displaystyle \sum _k{p}_k^2}+k\ast L{\displaystyle \sum _k{z}^2}$$

(42a)

$$s.t.$$

$$z=b_0^2$$

(42b)

$$p_k=b_k^2+2d_k{b}_k>=0$$

(42c)

$$c_k=d_k^2=||{\zeta }_k-\theta |{|}^2$$

(42d)

$$c_k={\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)}^T\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)$$

(42e)

$$\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)>=0$$

(42f)

$$c_k>=0$$

(42g)

$${(r_k-b_0)}^2-g_k=d_k^2$$

(42h)

$$g_k>=0$$

(42i)

$$c_k^(1/2)+c_n^(1/2)>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(42j)

$$(r_k-b_0)+(r_n-b_0)>=||{\zeta }_k-{\zeta }_n||,k\ne n$$

(42k)

$$r_k>b_0$$

(42l)

4.6. LOS Situation

The system model can be described as $r_k=d_k+h_k,k=1,2,\dots N$. We can get by squaring both sides:

$$r_k^2=d_k^2+h_k^2+2d_k{h}_k$$

(43)

Shift it to get:

$$r_k^2-d_k^2-h_k^2=2d_k{h}_k$$

(44)

We set $p_k=h_k^2$, so we can get the nonlinear least squares estimate of the unknown $\theta ,p,d$:

$$\underset{\theta ,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2-d_k^2-p_k{)}^2$$

(45a)

$$s.t.$$

$$p_k=h_k^2$$

(45b)

where, $j\left(k\right)$ is the weight value, which is generally taken as 1, and the value is the position estimation of the mobile node. We obtain the final location of the mobile node by calculating the value of θ.

In the above model, we can see that $\left(x_k,y_k\right)$ is the coordinates of the base station, and ${\zeta }_k$ is the coordinates, so we can get $c_k=d_k^2=||{\zeta }_k-\theta |{|}^2$. Convert $c_k$ to matrix vector mode $c_k={\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)}^T\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)$, and $\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)>=0$.

Therefore, by increasing the variable l, more constraints are added to make the final positioning more accurate and reduce errors. It can be known from the above variables:

$$c_k>=0$$

(46)

$$p_k>=0$$

(47)

Since the above variables satisfy the positive semi-definite, it can be transformed into a convex optimization problem:

$$\underset{\theta ,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2-d_k^2-p_k{)}^2+L{\displaystyle \sum _k{c}_k^2}+L{\displaystyle \sum _k{p}_k^2}$$

(48)

where, L is a penalty factor, and the variable $c_k$ is a newly added variable.

It can be known from the geometric relationship that the sum of both sides of a triangle is greater than the sum of the third side:

$$d_k+d_n>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(49)

Therefore, we can obtain:

$$c_k^(1/2)+c_n^(1/2)>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(50)

As can be seen from the above, in the LOS environment, since $m_k>0, r_k>d_k$ is not true, the measured distance is not necessarily greater than the real distance, so we can introduce a new variable $g_k$ that satisfies the condition of $g_k>=0$ constraint:

$$r_k^2+g_k>=d_k^2$$

(51)

$$g_k>=0$$

(52)

A new variable $g_k$ is added, which makes the constraint relaxed, so the newly added variable needs to be added to the objective function $q{{\displaystyle \sum }}_k{g}_k^2$. Among them, $q$ (>0), the value of $q$ (>0) determines the result of the objective function, which can be obtained by calculation. Constrained by the above constraint conditions, combined with the new objective function to make it more accurate.

The objective function of the new semi-definite programming is:

$$\underset{\theta ,p,d}{\arg \min }{\displaystyle \sum _{k}j(k)(}{r}_k^2-d_k^2-p_k{)}^2+L{\displaystyle \sum _k{c}_k^2}+L{\displaystyle \sum _k{p}_k^2}+q{\displaystyle \sum _k{g}_k^2}$$

(53a)

$$s.t.$$

$$c_k={\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)}^T\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)\left(\begin{array}{l}{\zeta }_k\\ 1\end{array}\right)$$

(53b)

$$\left(\begin{array}{l}{I}_2 \theta \\ \theta l\end{array}\right)>=0$$

(53c)

$$c_k>=0$$

(53d)

$$p_k>=0$$

(53e)

$$d_k+d_n>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(53f)

$$c_k^(1/2)+c_n^(1/2)>||{\zeta }_k-{\zeta }_n||,k\ne n$$

(53g)

$$r_k^2+g_k>=d_k^2$$

(53h)

$$g_k>=0$$

(53i)

4.7. Data Fusion

After NLOS convex optimization and LOS convex optimization, two coordinate points ${\theta }_1,{\theta }_2$ are output respectively. The final coordinates are obtained by data fusion. Thus, we can get:

$$\theta ={\theta }_1/2+{\theta }_2/2$$

(54)

5. Simulation and Experimental Results

This section mainly introduces the simulation results. As shown the Figure 3, we have deployed eight beacon nodes in the space, where we set the sampling period as 0.5 s and the sampling times as 100. The following figure shows the mobile node moving along a fixed trajectory. The initial position was denoted as $x_j\left(0\right)=\left[0.5, 20\right]$, the initial velocity was set as $Qv=1 \mathrm{m}/\mathrm{s}$, and the initial error covariance matrix was $P_{k,j}\left(0\right)=I_2$. In order to make the simulation environment more similar to the real environment, a parameter and NLOS threshold were given. This parameter is generated randomly and is more suitable for the actual environment. When the parameter value is less than the given threshold value, we judge him as NLOS, otherwise, we consider him as LOS. In this experiment, we simulate different noises such as Gaussian distribution, exponential distribution and uniform distribution. In this paper, we also compare extended Kalman filter (IMM-EKF) [6], reduced extended Kalman filter (IMM-REKF) [4], JPDA [11] and SU-SDP [2] algorithms. A total of 1000 Monte Carlo simulations were run for the simulation results. This paper uses root mean square error (RMSE) to evaluate the performance of the algorithm.

$$RMSE=\sqrt{\frac{1}{MC}\frac{1}{K}{\displaystyle \prod _{j=1}^{MC}{\displaystyle \prod _{k=1}^K({({\stackrel{\wedge }{x}}_j(k)-x_j(k))}^2+({({\stackrel{\wedge }{y}}_j(k)-y_j(k))}^2}}}$$

(55)

where, MC represents the moving steps of the mobile node, MC = 1000 is the number of Monte Carlo cycles, $({\stackrel{\wedge }{x}}_j\left(k\right),{\stackrel{\wedge }{y}}_j\left(k\right))$ is the measured position of the mobile node, and $\left(x_j\left(k\right),y_j\left(k\right)\right)$ is the real position of the mobile node.

5.1. Gaussian Distribution

We suppose that the NLOS error comply the Gaussian distribution, that is, $N\left({\mu }_{nlos},{\mu }_{nlos}^2\right)$. The measurement noise of the system, that is, the LOS noise, also obeys the Gaussian distribution, that is, $N\left(0,{\mu }_{los}^2\right)$. The size of the LOS noise has nothing to do with the measurement distance. The Gaussian distribution map of the default parameters is given in the following

Table 1. Gaussian distribution parameter map.

Parameter	Symbol	Default Values
The number of beacon nodes	M	8
NLOS error probability	NLOS	0.9
The measurements’ noise	$N(0,{\sigma }_{los}^2)$	$N(0,{0.5}^2)$
The NLOS errors	$N({\mu }_{nlos},{\sigma }_{nlos}^2)$	$N(8,8^2)$

.

As shown in the following Figure 4, we took the NLOS error mean from 3 to 8, and we find that RSME increases with the increase of the NLOS error mean, indicating that the NLOS error mean has a certain influence on the simulation results. When the mean value of NLOS changes from 3 to 8, the mean value of NLOS error of IMM-EKF, IMM-REKF and JPDA increases, and the RMSE of JPDA is approximately linear with the increase of NLOS. While the error of IMM-EKF and IMM-REKF increases with the increase of the mean value, the RMSE increases very quickly, and the RMSE of the SU-SDP method does not change significantly. It can be seen from the figure that our algorithm is better than IMM-EKF, IMM-REKF, JPDA, and SU-SDP algorithms. The positioning accuracy of this algorithm is 55.7%, 54.9%, 23.9% and 59.2% higher than that of IMM-EKF, IMM-REKF, JPDA and SU-SDP, respectively.

As shown in Figure 5, we changed the variance of NLOS from 3 to 8, and RMSE increased with the increase of variance, indicating that the variance of NLOS range had an impact on simulation results. When NLOS variance varies from 3 to 8, the errors of IMM-EKF, IMM-REKF and JPDA all increase with the increase of variance. As can be seen from the figure, our algorithm is superior to IMM-EKF, IMM-REKF, JPDA and SU-SDP algorithms. In the case of the VARIANCE of NLOS. Compared with IMM-EKF, IMM-REKF, JPDA and SU-SDP, the accuracy of the proposed algorithm is improved by 71.1%, 69.6%, 34.0%, and 60.7, respectively.

As shown in the following Figure 6, we change the probability of NLOS from 0.7 to 0.95, we find that RSME increases with the increase of the mean, but the increase in speed is relatively slow, indicating that it is very Strong robustness. When the NLOS probability changes from 0.7 to 0.95, the errors of IMM-EKF, IMM-REKF, and JPDA all increase with the increase of the mean, while the errors of IMM-EKF and IMM-REKF increase with the increase of the mean. The RMSE increases very quickly, and the RMSE of the SU-SDP method does not change significantly. It can be seen from the figure that our algorithm is better than IMM-EKF, IMM-REKF, and JPDA and SU-SDP algorithms. In the case of NLOS probability, the accuracy of this algorithm is 67.3%, 65.1%, 20.1%, and 55.3% higher than IMM-EKF, IMM-REKF, JPDA, and SU-SDP, respectively.

As shown in Figure 7, we change the variance of the LOS from 0.5 to 3, and we find that RSME increases with the increase of the variance of the LOS, indicating that the magnitude of the variance of the LOS has an impact on the simulation. It has a big impact. When the variance of the LOS varies from 0.5 to 3, IMM-EKF, IMM-REKF, and JPDA all increase slowly with the increase of the variance of the LOS and the RMSE increases very slowly. The RMSE of the SU-SDP method basically does not change much, indicating that it is extremely robust. It can be seen from the figure that our algorithm is better than IMM-EKF, IMM-REKF, and JPDA and SU-SDP algorithms. When the NLOS average value is 3, our positioning accuracy is better than IMM-EKF, IMM-REKF, and JPDA respectively. The SU-SDP algorithm is 64.8%, 63.4%, 11.0%, 50.8% higher.

5.2. Exponential Distribution

We suppose that the NLOS error complies with an exponential distribution, namely $E\left(\lambda \right)$. The measurement noise of the system, the NLOS, also obeys the Gaussian distribution, namely $N\left(0,{\sigma }_{los}^2\right)$. The size of the LOS noise has nothing to do with the measurement distance. The default parameter distribution map is given in

Table 2. Exponential distribution parameter diagram.

Parameter	Symbol	Default Values
The number of beacon nodes	M	8
NLOS error probability	NLOS	0.9
The measurements’ noise	$N(0,{\sigma }_{los}^2)$	$N(0,{0.5}^2)$
The NLOS errors	$E(\lambda )$	$E(\lambda )$

below.

It can be seen from the Figure 8 below that we change the index parameter data from 4 to 9. We find that RSME increases as the parameter increases. When the exponential distribution parameter changes from 4 to 9, IMM -EKF, IMM-REKF, and JPDA all increase the error with the increase of the mean value, the RMSE increases very fast, and the RMSE of the SU_SDP method fluctuates up and down within a certain range. It can be seen from the figure that our algorithm is better than IMM-EKF, IMM-REKF, and JPDA and SU-SDP algorithms. Under the exponential distribution, our positioning accuracy is better than IMM-EKF, IMM-REKF, and JPDA, and SU-SDP respectively. The average of the algorithm is 68.7%, 61.6%, 25.9%, and 57.1% higher.

5.3. Uniform Distribution

We suppose that the NLOS error comply an exponential distribution, namely $U(min,max)$. The measurement noise of the system, namely the line-of-sight noise, also obeys the Gaussian distribution, namely $N\left(0,{\sigma }_{los}^2\right)$. The size of the LOS noise has nothing to do with the measurement distance. The default parameter distribution map is given in

Table 3. Uniform distribution parameter map.

Parameter	Symbol	Default Values
The number of beacon nodes	M	8
NLOS error probability	NLOS	0.9
The measurements’ noise	$N(0,{\sigma }_{los}^2)$	$N(0,{0.5}^2)$
The NLOS errors	$U(\min ,\max )$	$U(6,14)$

below.

As can be seen from the Figure 9 below, when NLOS error is uniformly distributed and its parameters change from 6 to 14, we find that RMSE increases as a whole, but the change is not obvious. The size of uniform distribution has little influence on simulation results. When the NLOS errors are uniformly distributed and the parameters change from 6 to 14, the errors of IMM-EKF, IMM-REKF and JPDA all increase with the increase of the mean value, while RMSE of the SU-SDP method does not change significantly. As can be seen from the figure, our algorithm is superior to IMM-EKF, IMM-REKF, JPDA and SU-SDP algorithms. Compared with IMM-EKF, IMM-REKF, JPDA, AND SU-SDP algorithms, our positioning accuracy is 65.3%, 64.3%, 42.6%, and 63.2%, respectively, when NLOS errors are evenly distributed.

5.4. Experiment

5.4.1. Experimental Environment

In order to test the performance of the proposed algorithm, we conducted a localization experiment in an indoor environment. The signal transmission between the mobile node and beacon nodes was based on ultra-wideband (UWB) technology. UWB technology has been widely used in indoor positioning in recent years because of its low complexity and high positioning accuracy. The principle of UWB ranging is to approximate the distance between nodes based on the two-way propagation time of signals. In this experiment, both the mobile node and the beacon nodes adopted the UWB node. The difference was that the mobile node was connected with the power supply as shown in Figure 10, and was held by a pedestrian during the movement, while a beacon node was connected with a computer to transmit the collected data to the computer.

The experimental scene is depicted in Figure 11. The coordinates of eight beacon nodes are n1 (13.2, 3.6), n2 (9, 1.2), n3 (10.2, 8.4), n4 (3.6, 16.8), n5 (6,8.4), n6 (9.6, 13.8), n7 (6.6, 13.8), n8 (3.6, 4.8). The mobile node moves at a speed of 0.6 m/s along the true trajectory in the figure from the initial position R1 (12, 1.2) until it reaches R4 (4.8, 15.6). To avoid ground reflection, all nodes are placed 1.2 m above the ground. The sampling frequency is 1 Hz. Every 60 cm, one sampling point is recorded and there are 25 sampling points in the whole true trajectory. Twenty measurements are collected at every sampling point, and an average of 20 measurements at every sampling point is utilized as the measurement distance. The parameter settings for the experiment were the same as those in the simulation.

5.4.2. Experimental Results

The positioning error of each sampling point is shown in Figure 12. We can find that the positioning error of this algorithm is almost always minimal. It shows that our algorithm can effectively suppress the influence of strong NLOS error. According to experiments, the average errors of the IMM-EKF, IMM-REKF, JPDA, SU-SDP, and IMM-SDP algorithms are 0.9577, 0.9002, 0.9321, 6.7144, and 0.7834, respectively. The accuracies of our algorithm are 18.19%, 12.19%, 15.95%, and 88.33% higher than theirs, respectively.

5.4.3. Computation Time Comparison

Table 4. Running time of each algorithm.

Algorithm	Running Time/s
IMM-EKF	0.005328
IMM-REKF	0.00711
JPDA	0.0625
SU-SDP	0.9728
IMM-SDP	1.78786

shows the running time of each algorithm. Coded using MATLAB R2018a and tested on Windows 10 Professional workstation and Intel(R) Core (TM) i7-8700 @ 3.20 GHz and 8.00 GB RAM. IMM-SDP is more time-consuming, and the running time is longer due to the use of multiple convex programs.

6. Conclusions

In order to reduce NLOS errors, an improved NLOS/LOS hybrid localization algorithm was proposed. This paper proposed a robust IMM-SDP algorithm. In this paper, the IMM model was used to identify LOS and NLOS. The probability of LOS and NLOS was calculated by Markov transformation probability, and the visual distance and non-visual data were separated and processed, respectively. Finally, an improved convex programming algorithm was proposed to reduce NLOS errors, compared with IMM-EKF, IMM-REKF, JPDA and SU-SDP. Simulation and experimental results showed that the proposed algorithm has good positioning accuracy and strong robustness.