Don't hate me. I am not good with c, so seek for your advise.
trying to implement multilateration, found this algorithm in one paper.
Can you suggest me what should I do/ how should I define my parameters to avoid this ovf problem? I think numbers I get are too big, and because of this I can't find final value.
tried to make it long double, but it didn't work/ all of the Serial.prints just to follow where the code fails.
Here is my code:
Serial.begin(9600);
long TOAi=millis();
long TOAj=TOAi+3, TOAk=TOAi+7, TOAl=TOAi+18;
int CurrC=340;
int xi=0, yi=0, zi=0;
int xj=0, yj=50, zj=0;
int xk=50, yk=0, zk=0;
int xl=50, yl=50, zl=50;
float Rij=(float) CurrC*(TOAi-TOAj);
float Rik=(float) CurrC*(TOAi-TOAk);
float Ril=(float) CurrC*(TOAi-TOAl);
double Rkl=(double) CurrC*(TOAk-TOAl);
double Rkj=(double) CurrC*(TOAk-TOAj);
//A=Xikx/Xijy
double A=(Rik*(xj-xi)-Rij*(xk-xi))/(Rij*(yk-yi)-Rik*(yj-yi));
Serial.println(A);
//B=Xikz/Xijy
double B=(Rik*(zj-zi)-Rij*(zk-zi))/(Rij*(yk-yi)-Rik*(yj-yi));
Serial.println(B);
//C=Xklx/Xkjy
double C=(Rkl*(xj-xk)-Rkj*(xl-xk))/(Rkj*(yl-yk)-Rkl*(yj-yk));
Serial.println(C);
//D=Xklz/Xkjy
double D=(Rkl*(zj-zk)-Rkj*(zl-zk))/(Rkj*(yl-yk)-Rkl*(yj-yk));
Serial.println(D);
//E=(Rik*Rij2xyz-Rij*Rik2xyz)/2Xijy
double E=(Rik*(Rij*Rij+(xi*xi+yi*yi+zi*zi) - (xj*xj+yj*yj+zj*zj))-Rij*(Rik*Rik + (xi*xi+yi*yi+zi*zi) - (xk*xk+yk*yk+zk*zk)))/(2*(Rij*(yk-yi)-Rik*(yj-yi)));
Serial.println(E);
//F=(Rkl*Rkj2xyz - Rkj*Rkl2xyz)/2Xkjy
double F=((Rkl*(Rkj*Rkj+(xk*xk+yk*yk+zk*zk) - (xj*xj+yj*yj+zj*zj))-Rkj*(Rkl*Rkl + (xk*xk+yk*yk+zk*zk) - (xl*xl+yl*yl+zl*zl))))/(2*(Rkj*(yl-yk)-Rkl*(yj-yk)));
Serial.println(F);
double G=(D-B)/(A-C);
double H=(F-E)/(A-C);
Serial.println(G);
Serial.println(H);
double I=A*G+B;
double J=A*H+E;
Serial.println(I);
Serial.println(J);
double K=(Rik*Rik + (xi*xi+yi*yi+zi*zi) - (xk*xk+yk*yk+zk*zk) +2*(xk-xi)*H +2*(yk-yi)*J);
double L=2*((xk-xi)*G+(yk-yi)*I+(zk-zi));
double M=4*Rik*Rik*(G*G+I*I+1)-L*L;
double N=8*Rik*Rik*(G*(xi-H)+I*(yi-J)+zi)+2*L*K;
double O=4*Rik*Rik*((xi-H)*(xi-H)+(yi-J)*(yi-J)+zi*zi)-K*K;
Serial.println(K);
Serial.println(L);
Serial.println(M);
Serial.println(N);
Serial.println(O);
double Q;
//sign() function...
if (N>0) Q=N+sqrt(N*N-4*M*O);
else if(N<0) Q=N-sqrt(N*N-4*M*O);
else Q=N;
Serial.println(Q);
long double pos[6]={G*Q/2/M+H, I*Q/2/M+J, Q/2/M, G*2*O/Q+H, I*2*O/Q+J, 2*O/Q};