# Small-Range High-Precision Positioning Based on Two-Point Coordination for Robot

Authors: Zhengping Li; Chaoliang Qin; Hao Shi
Journal: Mobile Information Systems
 (2021)

---

## Abstract

This paper proposed a two-point coordinated positioning algorithm. Based on the assumption that the distance between two points was constant, a fusion algorithm was introduced into the positioning process to enhance the positioning accuracy. The simulation results showed that the proposed algorithm could reduce the RMS error to about 50% of the improved sinc interpolation-based positioning algorithm when the sampling frequency was 500 MHz and the interpolation number was 19.


---

## Body

## 1. Introduction

Accurate positioning is very important for robotic applications [1]. There are several kinds of indoor positioning systems. The non-radio-based technologies mainly employ cameras; the location accuracy is low when there are obstacles before the camera; and the image processing algorithm needs a high-speed processor that makes the cost and the power consumption very high. The commonly used approaches are radio-based technologies, such as Wi-Fi and ultra-wideband (UWB). Wi-Fi-based indoor positioning system primarily employs the strength of the Wi-Fi’s access points (APs), and the accuracy is about 2 m, which is too low to be used by many indoor applications [2–4]. UWB-based positioning system has high accuracy due to the large bandwidth, but because of the shadow fading and the random interferences, the positioning results are not stable. Moreover, time-based ranging technology was used in UWB positioning system, and several handshaking processes are needed to get the range between two nodes, which makes the positioning frequency low, and it cannot meet the requirements of some applications. Many research studies have been done on UWB-based localization [5–8] to enhance positioning stability. Some research studies employed filters to enhance the positioning accuracy [8], some research studies were focused on the ranging error elimination [5], and others attempted to fuse the data such as moving state and IMU data with the UWB positing data to eliminate the unstability [6, 7], and the problem is not solved yet. This paper proposed a two-point positioning data-fusing algorithm for the applications with more than one target nodes on an object [9].This paper designed an improved positioning system that employed the proposed improved sinc interpolation algorithm to enhance the positioning frequency of the system for TDOA values. Then the system estimated the position of the target node using Chan’s algorithm and used two-point coordination method to optimize the positioning results. As the result, we can reduce the sampling frequency to the maximum extent on the premise of ensuring accuracy.The rest of the paper is organized as follows. Section2 describes a survey of the related research studies; Section 3 describes the related principle of the proposed algorithm; Section 4 explains simulation and analysis of the algorithm; and Section 5 draws conclusion.


## 2. Related Works

With the rapid increase of data and multimedia services, the demand for positioning and navigation is increasing, especially in the complex indoor environment, such as the airport hall, exhibition hall, warehouse, supermarket, library, and underground parking lot.TDOA (time difference of arrival) positioning is a kind of wireless positioning. By measuring the time of arrival of the signal to the reference node, the distance of the target node can be determined [10]. Using the distance between the target node and the various reference nodes, the location of the target node can be determined. However, the absolute time is difficult to measure; by comparing the time difference between the signals to the reference nodes, we can make the hyperbola with focus on the reference node and the long axis of the distance difference. The intersection of the hyperbolic is the location of the target node [11].Based on TDOA, Li and Wang put forward a new algorithm that can greatly improve the positioning accuracy. Their system employs matched filter to calculate the TDOA value and does not need precise synchronization between the transmitter and receivers that makes the TDOA value more accurate [12]. In this paper, we propose the two-point coordination algorithm to improve data processing. The two-point coordination algorithm uses two-point information to calculate the position of the target node, while in the Small Range High Precision Positioning Algorithm Based on Improved Sinc only one-point information is used to calculate the position of the target node, so when we use two-point coordination algorithm to process positioning results, it can improve the positioning accuracy.


## 3. The Positioning Process and Related Algorithm

### 3.1. The Positioning Process

