Buenas. Estoy intentando hacer una geolocalización mediante WiFi midiendo la potencia RSSI que llega a la placa.
Tengo el código creado para medir esta medida de tres nodos diferentes y el método que quiero implementar para geolocalizar el arduino es trilateración.
He hecho pruebas por metro, calculando la medida de RSSI que aporta cada nodo para así, mediante una regresión lineal de los datos aportados, obtener una fórmula que me relacione distancia con valor de RSSI.
Hecho esto, ahora me propongo a usar el método de trilateración y el problema que me surge es el siguiente:
Tengo la medida de distancia estimada a cada nodo, calculada mediante la relación distancia/RSSI.
Esta distancia supongo que equivaldría a los radios R1, R2 y R3 del método de trilateración si no he entendido mal el método.
Una vez obtenido esto, propongo un sistema de coordenadas en el que el nodo 1 esta en el eje de coordenadas (0,0) y los otros dos a ciertos metros en "x" y en "y".
Sabiendo esto, debería ser capaz de calcular por tanto las coordenadas del punto P correspondiente a la placa, sin embargo no se que estoy haciendo mal que me da un error demasiado grande y no funciona.
Supongo que no estoy planteando bien el problema.
Dejo por aquí el código por si alguien sabe en qué me puedo estar equivocando.
void loop(){
float x = 0; //coordenada x del pto P
float y = 0; //coordenada y del pto P
Serial.println("Localizando redes WiFi cercanas.");
int n = WiFi.scanNetworks();// Localiza WiFis
Serial.println("Completado.");
if (n == 0)
Serial.println("No se ha encontrado ninguna red.");
else
{
Serial.print( n);
Serial.println(" Redes WiFi encontradas");
for (int i = 0; i < n; ++i)
{
// Nombre de red (SSID) y potencia (RSSI).
Serial.print(i + 1);
Serial.print(": ");
Serial.print(WiFi.SSID(i));
Serial.print(" (");
Serial.print(WiFi.RSSI(i))
Serial.print(" dBm)");
Serial.println((WiFi.encryptionType(i) == ENC_TYPE_NONE)?" ":"*");
delay(10);
if (WiFi.SSID(i)== pto_acceso_1 ){
rssi_1 = WiFi.RSSI(i);
r1 = (rssi_1+38.821)/-1.9405; //radio círculo nodo 1
Serial.print("r1: ");
Serial.println(r1);
}
else if (WiFi.SSID(i)== pto_acceso_2 ){
rssi_2 = WiFi.RSSI(i);
Serial.print("r2: ");
r2 = (rssi_2+37.857)/-4.994; //radio círculo nodo 2
Serial.println(r2);
}
else if (WiFi.SSID(i)== pto_acceso_3 ){
rssi_3 = WiFi.RSSI(i);
Serial.print("r3: ");
r3 = (rssi_3+31.063)/-5.7083; //radio círculo nodo 3
Serial.println(r3);
}
}
}
Serial.println("");
//sistema de ecuacion Ax+By=C
//Dx+Ey=F
A = -2*x1 + 2*x2;
B = -2*y_1 + 2*y2;
C = (r1*r1) - (r2*r2) - (x1*x1) - (x2*x2) - (y_1*y_1) - (y2*y2);
D = -2*x2 + 2*x3;
E = -2*y2 + 2*y3;
F = (r2*r2) - (r3*r3) - (x2*x2) - (x3*x3) - (y2*y2) - (y3*y3);
x = ((C*E)-(B*F))/((A*E)-(B*D));
Serial.println(x);
y = ((A*F)-(C*D))/((A*E)-(B*D));
Serial.println( y);
Serial.print("x = ");
Serial.println(x);
Serial.print("y = ");
Serial.println( y);
// Nueva búsqueda después de 3 segundos.
delay(3000);
Como se puede ver, saco primero las redes que me interesan entre todas las que hay disponibles.
Calculo el radio mediante la relación preestablecida y luego calculo el sistema de ecuaciones.
Donde x1,x2,x3 e y1,y2,y3 son las coordenadas fijas de los nodos que se declararon en el principio del sketch.
Porque no empiezas por un planteo en hoja y lapiz de tu análisis teorico porque ver lo que has puesto sin tener una introducción matemática, exige que uno se ponga a hacerlo.
Que sistema de coordenadas has elegido. cartesianas, esfericas, cilindricas. veo que ninguna de las ultimas dos porque no hay angulos involucrados.
Donde estan los 3 AP? en el mismo plano? haz una introduccion que no de lugar a planeos de ningún tipo, me parece que sería conveniente para tu respuesta.
Vale, perdona por obviar esos datos. El sistema de coordenadas que he escogido es el cartesiano. Donde los AP están en el (0,0), (0,7.8) y (4, 4,8). La matemática que he usado es sencilla, tengo la distancia a cada nodo calculada por el método mencionado anteriormente (experimentalmente con la potencia RSSI), por lo que tengo, o eso creo, los radios de cada circunferencia, obteniendo por tanto 3 ecuaciones de circunferencia. Restando la primera con la segunda y la tercera se obtiene el punto de intersección de las 3 y ahi obtengo el punto.
Bueno, estoy de acuerdo pero si la matemática es sencilla prueba con puntos intermedios, calculadora mediante y ve viendo quelos resultados parciales se cumplan y listo. Muchos Serial.print imprimiendo dichos valores y resuelves el tema.
Los puntos críticos son estos
x = ((C*E)-(B*F))/((A*E)-(B*D));
Pero eso es matematica. Si tu denominador tiene a 0 el valor de x se dispara a ser un número grande o muy grande, asi que controla esa diferencia AE - BD para que nunca sea 0 o cercano a el valor nulo.
Tal vez mas fácil una planilla de cálculo.
Yo no lo tengo tan en claro que matemáticamente eso este bien.
Tienes tres ecuaciones con dos incógnitas (X,Y)
Tomando dos de las ecuaciones y despejando una incógnita y remplazandola en la otra, tendrías una cuadrática con dos soluciones . En teoría, trabajando las ecuaciones por pares, tendrías que obtener tres resultados coincidentes de los seis.
PERO.......
En el mundo real, las tres mediciones pueden contener errores y la cosa se complica MUCHOOOOO.
Por ejemplo si tomas dos ecuaciones que tengan un error por defecto, la solución de la cuadrática, te dará un resultado en los números complejos (las dos circunferencia no se tocan) . Si el punto se encuentra en una linea entre los centros , se tocaran en un solo punto . Si las medidas son mayores, el resultado de la cuadrática te dará dos resultados (las circunferencias se tocan en dos puntos)
Es decir, para resolver el sistema no puedes ignorar el error en la medición.
Teniendo en cuenta el error, las ecuaciones quedan:
(R1+E 1)2=(X+X 1)2+(y+y 1)2
(R2+E 2)2=(X+X 2)2+(y+y 2)2
(R3+E 3)2=(X+X 3)2+(y+y 3)2
No son resolubles analíticamente, pero puedes crear una ecuación auxiliar
Rt2=R12+R22+R32
y minimizarla con respecto a x,y
Un enfoque mas practico es utilizar una resolución a la fuerza, darle valores x,y a las ecuaciones de las circunferencia y buscar el mínimo valor de Rt
Otra manera seria tratar por pares. Tomas dos ecuaciones de la circunferencia y obtienes un par de puntos (por ser una ecuación cuadrática). Luego tomas los dos puntos encontrados y calculas la distancia al tercer nodo. Te quedas con el punto que tenga una distancia parecida a la distancia calculada del tercer nodo.
De esa manera obtendrás tres puntos (por cada par de datos de distancia) y encuentras el centro de esos tres puntos.
De esa manera tendrías un valor posible de la ubicación, mas una idea de la dispersión calculando la distancia del centro a los tres puntos obtenidos.
Tienes razón con el error, no lo había tenido en cuenta, confirmaba una idealización en la que las tres circunferencias coincidían en un punto. Suponiendo ahora que las tres circunferencias cortan tendríamos lo siguiente:
Como se puede ver hay 6 intersecciones, de las que interesa la 1, la 2 y la 3. En la que se puede trazar un triángulo y calcular su centroide. Al calcular los puntos de corte, como bien dices se obtienen dos soluciones ya que es una ecuación cuadrática, como podría deducir cuales son los puntos 1, 2 y 3 del gráfico?. Entiendo que en este caso es sencillo ya que serían los tres más cercanos y quizás con unas comparaciones se podrían descartar los otros 3. Sin embargo, esto no siempre va a ser así y habrá ocasiones en las que ni se lleguen a cortar las tres circunferencias. Suponiendo que se llegasen a cortar las tres para realizar una primera toma de contacto, se os ocurre alguna forma de calcular esos tres puntos 1, 2 y 3?
