Simple trilateration algorithm in simulated 3D space

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