foo

1 files changed, 215 insertions, 0 deletions

diff --git a/controller/fw/levmarq/levmarq.c b/controller/fw/levmarq/levmarq.c
new file mode 100644
index 0000000..bbf00c0
--- /dev/null
+++ b/controller/fw/levmarq/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;
+}