Localización Indoor con arduino mkr 1010

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.
C++:
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.
 
Arriba