The positioning steps are as follows:Step 1:L fixed target nodes received the FM wave signals from the reference node, of which L was a positive integer. The modulation signal of the FM wave was a sawtooth signal, and therefore, the FM wave was called a sawtooth FM wave. A cycle of the sawtooth FM signal was called a chirp.Step 2: we conducted amplitude limitation on the received signals, sampled onM continuous chirps at interval T, and achieved the sample function xin in which i = 1, 2, …, M and n = 0, 1, …, N. N was the sample point number of each chirp.Step 3: by using improved sinc interpolation algorithm to reconstruct the sample function, we could get the reconstruction functionyk of which k = 0, 1, …, (N − 1) (M + 1) + M.Step 4: sampling the original sawtooth FM waves at intervalT/(M + 1) and achieving the sample function uk, where k = 0, 1, …, (N − 1) (M + 1) + M. Then, we took uk and yk to perform the cross-correlation operation and obtained the correlation peak location Ai of which i = 0, 1, …, L.Step 5: using correlation peak location gap, we could calculate the signal arrival time differencet2,1,t3,1,…,ti,1,…,tL,1 between the 2th,3th,…,Lth reference node and the 1th reference node. Among them t2,1,t3,1,…,ti,1,…,tL,1 were the TDOA values of which ti,1=Ai−A1∗T. T was the time interval of the sample points in yk.Step 6: the TDOA values and the coordinates of the reference nodes were put into Chan’s algorithm to calculate the position of the target node.Step 7: two-point coordination was used to optimize the positioning results.


### 3.2. The Related Algorithm

In Step 3, an improved sinc interpolation algorithm was mentioned, and a detailed description of the improved sinc interpolation algorithm was presented in theSmall Range High Precision Positioning Algorithm Based on Improved Sinc Interpolation. In Step 6, the Chan’s algorithm was mentioned and a detailed description of the Chan’s algorithm was presented in the Precision Wireless Positioning Scheme in Small Range Based on First-Order Difference and Correlation Inspection. Therefore, we need not repeat the algorithm here.In Step 7, the two-point coordination algorithm was mentioned, and a detailed description of the improved sinc interpolation algorithm was presented in this section.It is assumed that the distance of two target nodes is known ash; the coordinates of the two target nodes x1′,y1′ and x2′,y2′are estimated using the location algorithm.If the distance between two points is greater than a certain distance, this set of data is considered to be a gross error and should be removed. Namely,(1)h′=x1′−x2′2+y1′−y2′2>ωh,where ω is an empirical value that is greater than 1.Ifx1′,y1′ and x2′,y2′ can be retained, according to x1′,y1′ and x2′,y2′, we estimate the location of the target node two times. For example, according to the coordinates of the target node 1 x1′,y1′ to estimate the coordinates of the target node 2 x2″,y2″, as the distance between two nodes h is known, it is assumed that the target node 2 x2″,y2″ is in the circle with center point x1′,y1′ and a radius of h, and target node 2 is also in the straight line with the two points x1′,y1′ and x2′,y2′.As we know there are two intersection points of a straight line and a circle, we choose the point that is closer tox2′,y2′ as the target node 2 x2″,y2″. We can estimate the x2″,y2″ according to the following equations:(2)x−x1′2+y−y1′2=h,x−x1′x2′−x1′=y−y1′y2′−y1′.There are two solutions to the equations, and we choose the final solution that is closer tox2′,y2′. The same can be used to solve the second-time estimation coordinates x1″,y1″ of the target node 1.So far, the two estimation coordinates for each target node are obtained. Then the fusion algorithm is used to fuse the data of the two groups. The following is a method of calculating the weight.Set the actual horizontal coordinates of the target node 1x:(3)x1′=x+v1,x1″=x+v2.where vii=1,2 is a random error and vi∼N0,σi2, two observations are independent of each other.It is assumed that the final estimation resultsx^ of x are in linear relationship with the first estimate x1′ and the second estimate x1″, and the x^ is the unbiased estimate of x:(4)x^=ω1x1′ω2x1″.where Ω=ω1,ω2 is the weight value of the estimated value.Set the estimation error:(5)x˜=x−x^.Take the cost functionx˜ for the mean square error:(6)J=Ex˜2=Ex−ω1x+x1′−ω2x+x1″2.As thex^ is the unbiased estimate of x,(7)Ex˜=Ex−ω1x+x1′−ω2x+x1″=0.AsEv1=Ev2=0 and Ex=Ex^,(8)ω2=1−ω1.Then the cost function can be written as(9)J=Ex^2=Eω12v12+1−ω12v22+2ω11−ω1v1v2.AsEv12=σ12 and Ev22=σ22, v1 and v2are independent; Ev1,v2=0.Then,(10)J=Ex˜2=ω12σ12+1−ω12σ22.In order to obtain the minimum value ofJ and Ω derivatives,(11)∂J∂Ω=0.The optimal weight value is(12)ω1∗=σ22σ22+σ12,ω2∗=σ12σ22+σ12.Optimal estimation is(13)x^=σ22x1′σ22+σ12+σ12x1″σ22+σ12.In the same way, the vertical coordinates can also solved.The two-point coordination algorithm uses two-point information to calculate the position of the target node, while in theSmall Range High Precision Positioning Algorithm Based on Improved Sinc only one-point information is used to calculate the position of the target node, so when we use two-point coordination algorithm to process positioning results, it can improve the positioning accuracy.


