模拟 3D 中的简单三边测量算法 space
Simple trilateration algorithm in simulated 3D space
Context：我正在为 OpenComputers 添加的移动计算机实现导航系统，一个 Minecraft mod。对于那些不熟悉 mod 的人，它基本上添加了各种 Lua-可编程、可升级的计算机，包括移动计算机——即机器人、无人机和平板电脑。在尝试对机器人和无人机进行编程以执行自主任务时经常出现的众多挑战之一是确保它们始终知道自己的坐标。
最简单的解决方案是使用导航升级，这正是这样做的 - 为计算机提供相对于制作它的地图中心的精确坐标。然而，它有两个主要缺点 - 它占用了一个 II 级升级槽，这不是一件小事，并且仅限于地图区域。后者或多或少是可以接受的，但仍然使这种导航方法在某些使用情况下不可用。
另一种解决方案是让计算机记住它们的坐标一次，然后跟踪它们的运动，但这也有许多潜在的警告 - 你必须通过自定义子程序控制所有运动或使用 hack 来拦截组件电话，你不能每次都手动输入坐标来移动计算机，无人机有一些精度误差，这对平板电脑根本不起作用。
第三种方法——我正在研究的方法——类似于现实生活中的 GPS。它基于这样一个事实，即可以使用无线网卡升级计算机，以便能够在 400 块的相当大的距离内相互发送消息，并且随着消息本身，它们会收到一个精确的距离（浮点数，以块为单位） ) 发送者和接收者之间。如果我们将一些固定计算机指定为 "satellites"，不断广播它们的位置，我们可以使移动计算机能够使用来自 4 颗以上卫星的信息对其精确位置进行三边测量。
这种方法是可扩展的（你可以不断地向网络中添加更多的卫星来扩大它的覆盖范围），不占用额外的升级插槽，仅用于导航目的（因为许多移动计算机已经升级了无线网卡) 和精确，这使它比其他两种方法具有明显的优势。但是，它需要一些非常复杂的计算，这就是我遇到困难的地方。
问题：我需要找到一个三边测量算法（最好附带一个代码示例），它可以让任何移动计算机计算其位置（误差范围为 ~ 0.25 块）知道指定 "satellites" 的坐标和到它们的距离。我们假设所有的计算机和卫星都配备了 Tier II 无线网卡（即它们可以在 400 个区块的总范围内相互发送消息，并且知道发送者与其自身之间的距离，其精度为 float32 数字所允许的精度）。该解决方案将在不访问任何第三方服务的情况下以纯 Lua 编码，因此像 Mathematica 这样的数据包是不行的。目前我正在押注某种拟合方法，但我不知道如何实施，也不知道它如何适应范围内某些卫星广播错误位置的可能性。
在最基本的层面上，我们可以假设有 4 颗卫星不断正确地广播它们的位置，彼此以 mod 速率距离分开并且不位于单个二维平面上.理想情况下，算法应该能够适应一些可选条件 - 请参阅下面的部分。
奖励积分：
- 使算法足够小以适应无人机的 2KB 内存（假设 UTF8 编码）。不过，它应该比 space 少得多，这样主程序也可以适应。越小越好
- 制定一种算法，使卫星彼此非常接近并具有非整数坐标（以允许用一个不断移动的机器人或无人机代替多个固定卫星，或使移动计算机本身移动因为它从单个卫星进行测量）。
- 假设已经可以确定位置 - 例如，如果所讨论的移动数据终端是机器人，并且除了一个可能的位置之外，所有可能的位置都低于或高于块的允许高度范围（y<0 或 y>255）。如果三颗卫星位于例如 y=255.
的高度，则这种设置是可能的
- 制作一种算法可以抵抗某些卫星广播的轻微错误位置（设置中的小错误）。如果存在足够多的正确测量值，算法应该推断出正确的位置或完全排除错误。理想情况下，它还可以记录 "off" 卫星的位置。
- 制定一种算法，可以抵抗两组或更多组卫星同时存在，正确地广播它们在不同坐标系中的位置（设置中的一个主要错误）。每个网络都有一个（据说是唯一的）标识符，可以区分不同玩家（或者，好吧，只有一个）独立建立的不同网络。然而，如果他们不费心去正确设置标识符，不同的信号就会混淆，从而使移动数据终端感到困惑。因此，抵抗算法应该能够检测到这种情况，并且要么直接抛出错误，要么区分不同的网络（然后可以对其进行微调以适应特定应用程序的目的——即拒绝加载、选择最近的网络、选择最大的网络，提示用户或控制服务器等）。
我试过的方法: 除了尝试自己解决问题外，我还尝试在网上查找合适的解决方案。但是，我能找到的 none 解决方案适合此任务。
- 我通过谷歌搜索 "trilateration algorithms" 发现的大部分内容都与现实生活中的 GPS 系统有关——也就是说，仅使用 2 个坐标，严重考虑了错误并且通常没有提供足够的精度.
- 相反，有些是纯数学的，建议建立一系列方程来找到球体的交点。可悲的是，据我微弱的数学背景允许我理解，这种方法没有考虑浮点数的精度误差——圆不 完全 相交，点不 相当在相同的位置，所以方程没有解。
- 有些似乎解释了解决方案，但涉及很多我无法理解的复杂数学，并且没有包括确切的算法或至少没有代码示例。
- At least one 使用外部数据包，如 Mathematica，在这种情况下同样不可用。
如果我遗漏了一些要点，请发表评论以便我改进问题。提前致谢！
已经为另一个名为 ComputerCraft 的 mod 开发了这样的三边测量系统。由于它可能与您的特定问题不兼容，您将不得不 mod 确定并调整其逻辑，但算法本身应该可以工作。
这是源代码
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
询问您是否对源代码有任何具体问题。
函数 trilateration 需要卫星列表（它们的坐标和与移动数据终端的距离）和移动数据终端的先前坐标。
仅收集您自己组的卫星，排除所有其他组的卫星。
您的某些卫星可能会发送不正确的数据，没关系。
如果没有足够的卫星可用，函数returns nil 因为它无法确定当前位置。
否则，returns移动终端当前坐标和卫星索引列表功能被指责为不正确。
如果不明确，新位置将被选择为最接近移动数据终端先前位置的位置。
输出坐标为整数，Y坐标限制在0..255
范围内
正确的三边测量应满足以下条件：
- (number_of_correct_satellites) 必须 >= 3
- (number_of_correct_satellites) 如果至少存在一颗不正确的卫星，则必须 >= 4
- (number_of_correct_satellites) 必须 > (number_of_incorrect_satellites)
识别不正确的卫星是一项昂贵的 CPU 操作。
一旦卫星被识别为不正确，请将其存储在某个黑名单中，并将其排除在所有未来的计算之外。
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
用法示例：
-- 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
输出：
111 112 113
Satellites at the following indices are incorrect: 3,5
Context：我正在为 OpenComputers 添加的移动计算机实现导航系统，一个 Minecraft mod。对于那些不熟悉 mod 的人，它基本上添加了各种 Lua-可编程、可升级的计算机，包括移动计算机——即机器人、无人机和平板电脑。在尝试对机器人和无人机进行编程以执行自主任务时经常出现的众多挑战之一是确保它们始终知道自己的坐标。
最简单的解决方案是使用导航升级，这正是这样做的 - 为计算机提供相对于制作它的地图中心的精确坐标。然而，它有两个主要缺点 - 它占用了一个 II 级升级槽，这不是一件小事，并且仅限于地图区域。后者或多或少是可以接受的，但仍然使这种导航方法在某些使用情况下不可用。
另一种解决方案是让计算机记住它们的坐标一次，然后跟踪它们的运动，但这也有许多潜在的警告 - 你必须通过自定义子程序控制所有运动或使用 hack 来拦截组件电话，你不能每次都手动输入坐标来移动计算机，无人机有一些精度误差，这对平板电脑根本不起作用。
第三种方法——我正在研究的方法——类似于现实生活中的 GPS。它基于这样一个事实，即可以使用无线网卡升级计算机，以便能够在 400 块的相当大的距离内相互发送消息，并且随着消息本身，它们会收到一个精确的距离（浮点数，以块为单位） ) 发送者和接收者之间。如果我们将一些固定计算机指定为 "satellites"，不断广播它们的位置，我们可以使移动计算机能够使用来自 4 颗以上卫星的信息对其精确位置进行三边测量。
这种方法是可扩展的（你可以不断地向网络中添加更多的卫星来扩大它的覆盖范围），不占用额外的升级插槽，仅用于导航目的（因为许多移动计算机已经升级了无线网卡) 和精确，这使它比其他两种方法具有明显的优势。但是，它需要一些非常复杂的计算，这就是我遇到困难的地方。
问题：我需要找到一个三边测量算法（最好附带一个代码示例），它可以让任何移动计算机计算其位置（误差范围为 ~ 0.25 块）知道指定 "satellites" 的坐标和到它们的距离。我们假设所有的计算机和卫星都配备了 Tier II 无线网卡（即它们可以在 400 个区块的总范围内相互发送消息，并且知道发送者与其自身之间的距离，其精度为 float32 数字所允许的精度）。该解决方案将在不访问任何第三方服务的情况下以纯 Lua 编码，因此像 Mathematica 这样的数据包是不行的。目前我正在押注某种拟合方法，但我不知道如何实施，也不知道它如何适应范围内某些卫星广播错误位置的可能性。
在最基本的层面上，我们可以假设有 4 颗卫星不断正确地广播它们的位置，彼此以 mod 速率距离分开并且不位于单个二维平面上.理想情况下，算法应该能够适应一些可选条件 - 请参阅下面的部分。
奖励积分：
- 使算法足够小以适应无人机的 2KB 内存（假设 UTF8 编码）。不过，它应该比 space 少得多，这样主程序也可以适应。越小越好
- 制定一种算法，使卫星彼此非常接近并具有非整数坐标（以允许用一个不断移动的机器人或无人机代替多个固定卫星，或使移动计算机本身移动因为它从单个卫星进行测量）。
- 假设已经可以确定位置 - 例如，如果所讨论的移动数据终端是机器人，并且除了一个可能的位置之外，所有可能的位置都低于或高于块的允许高度范围（y<0 或 y>255）。如果三颗卫星位于例如 y=255. 的高度，则这种设置是可能的
- 制作一种算法可以抵抗某些卫星广播的轻微错误位置（设置中的小错误）。如果存在足够多的正确测量值，算法应该推断出正确的位置或完全排除错误。理想情况下，它还可以记录 "off" 卫星的位置。
- 制定一种算法，可以抵抗两组或更多组卫星同时存在，正确地广播它们在不同坐标系中的位置（设置中的一个主要错误）。每个网络都有一个（据说是唯一的）标识符，可以区分不同玩家（或者，好吧，只有一个）独立建立的不同网络。然而，如果他们不费心去正确设置标识符，不同的信号就会混淆，从而使移动数据终端感到困惑。因此，抵抗算法应该能够检测到这种情况，并且要么直接抛出错误，要么区分不同的网络（然后可以对其进行微调以适应特定应用程序的目的——即拒绝加载、选择最近的网络、选择最大的网络，提示用户或控制服务器等）。
我试过的方法: 除了尝试自己解决问题外，我还尝试在网上查找合适的解决方案。但是，我能找到的 none 解决方案适合此任务。
- 我通过谷歌搜索 "trilateration algorithms" 发现的大部分内容都与现实生活中的 GPS 系统有关——也就是说，仅使用 2 个坐标，严重考虑了错误并且通常没有提供足够的精度.
- 相反，有些是纯数学的，建议建立一系列方程来找到球体的交点。可悲的是，据我微弱的数学背景允许我理解，这种方法没有考虑浮点数的精度误差——圆不 完全 相交，点不 相当在相同的位置，所以方程没有解。
- 有些似乎解释了解决方案，但涉及很多我无法理解的复杂数学，并且没有包括确切的算法或至少没有代码示例。
- At least one 使用外部数据包，如 Mathematica，在这种情况下同样不可用。
如果我遗漏了一些要点，请发表评论以便我改进问题。提前致谢！
已经为另一个名为 ComputerCraft 的 mod 开发了这样的三边测量系统。由于它可能与您的特定问题不兼容，您将不得不 mod 确定并调整其逻辑，但算法本身应该可以工作。
这是源代码
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
询问您是否对源代码有任何具体问题。
函数 trilateration 需要卫星列表（它们的坐标和与移动数据终端的距离）和移动数据终端的先前坐标。
仅收集您自己组的卫星，排除所有其他组的卫星。
您的某些卫星可能会发送不正确的数据，没关系。
如果没有足够的卫星可用，函数returns nil 因为它无法确定当前位置。
否则，returns移动终端当前坐标和卫星索引列表功能被指责为不正确。
如果不明确，新位置将被选择为最接近移动数据终端先前位置的位置。
输出坐标为整数，Y坐标限制在0..255
正确的三边测量应满足以下条件：
- (number_of_correct_satellites) 必须 >= 3
- (number_of_correct_satellites) 如果至少存在一颗不正确的卫星，则必须 >= 4
- (number_of_correct_satellites) 必须 > (number_of_incorrect_satellites)
识别不正确的卫星是一项昂贵的 CPU 操作。
一旦卫星被识别为不正确，请将其存储在某个黑名单中，并将其排除在所有未来的计算之外。
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
用法示例：
-- 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
输出：
111 112 113
Satellites at the following indices are incorrect: 3,5