// NLLSQ Approximate trilateration
void locateNLLSQ(int numantennas, double* antennas, double* distances, double* x){
int npar, ny;
if (numantennas == 3){
npar = 3; // Assume k=1
ny = 3; // One measurement for each antenna
}
else if(numantennas >= 4){
npar = 4; // x, y, z, K
ny = numantennas; // one data point for each antenna
}
else{
// We're screwed - THIS WONT WORK
npar = 3; // Assume k=1
ny = 3; // One measurement for each antenna
}
// perform least-squares minimization using the Levenberg-Marquardt using following parameters:
levmarq(npar, x, ny, distances, NULL, antennas);
}
void fit(short* individual, int len,
float* params, int numParams,
float* X, int numVars,
double* y, int numPoints){
eval_data fdata;
fdata.X = X;
fdata.individual = individual;
fdata.len = len;
fdata.num_p = numParams;
fdata.num_x = numVars;
Stack S;
initializeStack(&S, (float*)malloc(sizeof(float)*len));
fdata.S = &S;
LMstat lmstat;
levmarq_init(&lmstat);
levmarq(numParams, params,
numPoints, y, NULL,
&func, &grad, &fdata,
&lmstat);
}