To review, in the previous articles we introduced the hardware design of the anchor and the tag, as well as the firmware design of the anchor and the tag, including the key points of clock synchronization, etc. Now we will introduce the design of the location engine, of course, the focus is on the TDOA algorithm.
Location engine design-TDOA algorithm
When using the DW1000 location system, the commonly used location solutions are TOF or TDOA.
The TOF solution is introduced in the routines provided by DecaWave and the Trek1000 code. The distance between the Tag and several Anchors is obtained by ranging, and then the Trilateration algorithm is used to calculate the coordinates of the Tag. For the specific algorithm, see wikipedia: https://en.wikipedia.org/wiki/Trilateration.
We use the TDOA solution. After the Tag sends out a location UWB packet, it is received by several Anchors in the location area. Each Anchor records the timestamp of receiving the UWB packet and forward it to the location engine RTLE. The RTLE calculates the coordinates of the Tag based on the time difference of each Anchor receiving the UWB packet. Usually, this coordinate calculation algorithm is called Multilateration. For a detailed introduction, please refer to https://en.wikipedia.org/wiki/Pseudo-range_multilateration. Wikipedia introduces GPS, which has the same principle as UWB.
In addition, TDOA location has two solutions: downlink and uplink. GPS uses the downlink solution, that is, the satellite sends out a location signal. After the GPS receiver receives the location signal sent by each satellite, it calculates its own coordinates based on the time difference of the signal received from each satellite; the uplink is that the located tag sends out a location signal, and each is responsible for receiving it, and the coordinate calculation location engine is calculated centrally.
Both the uplink and downlink solutions have their own advantages and disadvantages. For UWB location, in most cases, the system wants to know where the tag is, not the tag wants to know where it is; in addition, the uplink method has low requirements for the tag. The tag only needs to send out a UWB signal, and does not need too much computing power. The power requirement is also low. For example, the tag may be made into a work card or a bracelet. If the uplink solution is used, we can make the tag very small.
On the contrary, if the downlink solution is used, the Tag is required to have the ability to calculate coordinates, and the requirements for the MCU will be relatively high; because the coordinates need to be calculated frequently, the sleep time will also be very short. These factors will have a relatively large power consumption. Think about the previous GPS terminals, which were all large handheld devices with very short battery standby time. As time goes by, technology continues to advance, and the most important thing is that the market is getting bigger and bigger, and developers have profits and motivation to continue research and development. In recent years, GPS can be made very small and integrated into mobile phones.
There are many papers involving multilateration algorithms. Almost every paper will say that its algorithm is very powerful, list a bunch of data to prove that it is very powerful, and then list a bunch of mathematical formulas that people can’t understand.
For TDOA systems, we must have a concept: time difference is distance difference!
The time difference between the radio waves emitted by the tag and different anchors is essentially because the distance between the tag and each anchor is different, and the radio waves need time to fly. Because the distance is different, the time it takes for the radio waves to reach each anchor will be different. We can multiply the time difference by the speed of light (actually the speed of radio waves in the air) to get the distance difference.
Andersen’s algorithm
The first Multilateration algorithm we used was designed by André Chr. Andersen. This article was published on André Chr. Andersen’s blog, http://blog.andersen.im/2012/07/signal-emitter-location-using-multilateration. Unfortunately, his blog is not open now, maybe he is no longer maintaining it. André Chr. Andersen also provided MATLAB code at the end of the article, and Paul Hayes translated this MATLAB code into Python code, which can be found on github https://github.com/paulhayes/MultilaterationExample.
This algorithm is for ultrasonic location, but it is essentially the same as UWB location. The important thing is that this is a simple and easy-to-implement Multilateration algorithm, which can be used for UWB coordinate calculation after some modification. Many thanks to André Chr. Andersen and Paul Hayes. If it weren’t for their algorithm and code, maybe our project would have been terminated early.
Andersen’s algorithm can be used, but there are some problems. Mainly reflected in two aspects:
- The calculated coordinates are not very accurate. The tag is more accurate when it is in the center of the area, but when the standard is at the edge of the area, the error becomes very large.
- Errors may occur during the iteration process of the algorithm, resulting in sometimes inability to calculate the coordinates. Due to various factors, the TDOA value we get is definitely inaccurate and has errors. In fact, we expect to get an approximate value. However, the error is too large, and sometimes very outrageous coordinates are obtained, which is embarrassing.
Later, we developed two algorithms ourselves, and finally adopted the least squares method, which achieved a good balance between speed and accuracy.
The following is the Python code rewritten by Paul Hayes. .
###########################
#
# Python rewrite of multilateration technique by André Andersen in his [blog post](http://blog.andersen.im/2012/07/signal-emitter-location-using-multilateration).
#
############################
from numpy import *
from numpy.linalg import *
import json
#speedofsoundinmedium
v=3450
numOfDimensions = 3
nSensors = 5
region = 3
sensorRegion=2
#choose a random sensor location
emitterLocation = region * ( random.random_sample(numOfDimensions) - 0.5 )
sensorLocations = [ sensorRegion * ( random.random_sample(numOfDimensions)-0.5 ) for n in range(nSensors) ]
p = matrix( sensorLocations ).T
#Time from emitter to each sensor
sensorTimes = [ sqrt( dot(location-emitterLocation,location-emitterLocation) ) / v for location in sensorLocations ]
c = argmin(sensorTimes)
cTime = sensorTimes[c]
#sensors delta time relative to sensor c
t = sensorDeltaTimes = [ sensorTime - cTime for sensorTime in sensorTimes ]
ijs = range(nSensors)
delijs[c]
A = zeros([nSensors-1,numOfDimensions])
b = zeros([nSensors-1,1])
iRow = 0
rankA = 0
for i in ijs:
for j in ijs: A[iRow,:] = 2*( v*(t[j])*(p[:,i]-p[:,c]).T - v*(t[i])*(p[:,j]-p[:,c]).T )
b[iRow] = v*(t[i])*(v*v*(t[j])**2-p[:,j].T*p[:,j]) + \
(v*(t[i])-v*(t[j]))*p[:,c].T*p[:,c] + \
v*(t[j])*(p[:,i].T*p[:,i]-v*v*(t[i])**2)
rankA = matrix_rank(A)
if rankA >= numOfDimensions :
break
iRow += 1
if rankA >= numOfDimensions: break
calculatedLocation = asarray( lstsq(A,b)[0] )[:,0]
print "Emitter location: %s " % emitterLocation
print "Calculated position of emitter: %s " % calculatedLocation
This code can be easily translated into Java. Our first version of the location engine was written in Java. In fact, it didn’t take me much time to translate it into Java, and you should be able to do it too.
After using Andersen’s algorithm for a while, we found that the biggest problem was that when the tag was close to the edge of the polygon surrounded by the anchor, the error was relatively large. Then, we also found that we could expand the coordinates of the anchor from the polygon to get more accurate coordinates. This is not scientific.
Coordinate quality assessment
At this time, I developed a coordinate quality assessment algorithm to evaluate the quality of the calculated coordinates. After the tag coordinates are calculated, we don’t know whether it is correct or not, and if there is an error, how big the error is. What to do?
In fact, it is also very simple. We have a coordinate. We can calculate the distance from this coordinate to each anchor. We can get the distance difference from this coordinate to each anchor. This distance difference is actually the time difference, because the time difference is essentially the distance difference. We compare this distance difference (time difference) with the time difference (distance difference) of the actual signal of the anchor, and we can get a score, which represents the quality of the calculated coordinates.
Coordinate quality assessment is very important, almost as important as the TDOA algorithm. It will play a very important role in the entire system.
For example, there are two adjacent location areas. When the tag is on the boundary, both areas will calculate the coordinates of the tag. Usually, the values of these two coordinates will not be the same, but there will be differences. So, which coordinate is correct? Through coordinate quality assessment, we can know which coordinate is closer to the true value. Of course, this statement is simplified, and the actual situation will be much more complicated. For example, the tag moves from area A to area B. On the boundary, if we simply judge which coordinate has higher quality as described above, we will output it. Then, if we connect the output tag coordinates on the map, we will see a jump on the boundary, which is the moment when the tag switches from area A to area B. Due to the existence of errors, this quality assessment is not necessarily accurate. If the tag stays on the boundary for a while, we may see that the output coordinates keep switching between areas A and B, jumping back and forth. I will discuss what to do in this case in a future article.
Second Multilateration Algorithm
Now we can introduce our second algorithm, which is very simple and intuitive. We divide the location area into 4 blocks, similar to the 4 quadrants of Cartesian coordinates, and we now have 4 small rectangles. We assume that the center points of these four rectangles may be the coordinates of the tag. We calculate the quality of these four points respectively. The point with the highest quality should be in the area corresponding to it. Then we divide the rectangle into four pieces, calculate the quality of the center points of the four smaller rectangles, and take the point with the highest quality. The tag should be in this small rectangle… and so on, until some time, for example, the side length of the rectangle is less than 30cm, then we can assume that the center coordinates of the rectangle less than 30cm are the coordinates of the tag. Isn’t it very simple? However, there is an assumption here that the tag is in the rectangle with the highest quality coordinates! Is this assumption valid? I don’t know how to prove it mathematically, but it is valid in fact, because this algorithm can work normally. However, there is a problem with this algorithm, that is, the amount of calculation is relatively large, and the coordinate calculation speed is relatively slow.
The key point of this algorithm is that we regard this coordinate calculation process as a mathematical function, which is convergent, and it finally converges to a point, which is the coordinate we want.
The third multilateration algorithm
Then we developed the third algorithm. In fact, this algorithm is similar to the second one, but the convergence method is different. We use the least squares method. The least squares method has a faster convergence speed and does not need to divide into 4 rectangles. Based on the initial parameters, we get a vector that points to the direction of the coordinates. With continuous iteration, this vector keeps approaching the true coordinates. When the iteration ends,, this vector is the coordinate we need.
In essence, the algorithm of Multilateration does not solve equations in the standard way. We have all learned to solve equations in middle school mathematics. For multivariate equations, we can use some techniques to eliminate variables, find the value of a certain unknown number, and then substitute it into other equations to find the value of other unknown numbers. However, this routine cannot be used in practice for two reasons: First, the error is large. In a strict mathematical sense, this equation has no solution. In fact, what we require is an approximate value; Second, we have redundant parameters. If you calculate 2D coordinates, 3 anchors are enough. But there may be 4 or more anchors that can provide data. The data provided by the extra anchors can allow us to get more approximate (accurate) coordinates. But if you use the standard equation solution method, these redundant parameters are contradictory.
The practical algorithms of Multilateration basically try to find approximate values, and most of them use approximation methods for iteration. Estimate the range of the solution through some initial parameters, narrow the range, and then estimate the range of the solution again to get a smaller range. Iterate continuously and end the iteration after a certain condition is met. This condition is either to get a solution with suitable accuracy; or to fail to solve it because the error is too large and the function cannot converge.
These few articles have introduced the most valuable technologies for using TDOA technology to achieve UWB precise positioning. If you don’t know UWB before, it will seem more laborious, because I basically just introduce the technical points instead of popularizing science. If you are developing a similar system, you should be able to start writing code.
Next, I will write a few more articles to introduce some technical details.
The content of these articles looks a bit messy, and it is indeed a bit messy. I understand those authors who write online serial novels. It is too difficult to write whatever you think of and have coherence. Unlike writing technical solutions at ordinary times, you can revise and ponder repeatedly.
