summaryrefslogtreecommitdiff
path: root/controller/fw/levmarq
diff options
context:
space:
mode:
Diffstat (limited to 'controller/fw/levmarq')
-rw-r--r--controller/fw/levmarq/LICENSE24
-rw-r--r--controller/fw/levmarq/levmarq.c215
-rw-r--r--controller/fw/levmarq/levmarq.h30
3 files changed, 0 insertions, 269 deletions
diff --git a/controller/fw/levmarq/LICENSE b/controller/fw/levmarq/LICENSE
deleted file mode 100644
index 2a05e3e..0000000
--- a/controller/fw/levmarq/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/levmarq/levmarq.c b/controller/fw/levmarq/levmarq.c
deleted file mode 100644
index 4a764db..0000000
--- a/controller/fw/levmarq/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 "levmarq.h"
-#include "simulation.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 = 100;
- lmstat->init_lambda = 0.0001f;
- lmstat->up_factor = 10.0f;
- lmstat->down_factor = 10.0f;
- lmstat->target_derr = 1e-12f;
-}
-
-/* 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++) {
- //DEBUG_PRINT("iteration %d", 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;
-
- if (it == nit) {
- DEBUG_PRINT("did not converge");
- return -1;
- }
-
- return it;
-}
-
-
-/* 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;
-}
diff --git a/controller/fw/levmarq/levmarq.h b/controller/fw/levmarq/levmarq.h
deleted file mode 100644
index dff13ab..0000000
--- a/controller/fw/levmarq/levmarq.h
+++ /dev/null
@@ -1,30 +0,0 @@
-
-#ifndef __LEVMARQ_H__
-#define __LEVMARQ_H__
-
-typedef struct {
- int max_it;
- float init_lambda;
- float up_factor;
- float down_factor;
- float target_derr;
- int final_it;
- float final_err;
- float final_derr;
-} LMstat;
-
-void levmarq_init(LMstat *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);
-
-float error_func(float *par, int ny, float *y, float *dysq,
- float (*func)(float *, int, void *), void *fdata);
-
-void solve_axb_cholesky(int n, float l[n][n], float x[n], float b[n]);
-
-int cholesky_decomp(int n, float l[n][n], float a[n][n]);
-
-#endif /* __LEVMARQ_H__ */
generated by cgit (git 2.34.1) at 2024-11-14 11:15:15 +0000