André Chr. Andersen's paper "Signal Emitter Positioning Using Multilateration"

This article was published on André Chr. Andersen's blog, http://blog.andersen.im/2012/07/signal-emitter-positioning-using-multilateration .

Unfortunately, his blog cannot be opened now. Maybe he has no longer maintained it.

The TDOA algorithm used by our first version of the positioning engine comes from this article of his. André Chr. Andersen also provided MATLAB code at the end of the article, and Paul Hayes translated his MATLAB code toy into Python code, which can be found on github https://github.com/paulhayes/MultilaterationExample .

This algorithm is for ultrasonic positioning, but it is essentially the same as UWB positioning. The important thing is that this is a simple and easy-to-implement Multilateration algorithm, which can be used in UWB coordinate calculations after modification.

Many thanks to André Chr. Andersen and Paul Hayes. Without their algorithms and codes, our project might have been terminated early.

Andersen's algorithm works, but there are some problems. Mainly reflected in two aspects:

1. The calculated coordinates are not 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.
2. Errors may occur during the iteration process in the algorithm, resulting in coordinates sometimes not being calculated. Due to the influence of various factors, the TDOA value we get is definitely inaccurate and has errors. In fact, what we expect to get is an approximate value. But the error is too large, and sometimes outrageous coordinates are obtained, which is more embarrassing.

We later developed two algorithms ourselves, and finally adopted the least squares method, which achieved a better 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-positioning-using-multilateration).
#
############################

from numpy import *
from numpy.linalg import *
import json
#speed of sound in medium
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)
del ijs[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