## 3.1. The Positioning Process

The positioning steps are as follows:Step 1:L fixed target nodes received the FM wave signals from the reference node, of which L was a positive integer. The modulation signal of the FM wave was a sawtooth signal, and therefore, the FM wave was called a sawtooth FM wave. A cycle of the sawtooth FM signal was called a chirp.Step 2: we conducted amplitude limitation on the received signals, sampled onM continuous chirps at interval T, and achieved the sample function xin in which i = 1, 2, …, M and n = 0, 1, …, N. N was the sample point number of each chirp.Step 3: by using improved sinc interpolation algorithm to reconstruct the sample function, we could get the reconstruction functionyk of which k = 0, 1, …, (N − 1) (M + 1) + M.Step 4: sampling the original sawtooth FM waves at intervalT/(M + 1) and achieving the sample function uk, where k = 0, 1, …, (N − 1) (M + 1) + M. Then, we took uk and yk to perform the cross-correlation operation and obtained the correlation peak location Ai of which i = 0, 1, …, L.Step 5: using correlation peak location gap, we could calculate the signal arrival time differencet2,1,t3,1,…,ti,1,…,tL,1 between the 2th,3th,…,Lth reference node and the 1th reference node. Among them t2,1,t3,1,…,ti,1,…,tL,1 were the TDOA values of which ti,1=Ai−A1∗T. T was the time interval of the sample points in yk.Step 6: the TDOA values and the coordinates of the reference nodes were put into Chan’s algorithm to calculate the position of the target node.Step 7: two-point coordination was used to optimize the positioning results.


## 3.2. The Related Algorithm

