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:
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.
If I left some important points unclear, please leave a comment so that I could improve the question. Thanks in advance!
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:
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