Мультилитерационная реализация с неточными данными о расстоянии

Question

Я пытаюсь создать приложение для Android-смартфона, которое использует технологию Apples iBeacon, чтобы определить текущее местоположение внутри помещения. Мне уже удалось получить все доступные маяки и рассчитать расстояние до них по сигналу rssi.

В настоящее время я сталкиваюсь с проблемой, заключающейся в том, что я не могу найти какую-либо библиотеку или реализацию алгоритма, который вычисляет предполагаемое местоположение в 2D, используя 3 (или более) расстояния с фиксированными точками, при условии, что эти расстояния не являются точными (что означает, что три "круга трилатерации" не пересекаются в одной точке).

Я был бы глубоко признателен, если бы кто-нибудь мог опубликовать мне ссылку или ее реализацию на любом распространенном языке программирования (Java, C++, Python, PHP, Javascript или что-то еще). Я уже много читал о stackru по этой теме, но не смог найти ответа, который мне удалось преобразовать в коде (только некоторые математические подходы с матрицами и их инвертирование, вычисления с векторами или тому подобное).

РЕДАКТИРОВАТЬ

Я думал о собственном подходе, который мне подходит, но не такой эффективный и научный. Я перебираю каждый метр (или, как в моем примере 0,1 метра) сетки местоположений и вычисляю вероятность того, что это местоположение будет фактическим положением трубки, сравнивая расстояние этого местоположения со всеми маяками и расстояние, которое я вычисляю, с помощью получил сигнал rssi.

Пример кода:

public Location trilaterate(ArrayList<Beacon> beacons, double maxX, double maxY)
{
    for (double x = 0; x <= maxX; x += .1)
    {
        for (double y = 0; y <= maxY; y += .1)
        {
            double currentLocationProbability = 0;
            for (Beacon beacon : beacons)
            {
                // distance difference between calculated distance to beacon transmitter
                // (rssi-calculated distance) and current location:
                // |sqrt(dX^2 + dY^2) - distanceToTransmitter|
                double distanceDifference = Math
                    .abs(Math.sqrt(Math.pow(beacon.getLocation().x - x, 2)
                                   + Math.pow(beacon.getLocation().y - y, 2))
                         - beacon.getCurrentDistanceToTransmitter());
                // weight the distance difference with the beacon calculated rssi-distance. The
                // smaller the calculated rssi-distance is, the more the distance difference
                // will be weighted (it is assumed, that nearer beacons measure the distance
                // more accurate)
                distanceDifference /= Math.pow(beacon.getCurrentDistanceToTransmitter(), 0.9);
                // sum up all weighted distance differences for every beacon in
                // "currentLocationProbability"
                currentLocationProbability += distanceDifference;
            }
            addToLocationMap(currentLocationProbability, x, y);
            // the previous line is my approach, I create a Set of Locations with the 5 most probable locations in it to estimate the accuracy of the measurement afterwards. If that is not necessary, a simple variable assignment for the most probable location would do the job also
        }
    }
    Location bestLocation = getLocationSet().first().location;
    bestLocation.accuracy = calculateLocationAccuracy();
    Log.w("TRILATERATION", "Location " + bestLocation + " best with accuracy "
                           + bestLocation.accuracy);
    return bestLocation;
}

Конечно, недостатком этого является то, что у меня на полу 300 м ² 30000 мест, по которым мне приходилось перебирать и измерять расстояние до каждого маяка, с которого я получил сигнал (если это будет 5, я делаю 150000 вычислений только для определения единое местоположение). Это много - поэтому я позволю открыть вопрос и буду надеяться на дальнейшие решения или хорошее улучшение существующего решения, чтобы сделать его более эффективным.

Конечно, это не должен быть подход трилатерации, как было в первоначальном названии этого вопроса, также хорошо иметь алгоритм, который включает в себя более трех маяков для определения местоположения (Multilateration).

Answer 1

Вместо того, чтобы принимать во внимание уровни достоверности отдельных маяков, я бы вместо этого попытался назначить общий уровень достоверности для вашего результата после того, как вы сделаете наилучшее возможное предположение, используя имеющиеся данные. Я не думаю, что единственная доступная метрика (воспринимаемая мощность) является хорошим показателем точности. С плохой геометрией или неправильным сигналом вы можете доверять плохим данным. Возможно, было бы лучше определить общий уровень достоверности, основанный на том, насколько хорошо воспринимаемое расстояние до маяков совпадает с рассчитанной точкой, при условии, что вы в равной степени доверяете всем маякам.

Ниже я написал немного Python, который предлагает наилучшее предположение на основе предоставленных данных в случае с 3 маяками, вычисляя две точки пересечения окружностей для первых двух маяков, а затем выбирая точку, которая лучше всего соответствует третьему. Он предназначен для начала работы над проблемой и не является окончательным решением. Если маяки не пересекаются, это немного увеличивает радиус каждого вверх, пока они не встретятся или не будет достигнут порог. Кроме того, он гарантирует, что третий маяк согласен в пределах установленного порога. Для n-маяков я бы выбрал 3 или 4 из самых сильных сигналов и использовал их. Есть тонны оптимизаций, которые можно было бы сделать, и я думаю, что это проблема пробного запуска из-за громоздкой природы маяка.

import math

beacons = [[0.0,0.0,7.0],[0.0,10.0,7.0],[10.0,5.0,16.0]] # x, y, radius

