Реализация алгоритма трилатерации (2D)
Я пытаюсь реализовать процесс трилатерации в 2D. Статья в Википедии, относящаяся к этому: Tilateration
Я нашел хороший вопрос здесь на этом сайте, где алгоритм хорошо объяснен:искусственный интеллект
В конце концов, я попытался реализовать алгоритм в C ++. К сожалению, я столкнулся с некоторыми проблемами …
Давайте посмотрим мою реализацию. Это всего лишь функция: первые входы состоят из трех векторов, каждый из которых представляет 2D-точку с координатами X, Y. Другие (r1, r2, r3) входные переменные обозначают расстояние / радиус каждой точки.
#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>
#include <vector>
using namespace std;
std::vector<double> trilateration(double point1[], double point2[], double point3[], double r1, double r2, double r3) {
std::vector<double> resultPose;
//unit vector in a direction from point1 to point 2
double p2p1Distance = pow(pow(point2[0]-point1[0],2) + pow(point2[1]-point1[1],2),0.5);
double exx = (point2[0]-point1[0])/p2p1Distance;
double exy = (point2[1]-point1[1])/p2p1Distance;
//signed magnitude of the x component
double ix = exx*(point3[0]-point1[0]);
double iy = exy*(point3[1]-point1[1]);
//the unit vector in the y direction.
double eyx = (point3[0]-point1[0]-ix*exx)/pow(pow(point3[0]-point1[0]-ix*exx,2) + pow(point3[1]-point1[1]-iy*exy,2),0.5);
double eyy = (point3[1]-point1[1]-iy*exy)/pow(pow(point3[0]-point1[0]-ix*exx,2) + pow(point3[1]-point1[1]-iy*exy,2),0.5);
//the signed magnitude of the y component
double jx = eyx*(point3[0]-point1[0]);
double jy = eyy*(point3[1]-point1[1]);
//coordinates
double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
double y = (pow(r1,2) - pow(r3,2) + pow(iy,2) + pow(jy,2))/2*jy - ix*x/jx;
//result coordinates
double finalX = point1[0]+ x*exx + y*eyx;
double finalY = point1[1]+ x*exy + y*eyy;
resultPose.push_back(finalX);
resultPose.push_back(finalY);
return resultPose;
}
Как я уже говорил, я следовал этот статья. Я считаю, что проблема заключается в той части, где вычисляется координата y. Я также не уверен насчет последней части, где я вычисляю finalX, finalY …
Моя основная функция заключается в следующем:
int main(int argc, char* argv[]){
std::vector<double> finalPose;
double p1[] = {4.0,4.0};
double p2[] = {9.0,7.0};
double p3[] = {9.0,1.0};
double r1,r2,r3;
r1 = 4;
r2 = 3;
r3 = 3.25;
finalPose = trilateration(p1,p2,p3,r1,r2,r3);
cout<<"X::: "<<finalPose[0]<<endl;
cout<<"Y::: "<<finalPose[1]<<endl;
//x = 8, y = 4.1
}
Результат должен быть около X ~ 8 и Y ~ 4.1, но я получил X = 13.5542 и Y = -5.09038
Итак, моя проблема и вопрос: у меня проблема с делением вычислений для x и y. Я думаю, что мог бы решить алгоритм до х, после этого у меня есть проблемы с вычислением у.
Расчет для y следующий: y = (r12 — r32 + i2 + j2) / 2j — ix / j
Я не знаю, какие i и j мне следует использовать здесь, так как у меня есть два i (ix, iy) и два j (jx, jy). Как видите, я использовал iy и jy, но в конце строки я использовал ix из-за умножения на x.
Заранее спасибо!
Решение
Это немного неясно, и, возможно, неправильно, в связанный так ответ что значения i а также j являются скалярными значениями и вычисляются немного по-другому, чем другие векторные величины. Более подробно вы должны иметь:
я = еИкс · (P3 — P1) = ехх (Р3Икс — P1Икс) + еху (Р3Y — P1Y) = ix + iy
J = EY · (P3 — P1) = еуг (Р3Икс — P1Икс) + еуу (Р3Y — P1Y) = jx + jy
Обратите внимание, что · является точечным произведением двух векторов здесь. Таким образом, в вашем коде не должно быть ix, iy, jx или же jy,
Кроме того, в вашем расчете y Вы должны изменить знаменатель /2*j чтобы:
/ (2*j)
в противном случае вы умножаете на j вместо деления. Внесение этих изменений дает мне результат [7.05, 5.74] что ближе к вашим ожидаемым значениям.
Другие решения
Я использовал пару вспомогательных переменных, но это работает просто отлично …
#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>
#include <vector>
using namespace std;
struct point
{
float x,y;
};
float norm (point p) // get the norm of a vector
{
return pow(pow(p.x,2)+pow(p.y,2),.5);
}
point trilateration(point point1, point point2, point point3, double r1, double r2, double r3) {
point resultPose;
//unit vector in a direction from point1 to point 2
double p2p1Distance = pow(pow(point2.x-point1.x,2) + pow(point2.y- point1.y,2),0.5);
point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
point aux = {point3.x-point1.x,point3.y-point1.y};
//signed magnitude of the x component
double i = ex.x * aux.x + ex.y * aux.y;
//the unit vector in the y direction.
point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
point ey = { aux2.x / norm (aux2), aux2.y / norm (aux2) };
//the signed magnitude of the y component
double j = ey.x * aux.x + ey.y * aux.y;
//coordinates
double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
double y = (pow(r1,2) - pow(r3,2) + pow(i,2) + pow(j,2))/(2*j) - i*x/j;
//result coordinates
double finalX = point1.x+ x*ex.x + y*ey.x;
double finalY = point1.y+ x*ex.y + y*ey.y;
resultPose.x = finalX;
resultPose.y = finalY;
return resultPose;
}
int main(int argc, char* argv[]){
point finalPose;
point p1 = {4.0,4.0};
point p2 = {9.0,7.0};
point p3 = {9.0,1.0};
double r1,r2,r3;
r1 = 4;
r2 = 3;
r3 = 3.25;
finalPose = trilateration(p1,p2,p3,r1,r2,r3);
cout<<"X::: "<<finalPose.x<<endl;
cout<<"Y::: "<<finalPose.y<<endl;
}
$ вывод:
X::: 8.02188
Y::: 4.13021