In Step 3, an improved sinc interpolation algorithm was mentioned, and a detailed description of the improved sinc interpolation algorithm was presented in theSmall Range High Precision Positioning Algorithm Based on Improved Sinc Interpolation. In Step 6, the Chan’s algorithm was mentioned and a detailed description of the Chan’s algorithm was presented in the Precision Wireless Positioning Scheme in Small Range Based on First-Order Difference and Correlation Inspection. Therefore, we need not repeat the algorithm here.In Step 7, the two-point coordination algorithm was mentioned, and a detailed description of the improved sinc interpolation algorithm was presented in this section.It is assumed that the distance of two target nodes is known ash; the coordinates of the two target nodes x1′,y1′ and x2′,y2′are estimated using the location algorithm.If the distance between two points is greater than a certain distance, this set of data is considered to be a gross error and should be removed. Namely,(1)h′=x1′−x2′2+y1′−y2′2>ωh,where ω is an empirical value that is greater than 1.Ifx1′,y1′ and x2′,y2′ can be retained, according to x1′,y1′ and x2′,y2′, we estimate the location of the target node two times. For example, according to the coordinates of the target node 1 x1′,y1′ to estimate the coordinates of the target node 2 x2″,y2″, as the distance between two nodes h is known, it is assumed that the target node 2 x2″,y2″ is in the circle with center point x1′,y1′ and a radius of h, and target node 2 is also in the straight line with the two points x1′,y1′ and x2′,y2′.As we know there are two intersection points of a straight line and a circle, we choose the point that is closer tox2′,y2′ as the target node 2 x2″,y2″. We can estimate the x2″,y2″ according to the following equations:(2)x−x1′2+y−y1′2=h,x−x1′x2′−x1′=y−y1′y2′−y1′.There are two solutions to the equations, and we choose the final solution that is closer tox2′,y2′. The same can be used to solve the second-time estimation coordinates x1″,y1″ of the target node 1.So far, the two estimation coordinates for each target node are obtained. Then the fusion algorithm is used to fuse the data of the two groups. The following is a method of calculating the weight.Set the actual horizontal coordinates of the target node 1x:(3)x1′=x+v1,x1″=x+v2.where vii=1,2 is a random error and vi∼N0,σi2, two observations are independent of each other.It is assumed that the final estimation resultsx^ of x are in linear relationship with the first estimate x1′ and the second estimate x1″, and the x^ is the unbiased estimate of x:(4)x^=ω1x1′ω2x1″.where Ω=ω1,ω2 is the weight value of the estimated value.Set the estimation error:(5)x˜=x−x^.Take the cost functionx˜ for the mean square error:(6)J=Ex˜2=Ex−ω1x+x1′−ω2x+x1″2.As thex^ is the unbiased estimate of x,(7)Ex˜=Ex−ω1x+x1′−ω2x+x1″=0.AsEv1=Ev2=0 and Ex=Ex^,(8)ω2=1−ω1.Then the cost function can be written as(9)J=Ex^2=Eω12v12+1−ω12v22+2ω11−ω1v1v2.AsEv12=σ12 and Ev22=σ22, v1 and v2are independent; Ev1,v2=0.Then,(10)J=Ex˜2=ω12σ12+1−ω12σ22.In order to obtain the minimum value ofJ and Ω derivatives,(11)∂J∂Ω=0.The optimal weight value is(12)ω1∗=σ22σ22+σ12,ω2∗=σ12σ22+σ12.Optimal estimation is(13)x^=σ22x1′σ22+σ12+σ12x1″σ22+σ12.In the same way, the vertical coordinates can also solved.The two-point coordination algorithm uses two-point information to calculate the position of the target node, while in theSmall Range High Precision Positioning Algorithm Based on Improved Sinc only one-point information is used to calculate the position of the target node, so when we use two-point coordination algorithm to process positioning results, it can improve the positioning accuracy.


## 4. System Simulation and Analysis

