void init()
{ if(!SDL_Init(0))
{ if(SDLNet_Init()<0)
{ errorFunc(sdlnet);
}
}else
{ errorFunc(sdl);
}
}
pair<double, Vector2d> errorGradient(const GrParams ¶m, Vector2d s)
{
const int num_var = 2;
Vector2d res;
auto fval = errorFunc(param, s);
// choose dh for approximating derivative that is appropriate
// if we are already close to minimum(error is smalll)
const float h = min(fval, 0.01);
for(int k = 0; k < num_var; ++k)
{
auto s_h = s;
s_h[k] += h;
auto f_shift = errorFunc(param, s_h);
res[k] = (f_shift-fval)/h;
}
return make_pair(fval, res);
}
/// Cumulative distribution function (CDF) of the Normal-distribution.
/// Ref http://www.itl.nist.gov/div898/handbook/ 1.3.6.6.1
/// @param x input statistic
/// @param mu mean of the sample (location parameter of the distribution)
/// @param sig std dev of the sample (scale parameter of the distribution)
/// @return Normal distribution probability
double NormalCDF(const double& x, const double& mu, const double& sig)
throw(Exception)
{
if(sig <= 0.0) GPSTK_THROW(Exception("Non-positive sigma"));
try {
static const double sqrt2(::sqrt(2.0));
double arg(x-mu);
double erf = errorFunc(::fabs(arg)/(sqrt2*sig));
return (0.5 * (1.0 + (arg < 0.0 ? -erf : erf)));
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// Private methods
void MeetAndroid::processCommand(){
if(buffer[0]-FunctionBufferOffset < FunctionBufferLenght){
void (*H_FuncPtr)(uint8_t, uint8_t) = intFunc[buffer[0]-FunctionBufferOffset];
if (H_FuncPtr != 0) {
H_FuncPtr(buffer[0], getArrayLength());
}
else {
send("Flag not registered: ");
send(buffer[0]);
}
}
else {
if (customErrorFunc)
errorFunc(buffer[0], getArrayLength());
else {
send("Flag out of bounds: ");
send(buffer[0]);
}
}
}
// Perform least-squares minimization using the Levenberg-Marquardt algorithm.
int levmarq(int npar, double *par, int ny, double *y, double *dysq, double *fdata)
{
/*
The arguments are as follows :
npar number of parameters
par array of parameters to be varied
ny number of measurements to be fit
y array of measurements
dysq array of error in measurements, squared
(set dysq = NULL for unweighted least - squares)
func function to be fit
grad gradient of "func" with respect to the input parameters
fdata pointer to any additional data required by the function
*/
int x, i, j, it, nit, ill;
double lambda, up, down, mult, weight, err, newerr, derr, target_derr;
double h[npar*npar], ch[npar*npar];
double g[npar], d[npar], delta[npar], newpar[npar];
nit = MAX_IT;
lambda = INIT_LAMBDA;
up = UP_FACTOR;
down = 1.0/(DOWN_FACTOR);
target_derr = TARGET_DERR;
weight = 1;
derr = newerr = 0; /* to avoid compiler warnings */
/* calculate the initial error ("chi-squared") */
err = errorFunc(par, ny, y, dysq, fdata);
/* main iteration */
for (it = 0; it<nit; it++) {
#ifdef VERBOSE
Serial.print("\niteration: ");
Serial.println(it);
Serial.print("err:");
Serial.print(err);
Serial.print("x:");
Serial.print(par[0]);
Serial.print("y:");
Serial.print(par[1]);
Serial.print("z:");
Serial.println(par[2]);
Serial.print("target derr is: ");
Serial.println(target_derr);
Serial.print("up is: ");
Serial.println(up);
Serial.print("down is: ");
Serial.println(down);
#endif
/* calculate the approximation to the Hessian and the "derivative" d */
for (i = 0; i<npar; i++) {
d[i] = 0;
for (j = 0; j <= i; j++)
h[i*npar + j] = 0;
}
for (x = 0; x<ny; x++) {
if (dysq) weight = 1 / dysq[x]; /* for weighted least-squares */
grad(g, par, x, fdata);
for (i = 0; i<npar; i++) {
d[i] += (y[x] - func(par, x, fdata))*g[i] * weight;
for (j = 0; j <= i; j++)
h[i*npar + j] += g[i] * g[j] * weight;
}
}
/* make a step "delta." If the step is rejected, increase
lambda and try again */
mult = 1 + lambda;
ill = 1; /* ill-conditioned? */
while (ill && (it<nit)) {
for (i = 0; i<npar; i++)
h[i*npar + i] = h[i*npar + i] * mult;
ill = choleskyDecomp(npar, ch, h);
if (!ill) {
solveAxBCholesky(npar, ch, delta, d);
for (i = 0; i<npar; i++)
newpar[i] = par[i] + delta[i];
newerr = errorFunc(newpar, ny, y, dysq, fdata);
derr = newerr - err;
ill = (derr > 0);
}
#ifdef VERBOSE
Serial.println("New loop:");
Serial.print("it = ");
Serial.print(it);
Serial.print("lambda = ");
Serial.print(lambda,8);
Serial.print("err = ");
Serial.print(err,8);
Serial.print("derr = ");
Serial.println(derr,8);
Serial.print("ill = ");
Serial.println(ill);
#endif
if (ill) {
mult = (1 + lambda*up) / (1 + lambda);
lambda *= up;
it++;
}
}
for (i = 0; i<npar; i++)
par[i] = newpar[i];
err = newerr;
lambda *= down;
if ((!ill) && (-derr<target_derr)) break;
}
return (it == nit);
}
