Simple trilateration algorithm in simulated 3D space - algorithm

Question

Context: I am working on implementing a navigation system for the mobile computers added by OpenComputers, a Minecraft mod. For those not familiar with the mod, it basically adds a variety of Lua-programmable, upgradable computers, including mobile ones - namely, robots, drones, and tablets. One of the many challenges often arising when trying to program robots and drones to carry out an autonomous task is to ensure they know their coordinates at all times.
The simplest solution would be to use Navigation upgrade, which does exactly that - provides the computer with its exact coordinates relative to the center of the map it was crafted with. It has two major downsides, however - it takes up a Tier II upgrade slot, which is no small thing, and is limited to the area of the map. The latter is more or less acceptable, but still makes this navigation method unavailable for some usage cases.
Another solution would be to make computers memorise their coordinates once and then keep track of their movements, but this has a number of potential caveats, too - you have to control all movement through custom subroutines or use hacks to intercept component calls, you can't move a computer without having to manually enter the coordinates each time, there are some precision errors for the drones and this won't work at all for the tablets.
A third method - the one I'm working on - is similar to the real life GPS. It's based on the fact that computers can be upgraded with wireless network cards to be able to send messages to each other within a quite large distance of 400 blocks, and along with the message itself they receive an exact distance (floating point number, in blocks) between the sender and the receiver. If we designate some fixed computers as "satellites" which constantly broadcast their position, we can make a mobile computer able to trilaterate its exact position using information from 4+ satellites.
This approach is scalable (you can just keep adding more satellites to the network to expand its coverage), does not take up an extra upgrade slot for navigation purposes only (since many mobile computers are upgraded with wireless network cards already) and precise, which gives it a clear advantage over two other methods. However, it requires some surprisingly complicated calculations, and this is where I'm getting stuck.
Problem: I need to find a trilateration algorithm (ideally coming with a code example) which would allow any mobile computer to calculate its position (within a margin of error of ~0.25 blocks) knowing the coordinates of the designated "satellites" and the distances to them. We assume that all computers and satellites are equipped with Tier II wireless cards (i.e. that they can send messages to each other within the total range of 400 blocks and know the distance between a sender and itself with the precision allowed by float32 numbers). The solution will be coded in pure Lua without accessing any third-party services, so packets like Mathematica are a no-go. Currently I'm betting on some sort of a fitting method, though I don't know how to implement one and how well it could be adapted to the possibility of some satellites in range broadcasting a wrong position.
On the most basic level, we can assume there are 4 satellites which constantly and correctly broadcast their position, are set apart from each other at a moderate distance and do not lie on a single 2D plane. There are some optional conditions which the algorithm should ideally be able to adapt to - see section below.
Bonus points for:
Making the algorithm small enough to fit into the 2KB memory of the drone (assuming UTF8 encoding). It should take well less space than that, though, so that a main program could fit too. The smaller, the better.
Making an algorithm which allows the satellites to be very close to each other and to have non-integer coordinates (to allow for replacing several fixed satellites with one constantly moving robot or drone, or for making the mobile computer itself move as it takes measurements from a single satellite).
Making an algorithm which allows for less than 4 satellites to be present, assuming the position can be determined already - for instance, if the mobile computer in question is a robot and all but one possible positions are below or above the allowed height range for blocks (y<0 or y>255). Such setup is possible if there are three satellites positioned at the height of, say, y=255.
Making an algorithm which is resistant to some satellites broadcasting slightly wrong position (a minor mistake in the setup). Given the presense of enough correct measurements, the algorithm should deduce the correct position or flatly out throw an error. Ideally, it can also log the location of the "off" satellite.
Making an algorithm which is resistant to a simultaneous presence of two or more groups of satellites correctly broadcasting their positions in different systems of coordinates (a major mistake in the setup). Each network has a (supposedly unique) identificator that allows to distinguish between different networks independently set up by different players (or, well, just one). If, however, they didn't bother to properly set the identificators, different signals can mix up, confusing the mobile computer. The resistant algorithm should therefore be able to detect this situation and either flatly out throw an error or distinguish between different networks (then it could be fine-tuned to suit the purposes of a specific application - i.e. refuse to load, choose the closest network, choose the biggest network, prompt user or controlling server, etc.).
What I tried: Besides trying to solve the problem by myself, I've also tried to look up a fitting solution on the internet. However, none of the solutions I could find were fit for this task.
Most of the stuff I've found by googling up "trilateration algorithms" was dealing with the real-life GPS systems - that is, using just 2 coordinates, strongly accounting for errors and not giving enough precision in general.
Some, on the opposite, were purely mathematical, suggesting building series of equations to find the intersection points of the spheres. Sadly, as far as my weak mathematical background allows me to understand, this approach does not account for precision errors of floating numbers - circles do not quite intersect, points are not quite in the same locations, and so the equations do not have solutions.
Some seemed to explain the solution, but involved a lot of complicated math I couldn't understand and did not include an exact algorithm or at least a code example.
At least one used external packets like Mathematica, which, again, are not available in this case.
If I left some important points unclear, please leave a comment so that I could improve the question. Thanks in advance!

