summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--controller/fw/git_sucks/LICENSE24
-rw-r--r--controller/fw/git_sucks/levmarq.c215
-rw-r--r--controller/fw/git_sucks/levmarq.h30
m---------controller/fw/levmarq0
4 files changed, 269 insertions, 0 deletions
diff --git a/controller/fw/git_sucks/LICENSE b/controller/fw/git_sucks/LICENSE
new file mode 100644
index 0000000..2a05e3e
--- /dev/null
+++ b/controller/fw/git_sucks/LICENSE
@@ -0,0 +1,24 @@
+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
new file mode 100644
index 0000000..bbf00c0
--- /dev/null
+++ b/controller/fw/git_sucks/levmarq.c
@@ -0,0 +1,215 @@
+/*
+ * 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;
+}
diff --git a/controller/fw/git_sucks/levmarq.h b/controller/fw/git_sucks/levmarq.h
new file mode 100644
index 0000000..dff13ab
--- /dev/null
+++ b/controller/fw/git_sucks/levmarq.h
@@ -0,0 +1,30 @@
+
+#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__ */
diff --git a/controller/fw/levmarq b/controller/fw/levmarq
deleted file mode 160000
-Subproject d7734ce5a436b891d00dd3445dd0b57b1dfbe77
generated by cgit (git 2.34.1) at 2024-11-11 05:31:21 +0000