def point_dist(x1,y1,x2,y2):
    x = x2-x1
    y = y2-y1
    return math.sqrt((x*x)+(y*y))

# determines two points of intersection for two circles [x,y,radius]
# returns None if the circles do not intersect
def circle_intersection(beacon1,beacon2):
    r1 = beacon1[2]
    r2 = beacon2[2]
    dist = point_dist(beacon1[0],beacon1[1],beacon2[0],beacon2[1])
    heron_root = (dist+r1+r2)*(-dist+r1+r2)*(dist-r1+r2)*(dist+r1-r2)
    if ( heron_root > 0 ):
        heron = 0.25*math.sqrt(heron_root)
        xbase = (0.5)*(beacon1[0]+beacon2[0]) + (0.5)*(beacon2[0]-beacon1[0])*(r1*r1-r2*r2)/(dist*dist)
        xdiff = 2*(beacon2[1]-beacon1[1])*heron/(dist*dist) 
        ybase = (0.5)*(beacon1[1]+beacon2[1]) + (0.5)*(beacon2[1]-beacon1[1])*(r1*r1-r2*r2)/(dist*dist)
        ydiff = 2*(beacon2[0]-beacon1[0])*heron/(dist*dist) 
        return (xbase+xdiff,ybase-ydiff),(xbase-xdiff,ybase+ydiff)
    else:
        # no intersection, need to pseudo-increase beacon power and try again
        return None

# find the two points of intersection between beacon0 and beacon1
# will use beacon2 to determine the better of the two points
failing = True
power_increases = 0
while failing and power_increases < 10:
    res = circle_intersection(beacons[0],beacons[1])
    if ( res ):
        intersection = res
    else:
        beacons[0][2] *= 1.001
        beacons[1][2] *= 1.001
        power_increases += 1
        continue
    failing = False

# make sure the best fit is within x% (10% of the total distance from the 3rd beacon in this case)
# otherwise the results are too far off
THRESHOLD = 0.1

if failing:
    print 'Bad Beacon Data (Beacon0 & Beacon1 don\'t intersection after many "power increases")'
else:
    # finding best point between beacon1 and beacon2
    dist1 = point_dist(beacons[2][0],beacons[2][1],intersection[0][0],intersection[0][1])
    dist2 = point_dist(beacons[2][0],beacons[2][1],intersection[1][0],intersection[1][1])
    if ( math.fabs(dist1-beacons[2][2]) < math.fabs(dist2-beacons[2][2]) ):
        best_point = intersection[0]
        best_dist = dist1
    else:
        best_point = intersection[1]
        best_dist = dist2
    best_dist_diff = math.fabs(best_dist-beacons[2][2])
    if best_dist_diff < THRESHOLD*best_dist:
        print best_point
    else:
        print 'Bad Beacon Data (Beacon2 distance to best point not within threshold)'

Если вы хотите больше доверять более близким маякам, вы можете рассчитать точки пересечения между двумя ближайшими маяками, а затем использовать более дальний маяк для разрыва связи. Имейте в виду, что почти все, что вы делаете с "уровнями достоверности" для отдельных измерений, в лучшем случае будет взломом. Так как вы всегда будете работать с очень плохими данными, вам определенно нужно ослабить ограничение power_increases и процент пороговых значений.

Answer 2

Если текущий подход хорош, за исключением того, что он слишком медленный, вы можете ускорить его, рекурсивно разделив плоскость. Это работает как поиск ближайших соседей в kd-дереве. Предположим, что нам дан прямоугольник, выровненный по оси, и мы хотим найти в нем примерное наилучшее решение. Если коробка достаточно мала, верните центр.

В противном случае разделите поле пополам, либо на x, либо на y, в зависимости от того, какая сторона длиннее. Для обеих половин вычислите предел качества решения следующим образом. Поскольку целевая функция является аддитивной, суммируйте нижние оценки для каждого маяка. Нижняя граница для маяка - это расстояние от круга до прямоугольника, умноженное на коэффициент масштабирования. Рекурсивно найти лучшее решение у ребенка с нижней нижней границей. Осматривайте другого ребенка, только если лучшее решение для первого ребенка хуже, чем нижняя граница другого ребенка.

Большая часть работы по реализации здесь - это вычисление расстояния от коробки до круга. Поскольку прямоугольник выровнен по оси, мы можем использовать интервальную арифметику для определения точного диапазона расстояний от точек прямоугольника до центра окружности.

_PS: Math.hypot _{хорошая функция для вычисления 2D евклидовых расстояний.}

Answer 3

У вас есть 3 очка: A(xA,yA,zA), B(xB,yB,zB) и C(xC,yC,zC), которые соответственно находятся на уровне dA, dB и d C относительно вашей целевой точки G(xG, YG, ZG). Скажем, cA, cB и cC - это доверительный интервал ( 0

Вот как я это сделаю:

var sum = (cA*dA) + (cB*dB) + (cC*dC);
dA = cA*dA/sum;
dB = cB*dB/sum;
dC = cC*dC/sum;

xG = (xA*dA) + (xB*dB) + (xC*dC);
yG = (yA*dA) + (yB*dB) + (yC*dC);
xG = (zA*dA) + (zB*dB) + (zC*dC);

Базовый, и не очень умный, но выполнит работу для некоторых простых задач.

РЕДАКТИРОВАТЬ

Вы можете взять любую уверенность, какую захотите, в [0,inf[, но ИМХО, ограничение на [0,1] - хорошая идея, чтобы сохранить реалистичный результат.