In the simulation system, the positioning area was determined by the number of reference nodes, and the more the number of reference nodes, the larger the positioning area. The reference nodes were stationary, and they should be distributed around the positioning area uniformly as much as possible so that the system could get better positioning results.In this simulation system, it supposes that the positioning range is20m×20m. The coordinates of seven reference nodes were (0, 0), (0, 20), (10, −4), (20, 0), (10, 24), (20, 20), and (−4, 10). The target node acted as a transmitter. The modulation signal’s frequency of the target node was 1 MHz. The simulation supposes that the transmission channel was 6-path Rician channel that had 1 line-of-sight (LOS) path and 5 reflection paths. The reflection paths were caused by the multipath effect because of the signal reflection, diffraction, and scattering. The additional delay of 6 paths were [0, 31/1e9, 71/1e9, 109/1e9, 173/1e9, and 251/1e9] (s); the additional attenuation were [0, −1, −9, −10, −15, and −20] (db); and this was a common indoor channel. In addition, the received signal is summed together of the LOS and reflection signals. If the obstacles were on the LOS path, it should affect the TDOA value and cause TDOA errors.The positioning accuracy was measured with the root-mean-square error (RMSE) of positioning results, which was frequently used at present (Figures1–4). The positioning system simulation was done in different conditions through MATLAB (Tables 1–3):(1)
The positioning accuracy and time with different sinc interpolation algorithms are shown in Figure1.In Figure1, abscissa was three different algorithms: the algorithm without any interpolation algorithm, the algorithm with nonimproved interpolation algorithm, and the algorithm with improved interpolation algorithm. The ordinates were RMSE and time. Table 1 lists the details of each point in Figure 1. The sampling frequency of the three algorithms in Figure 1 was 500 MHz, and the carrier frequency was 100 MHz. From the simulation results, it could be seen that, in the process of data processing, the positioning accuracy and positioning time of the different degrees of improvement were compared between Chauvenet’s criterion and coordination algorithm. When we do not use any interpolation algorithm, the algorithm has a lower positioning accuracy. Therefore, the positioning accuracy can be significantly improved when we use the coordination algorithm in the data processing. However, the positioning time of the algorithm is very short, so it is not obvious that the positioning time is shortened after we use the coordination algorithm [13, 14]. When we use the improved interpolation algorithm, the algorithm has a lower positioning time. Therefore, the positioning time can be significantly improved when we use the coordination algorithm in the data processing. However, the positioning accuracy of the algorithm is very low, so it is not obvious that the positioning accuracy is promoted after we use the coordination algorithm. Then we will analyze the positioning accuracy in different situations in detail.(2)
The positioning accuracy in different sampling frequencies and interpolation points is shown in Figure2:In Figure2, abscissa was carrier frequency, and the values were 10 MHz, 20 MHz, 30 MHz, 40 MHz, 50 MHz, 60 MHz, 70 MHz, 80 MHz, 90 MHz, and 100 MHz. The ordinate was RMSE. Table 2 lists the details of each point in Figure 2. The target node was stationary, and it could be at any place in the positioning area. From the simulation results, it could be seen that the algorithm with improved sinc interpolation comparing with the algorithm without improved sinc interpolation in positioning accuracy had a very large enhancement. The RMSE decreased from about 3 m to about 0.01 m. From the simulation results, it could be seen that the algorithm with improved sinc interpolation with two-point coordination compared with the algorithm with improved sinc interpolation with Chauvenet’s criterion in positioning accuracy had certain enhancement. The RMSE decreased from about 0.015 m to about 0.010 m. We could also see from the results that the carrier frequency had little influence on the positioning accuracy when the carrier frequency varies from 10 MHz to 100 MHz.(3)
The positioning accuracy in different numbers of interpolation points is shown in Figure3.In Figure3, abscissa was the numbers of interpolation points, and the values were 0, 9 and 19. The ordinate was RMSE. We set the carrier frequency to 50 MHz. Table 3 lists the details of each point in Figure 3. From the simulation results, it could be seen that when there were no interpolation points, whether the Chauvenet’s criterion or two-point coordination is used, the positioning accuracy is relatively low. The RMSE is over 3.0 m. When we interpolated 9 points to the 500 MHz/1 GHz sampling chips, the positioning accuracy improved obviously. The algorithm with two-point coordination compared with the algorithm with Chauvenet’s criterion in positioning accuracy had certain enhancement. Since the positioning accuracy of the sampling frequency with 9 interpolation points to 1 GHz sampling chips was enough high, there was little accuracy improvement when we interpolated 19 points to the 1 GHz sampling chips.(4)
The positioning accuracy in different sampling frequencies is shown in Figure4.Figure 1
(a) RMSE and (b) time of different sinc interpolation algorithms.
(a)(b)Figure 2
RMSE of different algorithms in different sampling frequencies and interpolation points.Figure 3
RMSE of different algorithms in different numbers of interpolation points.Figure 4
RMSE of different algorithms in different numbers of interpolation points.Table 1
RMSE and time of different sinc interpolation algorithms.
RMSE squareRMSE circleRMSE (m)Without sinc3.89793.4147Nonimproved sinc3.48132.8058Improved sinc0.01570.0142Time (s)Without sinc2.17401.9254Nonimproved sinc15.156111.2563Improved sinc151.2365143.2465Table 2
RMSE of different algorithms in different sampling frequencies and interpolation points.
CF (MHz)RMSE (m)RMSE squareRMSE circleRMSE asteriskRMSE cross103.82950.01253.69160.0118203.87240.01483.68350.0113303.89280.01073.65570.0102403.89950.01763.65310.0112503.89830.01843.64590.0104603.81960.01483.61840.0121703.88120.01193.63570.0089803.88860.01383.61980.0109903.87000.01743.65250.01241003.89790.01573.65540.0093Table 3
RMSE of different algorithms in different numbers of interpolation points.
RMSE (m)Interpolation numbers0919RMSE (square)3.89831.65200.0184RMSE (circle)3.64591.56490.0104RMSE (asterisk)3.54300.00920.0072RMSE (cross)3.47580.00840.0068In Figure4, abscissa was the sampling frequency, and the values were 250 MHz, 500 MHz, and 1000 MHz (1 GHz). The ordinate was RMSE (m). We set the carrier frequency to 50 MHz. Table 4 lists the details of each point in Figure 4. From the simulation results, it could be seen that the positioning accuracy was not high in all three sampling frequencies when there were no interpolation points, whether the Chauvenet’s criterion or two-point coordination is used. The accuracy of the 250 MHz sampling frequency with 9/19 interpolation points was close to that of the 2.5 GHz/5 GHz sampling frequency without interpolation points. When the positioning accuracy is relatively low, the effect of using two-point coordination to improve the positioning accuracy is obviously compared with the Chauvenet’s criterion. When the accuracy is over about 3.0 m, using two-point coordination can improve the positioning accuracy by about 0.2 m–0.3 m. Due to the improved sinc algorithm, the positioning accuracy improves obviously. The effect of using two-point coordination to improve the positioning accuracy is not obvious comparing with the Chauvenet’s criterion. When the accuracy is under 1.0 m, using two-point coordination can improve the positioning accuracy by about 0.001 m–0.003 m.Table 4
RMSE of different algorithms in different sampling frequencies.
RMSE (m)SF (MHz)2505001000RMSE (square)4.14633.89833.5430RMSE (circle)3.81723.64593.4758RMSE (asterisk)2.81201.65200.0092RMSE (cross)2.62741.56490.0084The positioning accuracy and positioning time of the have different degrees of improvement compared between Chauvenet’s criterion, which is used in theSmall Range High Precision Positioning Algorithm Based on Improved Sinc and coordination algorithm.


## 5. Conclusion

This paper introduces the present situation and the future development of the wireless location, summarizes the related technologies and algorithms, and proposes a coordination localization algorithm. The analysis and simulation results show that if the coordination algorithm is used in the data processing, it can improve the positioning accuracy of the system. The primary contribution was that a two-point coordination algorithm is proposed that could greatly increase positioning accuracy when the sampling frequency was low.The problem is that when we use the improved sinc interpolation positioning algorithm, the positioning accuracy can meet the requirements, but we have to wait for a certain amount of time to form a new chip to calculate the TDOA values even though we use the coordination algorithm to optimize the positioning time. And when there were more than one target nodes in the positioning area, it would take longer to estimate a position. The next work will be to continue to study the relationship between positioning accuracy and positioning time.



---

*Source: 8732006-2021-06-16.xml*