Answer 1

Such a trilateration system was already developed for a different mod, named ComputerCraft. Since its propably not compatible for your specific problem, you will have to modify and adapt its logic but the algorithm itself should work.
Here is the Source Code
CHANNEL_GPS = 65534
local function trilaterate( A, B, C )
local a2b = B.vPosition - A.vPosition
local a2c = C.vPosition - A.vPosition
if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
return nil
end
local d = a2b:length()
local ex = a2b:normalize( )
local i = ex:dot( a2c )
local ey = (a2c - (ex * i)):normalize()
local j = ey:dot( a2c )
local ez = ex:cross( ey )
local r1 = A.nDistance
local r2 = B.nDistance
local r3 = C.nDistance
local x = (r1*r1 - r2*r2 + d*d) / (2*d)
local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
local result = A.vPosition + (ex * x) + (ey * y)
local zSquared = r1*r1 - x*x - y*y
if zSquared > 0 then
local z = math.sqrt( zSquared )
local result1 = result + (ez * z)
local result2 = result - (ez * z)
local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
return rounded1, rounded2
else
return rounded1
end
end
return result:round( 0.01 )
end
local function narrow( p1, p2, fix )
local dist1 = math.abs( (p1 - fix.vPosition):length() - fix.nDistance )
local dist2 = math.abs( (p2 - fix.vPosition):length() - fix.nDistance )
if math.abs(dist1 - dist2) < 0.01 then
return p1, p2
elseif dist1 < dist2 then
return p1:round( 0.01 )
else
return p2:round( 0.01 )
end
end
function locate( _nTimeout, _bDebug )
-- Let command computers use their magic fourth-wall-breaking special abilities
if commands then
return commands.getBlockPosition()
end
-- Find a modem
local sModemSide = nil
for n,sSide in ipairs( rs.getSides() ) do
if peripheral.getType( sSide ) == "modem" and peripheral.call( sSide, "isWireless" ) then
sModemSide = sSide
break
end
end
if sModemSide == nil then
if _bDebug then
print( "No wireless modem attached" )
end
return nil
end
if _bDebug then
print( "Finding position..." )
end
-- Open a channel
local modem = peripheral.wrap( sModemSide )
local bCloseChannel = false
if not modem.isOpen( os.getComputerID() ) then
modem.open( os.getComputerID() )
bCloseChannel = true
end
-- Send a ping to listening GPS hosts
modem.transmit( CHANNEL_GPS, os.getComputerID(), "PING" )
-- Wait for the responses
local tFixes = {}
local pos1, pos2 = nil, nil
local timeout = os.startTimer( _nTimeout or 2 )
while true do
local e, p1, p2, p3, p4, p5 = os.pullEvent()
if e == "modem_message" then
-- We received a reply from a modem
local sSide, sChannel, sReplyChannel, tMessage, nDistance = p1, p2, p3, p4, p5
if sSide == sModemSide and sChannel == os.getComputerID() and sReplyChannel == CHANNEL_GPS and nDistance then
-- Received the correct message from the correct modem: use it to determine position
if type(tMessage) == "table" and #tMessage == 3 then
local tFix = { vPosition = vector.new( tMessage[1], tMessage[2], tMessage[3] ), nDistance = nDistance }
if _bDebug then
print( tFix.nDistance.." metres from "..tostring( tFix.vPosition ) )
end
if tFix.nDistance == 0 then
pos1, pos2 = tFix.vPosition, nil
else
table.insert( tFixes, tFix )
if #tFixes >= 3 then
if not pos1 then
pos1, pos2 = trilaterate( tFixes[1], tFixes[2], tFixes[#tFixes] )
else
pos1, pos2 = narrow( pos1, pos2, tFixes[#tFixes] )
end
end
end
if pos1 and not pos2 then
break
end
end
end
elseif e == "timer" then
-- We received a timeout
local timer = p1
if timer == timeout then
break
end
end
end
-- Close the channel, if we opened one
if bCloseChannel then
modem.close( os.getComputerID() )
end
-- Return the response
if pos1 and pos2 then
if _bDebug then
print( "Ambiguous position" )
print( "Could be "..pos1.x..","..pos1.y..","..pos1.z.." or "..pos2.x..","..pos2.y..","..pos2.z )
end
return nil
elseif pos1 then
if _bDebug then
print( "Position is "..pos1.x..","..pos1.y..","..pos1.z )
end
return pos1.x, pos1.y, pos1.z
else
if _bDebug then
print( "Could not determine position" )
end
return nil
end
end
From https://github.com/dan200/ComputerCraft/blob/master/src/main/resources/assets/computercraft/lua/rom/apis/gps.lua
Ask if you have any specific questions about the source code.

Answer 2

Function trilateration expects list of satellites (their coordinates and distances from mobile computer) and previous coordinates of the mobile computer.
Gather only satellites from your own group, exclude satellites from all other groups.
Some of your satellites might send incorrect data, it's OK.
If there is not enough satellites accessible, the function returns nil as it can't determine the current position.
Otherwise, the function returns current coordinates of the mobile computer and list of indices of satellites been blamed as incorrect.
In case of ambiguity the new position is selected as nearest one to the previous position of the mobile computer.
The output coordinates are integer, Y coordinate is limited to the range 0..255
The following conditions should be satisfied for proper trilateration:
(number_of_correct_satellites) must be >= 3
(number_of_correct_satellites) must be >= 4 if at least one incorrect satellite exists
(number_of_correct_satellites) must be > (number_of_incorrect_satellites)
Recognizing an incorrect satellite is costly CPU operation.
Once a satellite is recognized as incorrect, please store it in some blacklist and exclude it from all future calculations.
do
local floor, exp, max, min, abs, table_insert = math.floor, math.exp, math.max, math.min, math.abs, table.insert
local function try_this_subset_of_sat(satellites, is_sat_incorrect, X, Y, Z)
local last_max_err, max_err = math.huge
for k = 1, math.huge do
local oldX, oldY, oldZ = X, Y, Z
local DX, DY, DZ = 0, 0, 0
max_err = 0
for j = 1, #satellites do
if not is_sat_incorrect[j] then
local sat = satellites[j]
local dx, dy, dz = X - sat.x, Y - sat.y, Z - sat.z
local d = (dx*dx + dy*dy + dz*dz)^0.5
local err = sat.distance - d
local e = exp(err+err)
e = (e-1)/(e+1)/(d+1)
DX = DX + dx*e
DY = DY + dy*e
DZ = DZ + dz*e
max_err = max(max_err, abs(err))
end
end
if k % 16 == 0 then
if max_err >= last_max_err then
break
end
last_max_err = max_err
end
local e = 1/(1+(DX*DX+DY*DY+DZ*DZ)^0.5/max_err)
X = X + DX*e
Y = max(0, min(255, Y + DY*e))
Z = Z + DZ*e
if abs(oldX - X) + abs(oldY - Y) + abs(oldZ - Z) <= 1e-4 then
break
end
end
return max_err, floor(X + 0.5), floor(Y + 0.5), floor(Z + 0.5)
end
local function init_set(is_sat_incorrect, len, ctr)
for j = 1, len do
is_sat_incorrect[j] = (j <= ctr)
end
end
local function last_combination(is_sat_incorrect)
local first = 1
while not is_sat_incorrect[first] do
first = first + 1
end
local last = first + 1
while is_sat_incorrect[last] do
last = last + 1
end
if is_sat_incorrect[last] == nil then
return true
end
is_sat_incorrect[last] = true
init_set(is_sat_incorrect, last - 1, last - first - 1)
end
function trilateration(list_of_satellites, previous_X, previous_Y, previous_Z)
local N = #list_of_satellites
if N >= 3 then
local is_sat_incorrect = {}
init_set(is_sat_incorrect, N, 0)
local err, X, Y, Z = try_this_subset_of_sat(list_of_satellites, is_sat_incorrect, previous_X, previous_Y, previous_Z)
local incorrect_sat_indices = {}
if err < 0.1 then
return X, Y, Z, incorrect_sat_indices
end
for incorrect_ctr = 1, min(floor((N - 1) / 2), N - 4) do
init_set(is_sat_incorrect, N, incorrect_ctr)
repeat
err, X, Y, Z = try_this_subset_of_sat(list_of_satellites, is_sat_incorrect, previous_X, previous_Y, previous_Z)
if err < 0.1 then
for j = 1, N do
if is_sat_incorrect[j] then
table_insert(incorrect_sat_indices, j)
end
end
return X, Y, Z, incorrect_sat_indices
end
until last_combination(is_sat_incorrect)
end
end
end
end
Usage example:
-- assuming your mobile computer previous coordinates were 99 120 100
local previous_X, previous_Y, previous_Z = 99, 120, 100
-- assuming your mobile computer current coordinates are 111 112 113
local list_of_satellites = {
{x=22, y=55, z=77, distance=((111-22)^2+(112-55)^2+(113-77)^2)^0.5}, -- correct satellite
{x=35, y=99, z=42, distance=((111-35)^2+(112-99)^2+(113-42)^2)^0.5}, -- correct satellite
{x=44, y=44, z=44, distance=((111-94)^2+(112-94)^2+(113-94)^2)^0.5}, -- incorrect satellite
{x=10, y=88, z=70, distance=((111-10)^2+(112-88)^2+(113-70)^2)^0.5}, -- correct satellite
{x=54, y=54, z=54, distance=((111-64)^2+(112-64)^2+(113-64)^2)^0.5}, -- incorrect satellite
{x=91, y=33, z=15, distance=((111-91)^2+(112-33)^2+(113-15)^2)^0.5}, -- correct satellite
}
local X, Y, Z, list_of_incorrect_sat_indices = trilateration(list_of_satellites, previous_X, previous_Y, previous_Z)
if X then
print(X, Y, Z)
if #list_of_incorrect_sat_indices > 0 then
print("Satellites at the following indices are incorrect: "..table.concat(list_of_incorrect_sat_indices, ","))
end
else
print"Not enough satellites"
end
Output:
111 112 113
Satellites at the following indices are incorrect: 3,5