From d493d9e99c28e7c3245b1cc2bfbbbe8b44db301a Mon Sep 17 00:00:00 2001 From: jaseg Date: Mon, 2 Mar 2020 19:45:47 +0100 Subject: foo --- controller/fw/git_sucks/LICENSE | 24 ----- controller/fw/git_sucks/levmarq.c | 215 -------------------------------------- controller/fw/git_sucks/levmarq.h | 30 ------ controller/fw/levmarq/LICENSE | 24 +++++ controller/fw/levmarq/levmarq.c | 215 ++++++++++++++++++++++++++++++++++++++ controller/fw/levmarq/levmarq.h | 30 ++++++ 6 files changed, 269 insertions(+), 269 deletions(-) delete mode 100644 controller/fw/git_sucks/LICENSE delete mode 100644 controller/fw/git_sucks/levmarq.c delete mode 100644 controller/fw/git_sucks/levmarq.h create mode 100644 controller/fw/levmarq/LICENSE create mode 100644 controller/fw/levmarq/levmarq.c create mode 100644 controller/fw/levmarq/levmarq.h (limited to 'controller/fw') diff --git a/controller/fw/git_sucks/LICENSE b/controller/fw/git_sucks/LICENSE deleted file mode 100644 index 2a05e3e..0000000 --- a/controller/fw/git_sucks/LICENSE +++ /dev/null @@ -1,24 +0,0 @@ -levmarq.c, levmarq.h, and examples are provided under the MIT license. - -Copyright (c) 2008-2016 Ron Babich - -Permission is hereby granted, free of charge, to any person -obtaining a copy of this software and associated documentation -files (the "Software"), to deal in the Software without -restriction, including without limitation the rights to use, -copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the -Software is furnished to do so, subject to the following -conditions: - -The above copyright notice and this permission notice shall be -included in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, -EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES -OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND -NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT -HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, -WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR -OTHER DEALINGS IN THE SOFTWARE. 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 -#include - -#include - -#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 0); - } - if (ill) { - mult = (1 + lambda*up)/(1 + lambda); - lambda *= up; - it++; - } - } - for (i=0; ifinal_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=0; i--) { - sum = 0; - for (j=i+1; j +#include + +#include + +#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 0); + } + if (ill) { + mult = (1 + lambda*up)/(1 + lambda); + lambda *= up; + it++; + } + } + for (i=0; ifinal_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=0; i--) { + sum = 0; + for (j=i+1; j