diff options
Diffstat (limited to 'controller/fw/git_sucks/levmarq.c')
| -rw-r--r-- | controller/fw/git_sucks/levmarq.c | 215 |
1 files changed, 0 insertions, 215 deletions
diff --git a/controller/fw/git_sucks/levmarq.c b/controller/fw/git_sucks/levmarq.c deleted file mode 100644 index bbf00c0..0000000 --- a/controller/fw/git_sucks/levmarq.c +++ /dev/null @@ -1,215 +0,0 @@ -/* - * levmarq.c - * - * This file contains an implementation of the Levenberg-Marquardt algorithm - * for solving least-squares problems, together with some supporting routines - * for Cholesky decomposition and inversion. No attempt has been made at - * optimization. In particular, memory use in the matrix routines could be - * cut in half with a little effort (and some loss of clarity). - * - * It is assumed that the compiler supports variable-length arrays as - * specified by the C99 standard. - * - * Ron Babich, May 2008 - * - */ - -#include <stdio.h> -#include <math.h> - -#include <arm_math.h> - -#include "levmarq.h" - - -#define TOL 1e-20f /* smallest value allowed in cholesky_decomp() */ - - -/* set parameters required by levmarq() to default values */ -void levmarq_init(LMstat *lmstat) -{ - lmstat->max_it = 10000; - lmstat->init_lambda = 0.0001f; - lmstat->up_factor = 10.0f; - lmstat->down_factor = 10.0f; - lmstat->target_derr = 1e-12f; -} - -float sqrtf(float arg) { - float out=NAN; - arm_sqrt_f32(arg, &out); - return out; -} - -/* perform least-squares minimization using the Levenberg-Marquardt - algorithm. 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 - lmstat pointer to the "status" structure, where minimization parameters - are set and the final status is returned. - - Before calling levmarq, several of the parameters in lmstat must be set. - For default values, call levmarq_init(lmstat). - */ -int levmarq(int npar, float *par, int ny, float *y, float *dysq, - float (*func)(float *, int, void *), - void (*grad)(float *, float *, int, void *), - void *fdata, LMstat *lmstat) -{ - int x,i,j,it,nit,ill; - float lambda,up,down,mult,weight,err,newerr,derr,target_derr; - float h[npar][npar],ch[npar][npar]; - float g[npar],d[npar],delta[npar],newpar[npar]; - - nit = lmstat->max_it; - lambda = lmstat->init_lambda; - up = lmstat->up_factor; - down = 1/lmstat->down_factor; - target_derr = lmstat->target_derr; - weight = 1; - derr = newerr = 0; /* to avoid compiler warnings */ - - /* calculate the initial error ("chi-squared") */ - err = error_func(par, ny, y, dysq, func, fdata); - - /* main iteration */ - for (it=0; it<nit; it++) { - - /* 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][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][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][i] = h[i][i]*mult; - - ill = cholesky_decomp(npar, ch, h); - - if (!ill) { - solve_axb_cholesky(npar, ch, delta, d); - for (i=0; i<npar; i++) - newpar[i] = par[i] + delta[i]; - newerr = error_func(newpar, ny, y, dysq, func, fdata); - derr = newerr - err; - ill = (derr > 0); - } - 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; - } - - lmstat->final_it = it; - lmstat->final_err = err; - lmstat->final_derr = derr; - - return (it==nit); -} - - -/* calculate the error function (chi-squared) */ -float error_func(float *par, int ny, float *y, float *dysq, - float (*func)(float *, int, void *), void *fdata) -{ - int x; - float res,e=0; - - for (x=0; x<ny; x++) { - res = func(par, x, fdata) - y[x]; - if (dysq) /* weighted least-squares */ - e += res*res/dysq[x]; - else - e += res*res; - } - return e; -} - - -/* solve the equation Ax=b for a symmetric positive-definite matrix A, - using the Cholesky decomposition A=LL^T. The matrix L is passed in "l". - Elements above the diagonal are ignored. -*/ -void solve_axb_cholesky(int n, float l[n][n], float x[n], float b[n]) -{ - int i,j; - float sum; - - /* solve L*y = b for y (where x[] is used to store y) */ - - for (i=0; i<n; i++) { - sum = 0; - for (j=0; j<i; j++) - sum += l[i][j] * x[j]; - x[i] = (b[i] - sum)/l[i][i]; - } - - /* solve L^T*x = y for x (where x[] is used to store both y and x) */ - - for (i=n-1; i>=0; i--) { - sum = 0; - for (j=i+1; j<n; j++) - sum += l[j][i] * x[j]; - x[i] = (x[i] - sum)/l[i][i]; - } -} - - -/* This function takes a symmetric, positive-definite matrix "a" and returns - its (lower-triangular) Cholesky factor in "l". Elements above the - diagonal are neither used nor modified. The same array may be passed - as both l and a, in which case the decomposition is performed in place. -*/ -int cholesky_decomp(int n, float l[n][n], float a[n][n]) -{ - int i,j,k; - float sum; - - for (i=0; i<n; i++) { - for (j=0; j<i; j++) { - sum = 0; - for (k=0; k<j; k++) - sum += l[i][k] * l[j][k]; - l[i][j] = (a[i][j] - sum)/l[j][j]; - } - - sum = 0; - for (k=0; k<i; k++) - sum += l[i][k] * l[i][k]; - sum = a[i][i] - sum; - if (sum<TOL) return 1; /* not positive-definite */ - l[i][i] = sqrtf(sum); - } - return 0; -} |