diff --git a/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml b/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
index 1ff6bb8f71..5eb1f770e4 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
@@ -100,30 +100,12 @@
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -141,9 +123,6 @@
-
-
-
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
index 6a66ba2921..81eb0ca0d5 100644
--- a/conf/modules/stabilization_indi.xml
+++ b/conf/modules/stabilization_indi.xml
@@ -11,25 +11,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
@@ -39,9 +26,10 @@
-
-
-
+
+
+
+
@@ -69,6 +57,9 @@
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index 9a28c09e4d..f95caaa286 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -42,6 +42,15 @@
#include "subsystems/actuators.h"
#include "subsystems/abi.h"
#include "filters/low_pass_filter.h"
+#include "wls/wls_alloc.h"
+#include
+
+float du_min[4];
+float du_max[4];
+float du_pref[4];
+float indi_v[4];
+float* Bwls[4];
+int num_iter = 0;
static void lms_estimation(void);
static void get_actuator_state(void);
@@ -85,6 +94,14 @@ bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
bool act_is_servo[INDI_NUM_ACT] = {0};
#endif
+#ifdef STABILIZATION_INDI_ACT_PREF
+// Preferred (neutral, least energy) actuator value
+float act_pref[4] = STABILIZATION_INDI_ACT_PREF;
+#else
+// Assume 0 is neutral
+float act_pref[4] = {0.0};
+#endif
+
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
/** Maximum rate you can request in RC rate mode (rad/s)*/
@@ -193,6 +210,12 @@ void stabilization_indi_init(void)
//Calculate G1G2_PSEUDO_INVERSE
calc_g1g2_pseudo_inv();
+ // Initialize the array of pointers to the rows of g1g2
+ uint8_t i;
+ for(i=0; i
+# include
+
+# include "qr_solve.h"
+# include "r8lib_min.h"
+
+#define DEBUG_FPRINTF(...)
+#define DEBUG_EXIT(...)
+
+/******************************************************************************/
+
+void daxpy ( int n, float da, float dx[], int incx, float dy[], int incy )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DAXPY computes constant times a vector plus a vector.
+
+ Discussion:
+
+ This routine uses unrolled loops for increments equal to one.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 30 March 2007
+
+ Author:
+
+ C version by John Burkardt
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979.
+
+ Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+ Basic Linear Algebra Subprograms for Fortran Usage,
+ Algorithm 539,
+ ACM Transactions on Mathematical Software,
+ Volume 5, Number 3, September 1979, pages 308-323.
+
+ Parameters:
+
+ Input, int N, the number of elements in DX and DY.
+
+ Input, float DA, the multiplier of DX.
+
+ Input, float DX[*], the first vector.
+
+ Input, int INCX, the increment between successive entries of DX.
+
+ Input/output, float DY[*], the second vector.
+ On output, DY[*] has been replaced by DY[*] + DA * DX[*].
+
+ Input, int INCY, the increment between successive entries of DY.
+*/
+{
+ int i;
+ int ix;
+ int iy;
+ int m;
+
+ if ( n <= 0 )
+ {
+ return;
+ }
+
+ if ( da == 0.0 )
+ {
+ return;
+ }
+/*
+ Code for unequal increments or equal increments
+ not equal to 1.
+*/
+ if ( incx != 1 || incy != 1 )
+ {
+ if ( 0 <= incx )
+ {
+ ix = 0;
+ }
+ else
+ {
+ ix = ( - n + 1 ) * incx;
+ }
+
+ if ( 0 <= incy )
+ {
+ iy = 0;
+ }
+ else
+ {
+ iy = ( - n + 1 ) * incy;
+ }
+
+ for ( i = 0; i < n; i++ )
+ {
+ dy[iy] = dy[iy] + da * dx[ix];
+ ix = ix + incx;
+ iy = iy + incy;
+ }
+ }
+/*
+ Code for both increments equal to 1.
+*/
+ else
+ {
+ m = n % 4;
+
+ for ( i = 0; i < m; i++ )
+ {
+ dy[i] = dy[i] + da * dx[i];
+ }
+
+ for ( i = m; i < n; i = i + 4 )
+ {
+ dy[i ] = dy[i ] + da * dx[i ];
+ dy[i+1] = dy[i+1] + da * dx[i+1];
+ dy[i+2] = dy[i+2] + da * dx[i+2];
+ dy[i+3] = dy[i+3] + da * dx[i+3];
+ }
+ }
+ return;
+}
+/******************************************************************************/
+
+float ddot ( int n, float dx[], int incx, float dy[], int incy )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DDOT forms the dot product of two vectors.
+
+ Discussion:
+
+ This routine uses unrolled loops for increments equal to one.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 30 March 2007
+
+ Author:
+
+ C version by John Burkardt
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979.
+
+ Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+ Basic Linear Algebra Subprograms for Fortran Usage,
+ Algorithm 539,
+ ACM Transactions on Mathematical Software,
+ Volume 5, Number 3, September 1979, pages 308-323.
+
+ Parameters:
+
+ Input, int N, the number of entries in the vectors.
+
+ Input, float DX[*], the first vector.
+
+ Input, int INCX, the increment between successive entries in DX.
+
+ Input, float DY[*], the second vector.
+
+ Input, int INCY, the increment between successive entries in DY.
+
+ Output, float DDOT, the sum of the product of the corresponding
+ entries of DX and DY.
+*/
+{
+ float dtemp;
+ int i;
+ int ix;
+ int iy;
+ int m;
+
+ dtemp = 0.0;
+
+ if ( n <= 0 )
+ {
+ return dtemp;
+ }
+/*
+ Code for unequal increments or equal increments
+ not equal to 1.
+*/
+ if ( incx != 1 || incy != 1 )
+ {
+ if ( 0 <= incx )
+ {
+ ix = 0;
+ }
+ else
+ {
+ ix = ( - n + 1 ) * incx;
+ }
+
+ if ( 0 <= incy )
+ {
+ iy = 0;
+ }
+ else
+ {
+ iy = ( - n + 1 ) * incy;
+ }
+
+ for ( i = 0; i < n; i++ )
+ {
+ dtemp = dtemp + dx[ix] * dy[iy];
+ ix = ix + incx;
+ iy = iy + incy;
+ }
+ }
+/*
+ Code for both increments equal to 1.
+*/
+ else
+ {
+ m = n % 5;
+
+ for ( i = 0; i < m; i++ )
+ {
+ dtemp = dtemp + dx[i] * dy[i];
+ }
+
+ for ( i = m; i < n; i = i + 5 )
+ {
+ dtemp = dtemp + dx[i ] * dy[i ]
+ + dx[i+1] * dy[i+1]
+ + dx[i+2] * dy[i+2]
+ + dx[i+3] * dy[i+3]
+ + dx[i+4] * dy[i+4];
+ }
+ }
+ return dtemp;
+}
+/******************************************************************************/
+
+float dnrm2 ( int n, float x[], int incx )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DNRM2 returns the euclidean norm of a vector.
+
+ Discussion:
+
+ DNRM2 ( X ) = sqrt ( X' * X )
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 30 March 2007
+
+ Author:
+
+ C version by John Burkardt
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979.
+
+ Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+ Basic Linear Algebra Subprograms for Fortran Usage,
+ Algorithm 539,
+ ACM Transactions on Mathematical Software,
+ Volume 5, Number 3, September 1979, pages 308-323.
+
+ Parameters:
+
+ Input, int N, the number of entries in the vector.
+
+ Input, float X[*], the vector whose norm is to be computed.
+
+ Input, int INCX, the increment between successive entries of X.
+
+ Output, float DNRM2, the Euclidean norm of X.
+*/
+{
+ float absxi;
+ int i;
+ int ix;
+ float norm;
+ float scale;
+ float ssq;
+
+ if ( n < 1 || incx < 1 )
+ {
+ norm = 0.0;
+ }
+ else if ( n == 1 )
+ {
+ norm = fabs ( x[0] );
+ }
+ else
+ {
+ scale = 0.0;
+ ssq = 1.0;
+ ix = 0;
+
+ for ( i = 0; i < n; i++ )
+ {
+ if ( x[ix] != 0.0 )
+ {
+ absxi = fabs ( x[ix] );
+ if ( scale < absxi )
+ {
+ ssq = 1.0 + ssq * ( scale / absxi ) * ( scale / absxi );
+ scale = absxi;
+ }
+ else
+ {
+ ssq = ssq + ( absxi / scale ) * ( absxi / scale );
+ }
+ }
+ ix = ix + incx;
+ }
+
+ norm = scale * sqrt ( ssq );
+ }
+
+ return norm;
+}
+/******************************************************************************/
+
+void dqrank ( float a[], int lda, int m, int n, float tol, int *kr,
+ int jpvt[], float qraux[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DQRANK computes the QR factorization of a rectangular matrix.
+
+ Discussion:
+
+ This routine is used in conjunction with DQRLSS to solve
+ overdetermined, underdetermined and singular linear systems
+ in a least squares sense.
+
+ DQRANK uses the LINPACK subroutine DQRDC to compute the QR
+ factorization, with column pivoting, of an M by N matrix A.
+ The numerical rank is determined using the tolerance TOL.
+
+ Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
+ of the condition number of the matrix of independent columns,
+ and of R. This estimate will be <= 1/TOL.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 21 April 2012
+
+ Author:
+
+ C version by John Burkardt.
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979,
+ ISBN13: 978-0-898711-72-1,
+ LC: QA214.L56.
+
+ Parameters:
+
+ Input/output, float A[LDA*N]. On input, the matrix whose
+ decomposition is to be computed. On output, the information from DQRDC.
+ The triangular matrix R of the QR factorization is contained in the
+ upper triangle and information needed to recover the orthogonal
+ matrix Q is stored below the diagonal in A and in the vector QRAUX.
+
+ Input, int LDA, the leading dimension of A, which must
+ be at least M.
+
+ Input, int M, the number of rows of A.
+
+ Input, int N, the number of columns of A.
+
+ Input, float TOL, a relative tolerance used to determine the
+ numerical rank. The problem should be scaled so that all the elements
+ of A have roughly the same absolute accuracy, EPS. Then a reasonable
+ value for TOL is roughly EPS divided by the magnitude of the largest
+ element.
+
+ Output, int *KR, the numerical rank.
+
+ Output, int JPVT[N], the pivot information from DQRDC.
+ Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
+ independent to within the tolerance TOL and the remaining columns
+ are linearly dependent.
+
+ Output, float QRAUX[N], will contain extra information defining
+ the QR factorization.
+*/
+{
+ int i;
+ int j;
+ int job;
+ int k;
+ /*float *work;*/
+
+ for ( i = 0; i < n; i++ )
+ {
+ jpvt[i] = 0;
+ }
+
+ float work[n];
+ /*work = ( float * ) malloc ( n * sizeof ( float ) );*/
+ job = 1;
+
+ dqrdc ( a, lda, m, n, qraux, jpvt, work, job );
+
+ *kr = 0;
+ k = i4_min ( m, n );
+
+ for ( j = 0; j < k; j++ )
+ {
+ if ( fabs ( a[j+j*lda] ) <= tol * fabs ( a[0+0*lda] ) )
+ {
+ return;
+ }
+ *kr = j + 1;
+ }
+
+ return;
+}
+/******************************************************************************/
+
+void dqrdc ( float a[], int lda, int n, int p, float qraux[], int jpvt[],
+ float work[], int job )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DQRDC computes the QR factorization of a real rectangular matrix.
+
+ Discussion:
+
+ DQRDC uses Householder transformations.
+
+ Column pivoting based on the 2-norms of the reduced columns may be
+ performed at the user's option.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 07 June 2005
+
+ Author:
+
+ C version by John Burkardt.
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, (Society for Industrial and Applied Mathematics),
+ 3600 University City Science Center,
+ Philadelphia, PA, 19104-2688.
+ ISBN 0-89871-172-X
+
+ Parameters:
+
+ Input/output, float A(LDA,P). On input, the N by P matrix
+ whose decomposition is to be computed. On output, A contains in
+ its upper triangle the upper triangular matrix R of the QR
+ factorization. Below its diagonal A contains information from
+ which the orthogonal part of the decomposition can be recovered.
+ Note that if pivoting has been requested, the decomposition is not that
+ of the original matrix A but that of A with its columns permuted
+ as described by JPVT.
+
+ Input, int LDA, the leading dimension of the array A. LDA must
+ be at least N.
+
+ Input, int N, the number of rows of the matrix A.
+
+ Input, int P, the number of columns of the matrix A.
+
+ Output, float QRAUX[P], contains further information required
+ to recover the orthogonal part of the decomposition.
+
+ Input/output, integer JPVT[P]. On input, JPVT contains integers that
+ control the selection of the pivot columns. The K-th column A(*,K) of A
+ is placed in one of three classes according to the value of JPVT(K).
+ > 0, then A(K) is an initial column.
+ = 0, then A(K) is a free column.
+ < 0, then A(K) is a final column.
+ Before the decomposition is computed, initial columns are moved to
+ the beginning of the array A and final columns to the end. Both
+ initial and final columns are frozen in place during the computation
+ and only free columns are moved. At the K-th stage of the
+ reduction, if A(*,K) is occupied by a free column it is interchanged
+ with the free column of largest reduced norm. JPVT is not referenced
+ if JOB == 0. On output, JPVT(K) contains the index of the column of the
+ original matrix that has been interchanged into the K-th column, if
+ pivoting was requested.
+
+ Workspace, float WORK[P]. WORK is not referenced if JOB == 0.
+
+ Input, int JOB, initiates column pivoting.
+ 0, no pivoting is done.
+ nonzero, pivoting is done.
+*/
+{
+ int j;
+ int jp;
+ int l;
+ int lup;
+ int maxj;
+ float maxnrm;
+ float nrmxl;
+ int pl;
+ int pu;
+ int swapj;
+ float t;
+ float tt;
+
+ pl = 1;
+ pu = 0;
+/*
+ If pivoting is requested, rearrange the columns.
+*/
+ if ( job != 0 )
+ {
+ for ( j = 1; j <= p; j++ )
+ {
+ swapj = ( 0 < jpvt[j-1] );
+
+ if ( jpvt[j-1] < 0 )
+ {
+ jpvt[j-1] = -j;
+ }
+ else
+ {
+ jpvt[j-1] = j;
+ }
+
+ if ( swapj )
+ {
+ if ( j != pl )
+ {
+ dswap ( n, a+0+(pl-1)*lda, 1, a+0+(j-1), 1 );
+ }
+ jpvt[j-1] = jpvt[pl-1];
+ jpvt[pl-1] = j;
+ pl = pl + 1;
+ }
+ }
+ pu = p;
+
+ for ( j = p; 1 <= j; j-- )
+ {
+ if ( jpvt[j-1] < 0 )
+ {
+ jpvt[j-1] = -jpvt[j-1];
+
+ if ( j != pu )
+ {
+ dswap ( n, a+0+(pu-1)*lda, 1, a+0+(j-1)*lda, 1 );
+ jp = jpvt[pu-1];
+ jpvt[pu-1] = jpvt[j-1];
+ jpvt[j-1] = jp;
+ }
+ pu = pu - 1;
+ }
+ }
+ }
+/*
+ Compute the norms of the free columns.
+*/
+ for ( j = pl; j <= pu; j++ )
+ {
+ qraux[j-1] = dnrm2 ( n, a+0+(j-1)*lda, 1 );
+ }
+
+ for ( j = pl; j <= pu; j++ )
+ {
+ work[j-1] = qraux[j-1];
+ }
+/*
+ Perform the Householder reduction of A.
+*/
+ lup = i4_min ( n, p );
+
+ for ( l = 1; l <= lup; l++ )
+ {
+/*
+ Bring the column of largest norm into the pivot position.
+*/
+ if ( pl <= l && l < pu )
+ {
+ maxnrm = 0.0;
+ maxj = l;
+ for ( j = l; j <= pu; j++ )
+ {
+ if ( maxnrm < qraux[j-1] )
+ {
+ maxnrm = qraux[j-1];
+ maxj = j;
+ }
+ }
+
+ if ( maxj != l )
+ {
+ dswap ( n, a+0+(l-1)*lda, 1, a+0+(maxj-1)*lda, 1 );
+ qraux[maxj-1] = qraux[l-1];
+ work[maxj-1] = work[l-1];
+ jp = jpvt[maxj-1];
+ jpvt[maxj-1] = jpvt[l-1];
+ jpvt[l-1] = jp;
+ }
+ }
+/*
+ Compute the Householder transformation for column L.
+*/
+ qraux[l-1] = 0.0;
+
+ if ( l != n )
+ {
+ nrmxl = dnrm2 ( n-l+1, a+l-1+(l-1)*lda, 1 );
+
+ if ( nrmxl != 0.0 )
+ {
+ if ( a[l-1+(l-1)*lda] != 0.0 )
+ {
+ nrmxl = nrmxl * r8_sign ( a[l-1+(l-1)*lda] );
+ }
+
+ dscal ( n-l+1, 1.0 / nrmxl, a+l-1+(l-1)*lda, 1 );
+ a[l-1+(l-1)*lda] = 1.0 + a[l-1+(l-1)*lda];
+/*
+ Apply the transformation to the remaining columns, updating the norms.
+*/
+ for ( j = l + 1; j <= p; j++ )
+ {
+ t = -ddot ( n-l+1, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 )
+ / a[l-1+(l-1)*lda];
+ daxpy ( n-l+1, t, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 );
+
+ if ( pl <= j && j <= pu )
+ {
+ if ( qraux[j-1] != 0.0 )
+ {
+ tt = 1.0 - pow ( fabs ( a[l-1+(j-1)*lda] ) / qraux[j-1], 2 );
+ tt = r8_max ( tt, 0.0 );
+ t = tt;
+ tt = 1.0 + 0.05 * tt * pow ( qraux[j-1] / work[j-1], 2 );
+
+ if ( tt != 1.0 )
+ {
+ qraux[j-1] = qraux[j-1] * sqrt ( t );
+ }
+ else
+ {
+ qraux[j-1] = dnrm2 ( n-l, a+l+(j-1)*lda, 1 );
+ work[j-1] = qraux[j-1];
+ }
+ }
+ }
+ }
+/*
+ Save the transformation.
+*/
+ qraux[l-1] = a[l-1+(l-1)*lda];
+ a[l-1+(l-1)*lda] = -nrmxl;
+ }
+ }
+ }
+ return;
+}
+/******************************************************************************/
+
+int dqrls ( float a[], int lda, int m, int n, float tol, int *kr, float b[],
+ float x[], float rsd[], int jpvt[], float qraux[], int itask )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DQRLS factors and solves a linear system in the least squares sense.
+
+ Discussion:
+
+ The linear system may be overdetermined, underdetermined or singular.
+ The solution is obtained using a QR factorization of the
+ coefficient matrix.
+
+ DQRLS can be efficiently used to solve several least squares
+ problems with the same matrix A. The first system is solved
+ with ITASK = 1. The subsequent systems are solved with
+ ITASK = 2, to avoid the recomputation of the matrix factors.
+ The parameters KR, JPVT, and QRAUX must not be modified
+ between calls to DQRLS.
+
+ DQRLS is used to solve in a least squares sense
+ overdetermined, underdetermined and singular linear systems.
+ The system is A*X approximates B where A is M by N.
+ B is a given M-vector, and X is the N-vector to be computed.
+ A solution X is found which minimimzes the sum of squares (2-norm)
+ of the residual, A*X - B.
+
+ The numerical rank of A is determined using the tolerance TOL.
+
+ DQRLS uses the LINPACK subroutine DQRDC to compute the QR
+ factorization, with column pivoting, of an M by N matrix A.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 10 September 2012
+
+ Author:
+
+ C version by John Burkardt.
+
+ Reference:
+
+ David Kahaner, Cleve Moler, Steven Nash,
+ Numerical Methods and Software,
+ Prentice Hall, 1989,
+ ISBN: 0-13-627258-4,
+ LC: TA345.K34.
+
+ Parameters:
+
+ Input/output, float A[LDA*N], an M by N matrix.
+ On input, the matrix whose decomposition is to be computed.
+ In a least squares data fitting problem, A(I,J) is the
+ value of the J-th basis (model) function at the I-th data point.
+ On output, A contains the output from DQRDC. The triangular matrix R
+ of the QR factorization is contained in the upper triangle and
+ information needed to recover the orthogonal matrix Q is stored
+ below the diagonal in A and in the vector QRAUX.
+
+ Input, int LDA, the leading dimension of A.
+
+ Input, int M, the number of rows of A.
+
+ Input, int N, the number of columns of A.
+
+ Input, float TOL, a relative tolerance used to determine the
+ numerical rank. The problem should be scaled so that all the elements
+ of A have roughly the same absolute accuracy EPS. Then a reasonable
+ value for TOL is roughly EPS divided by the magnitude of the largest
+ element.
+
+ Output, int *KR, the numerical rank.
+
+ Input, float B[M], the right hand side of the linear system.
+
+ Output, float X[N], a least squares solution to the linear
+ system.
+
+ Output, float RSD[M], the residual, B - A*X. RSD may
+ overwrite B.
+
+ Workspace, int JPVT[N], required if ITASK = 1.
+ Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
+ independent to within the tolerance TOL and the remaining columns
+ are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
+ of the condition number of the matrix of independent columns,
+ and of R. This estimate will be <= 1/TOL.
+
+ Workspace, float QRAUX[N], required if ITASK = 1.
+
+ Input, int ITASK.
+ 1, DQRLS factors the matrix A and solves the least squares problem.
+ 2, DQRLS assumes that the matrix A was factored with an earlier
+ call to DQRLS, and only solves the least squares problem.
+
+ Output, int DQRLS, error code.
+ 0: no error
+ -1: LDA < M (fatal error)
+ -2: N < 1 (fatal error)
+ -3: ITASK < 1 (fatal error)
+*/
+{
+ int ind;
+
+ if ( lda < m )
+ {
+ DEBUG_FPRINTF ( stderr, "\n" );
+ DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
+ DEBUG_FPRINTF ( stderr, " LDA < M.\n" );
+ ind = -1;
+ return ind;
+ }
+
+ if ( n <= 0 )
+ {
+ DEBUG_FPRINTF ( stderr, "\n" );
+ DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
+ DEBUG_FPRINTF ( stderr, " N <= 0.\n" );
+ ind = -2;
+ return ind;
+ }
+
+ if ( itask < 1 )
+ {
+ DEBUG_FPRINTF ( stderr, "\n" );
+ DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
+ DEBUG_FPRINTF ( stderr, " ITASK < 1.\n" );
+ ind = -3;
+ return ind;
+ }
+
+ ind = 0;
+/*
+ Factor the matrix.
+*/
+ if ( itask == 1 )
+ {
+ dqrank ( a, lda, m, n, tol, kr, jpvt, qraux );
+ }
+/*
+ Solve the least-squares problem.
+*/
+ dqrlss ( a, lda, m, n, *kr, b, x, rsd, jpvt, qraux );
+
+ return ind;
+}
+/******************************************************************************/
+void dqrlss ( float a[], int lda, int m, int n, int kr, float b[], float x[],
+ float rsd[], int jpvt[], float qraux[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DQRLSS solves a linear system in a least squares sense.
+
+ Discussion:
+
+ DQRLSS must be preceeded by a call to DQRANK.
+
+ The system is to be solved is
+ A * X = B
+ where
+ A is an M by N matrix with rank KR, as determined by DQRANK,
+ B is a given M-vector,
+ X is the N-vector to be computed.
+
+ A solution X, with at most KR nonzero components, is found which
+ minimizes the 2-norm of the residual (A*X-B).
+
+ Once the matrix A has been formed, DQRANK should be
+ called once to decompose it. Then, for each right hand
+ side B, DQRLSS should be called once to obtain the
+ solution and residual.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 10 September 2012
+
+ Author:
+
+ C version by John Burkardt
+
+ Parameters:
+
+ Input, float A[LDA*N], the QR factorization information
+ from DQRANK. The triangular matrix R of the QR factorization is
+ contained in the upper triangle and information needed to recover
+ the orthogonal matrix Q is stored below the diagonal in A and in
+ the vector QRAUX.
+
+ Input, int LDA, the leading dimension of A, which must
+ be at least M.
+
+ Input, int M, the number of rows of A.
+
+ Input, int N, the number of columns of A.
+
+ Input, int KR, the rank of the matrix, as estimated by DQRANK.
+
+ Input, float B[M], the right hand side of the linear system.
+
+ Output, float X[N], a least squares solution to the
+ linear system.
+
+ Output, float RSD[M], the residual, B - A*X. RSD may
+ overwite B.
+
+ Input, int JPVT[N], the pivot information from DQRANK.
+ Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
+ independent to within the tolerance TOL and the remaining columns
+ are linearly dependent.
+
+ Input, float QRAUX[N], auxiliary information from DQRANK
+ defining the QR factorization.
+*/
+{
+ int i;
+ int info UNUSED;
+ int j;
+ int job;
+ int k;
+ float t;
+
+ if ( kr != 0 )
+ {
+ job = 110;
+ info = dqrsl ( a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job );
+ }
+
+ for ( i = 0; i < n; i++ )
+ {
+ jpvt[i] = - jpvt[i];
+ }
+
+ for ( i = kr; i < n; i++ )
+ {
+ x[i] = 0.0;
+ }
+
+ for ( j = 1; j <= n; j++ )
+ {
+ if ( jpvt[j-1] <= 0 )
+ {
+ k = - jpvt[j-1];
+ jpvt[j-1] = k;
+
+ while ( k != j )
+ {
+ t = x[j-1];
+ x[j-1] = x[k-1];
+ x[k-1] = t;
+ jpvt[k-1] = -jpvt[k-1];
+ k = jpvt[k-1];
+ }
+ }
+ }
+ return;
+}
+/******************************************************************************/
+
+int dqrsl ( float a[], int lda, int n, int k, float qraux[], float y[],
+ float qy[], float qty[], float b[], float rsd[], float ab[], int job )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DQRSL computes transformations, projections, and least squares solutions.
+
+ Discussion:
+
+ DQRSL requires the output of DQRDC.
+
+ For K <= min(N,P), let AK be the matrix
+
+ AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
+
+ formed from columns JPVT[0], ..., JPVT(K) of the original
+ N by P matrix A that was input to DQRDC. If no pivoting was
+ done, AK consists of the first K columns of A in their
+ original order. DQRDC produces a factored orthogonal matrix Q
+ and an upper triangular matrix R such that
+
+ AK = Q * (R)
+ (0)
+
+ This information is contained in coded form in the arrays
+ A and QRAUX.
+
+ The parameters QY, QTY, B, RSD, and AB are not referenced
+ if their computation is not requested and in this case
+ can be replaced by dummy variables in the calling program.
+ To save storage, the user may in some cases use the same
+ array for different parameters in the calling sequence. A
+ frequently occuring example is when one wishes to compute
+ any of B, RSD, or AB and does not need Y or QTY. In this
+ case one may identify Y, QTY, and one of B, RSD, or AB, while
+ providing separate arrays for anything else that is to be
+ computed.
+
+ Thus the calling sequence
+
+ dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
+
+ will result in the computation of B and RSD, with RSD
+ overwriting Y. More generally, each item in the following
+ list contains groups of permissible identifications for
+ a single calling sequence.
+
+ 1. (Y,QTY,B) (RSD) (AB) (QY)
+
+ 2. (Y,QTY,RSD) (B) (AB) (QY)
+
+ 3. (Y,QTY,AB) (B) (RSD) (QY)
+
+ 4. (Y,QY) (QTY,B) (RSD) (AB)
+
+ 5. (Y,QY) (QTY,RSD) (B) (AB)
+
+ 6. (Y,QY) (QTY,AB) (B) (RSD)
+
+ In any group the value returned in the array allocated to
+ the group corresponds to the last member of the group.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 07 June 2005
+
+ Author:
+
+ C version by John Burkardt.
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, (Society for Industrial and Applied Mathematics),
+ 3600 University City Science Center,
+ Philadelphia, PA, 19104-2688.
+ ISBN 0-89871-172-X
+
+ Parameters:
+
+ Input, float A[LDA*P], contains the output of DQRDC.
+
+ Input, int LDA, the leading dimension of the array A.
+
+ Input, int N, the number of rows of the matrix AK. It must
+ have the same value as N in DQRDC.
+
+ Input, int K, the number of columns of the matrix AK. K
+ must not be greater than min(N,P), where P is the same as in the
+ calling sequence to DQRDC.
+
+ Input, float QRAUX[P], the auxiliary output from DQRDC.
+
+ Input, float Y[N], a vector to be manipulated by DQRSL.
+
+ Output, float QY[N], contains Q * Y, if requested.
+
+ Output, float QTY[N], contains Q' * Y, if requested.
+
+ Output, float B[K], the solution of the least squares problem
+ minimize norm2 ( Y - AK * B),
+ if its computation has been requested. Note that if pivoting was
+ requested in DQRDC, the J-th component of B will be associated with
+ column JPVT(J) of the original matrix A that was input into DQRDC.
+
+ Output, float RSD[N], the least squares residual Y - AK * B,
+ if its computation has been requested. RSD is also the orthogonal
+ projection of Y onto the orthogonal complement of the column space
+ of AK.
+
+ Output, float AB[N], the least squares approximation Ak * B,
+ if its computation has been requested. AB is also the orthogonal
+ projection of Y onto the column space of A.
+
+ Input, integer JOB, specifies what is to be computed. JOB has
+ the decimal expansion ABCDE, with the following meaning:
+
+ if A != 0, compute QY.
+ if B != 0, compute QTY.
+ if C != 0, compute QTY and B.
+ if D != 0, compute QTY and RSD.
+ if E != 0, compute QTY and AB.
+
+ Note that a request to compute B, RSD, or AB automatically triggers
+ the computation of QTY, for which an array must be provided in the
+ calling sequence.
+
+ Output, int DQRSL, is zero unless the computation of B has
+ been requested and R is exactly singular. In this case, INFO is the
+ index of the first zero diagonal element of R, and B is left unaltered.
+*/
+{
+ int cab;
+ int cb;
+ int cqty;
+ int cqy;
+ int cr;
+ int i;
+ int info;
+ int j;
+ int jj;
+ int ju;
+ float t;
+ float temp;
+/*
+ Set INFO flag.
+*/
+ info = 0;
+/*
+ Determine what is to be computed.
+*/
+ cqy = ( job / 10000 != 0 );
+ cqty = ( ( job % 10000 ) != 0 );
+ cb = ( ( job % 1000 ) / 100 != 0 );
+ cr = ( ( job % 100 ) / 10 != 0 );
+ cab = ( ( job % 10 ) != 0 );
+
+ ju = i4_min ( k, n-1 );
+/*
+ Special action when N = 1.
+*/
+ if ( ju == 0 )
+ {
+ if ( cqy )
+ {
+ qy[0] = y[0];
+ }
+
+ if ( cqty )
+ {
+ qty[0] = y[0];
+ }
+
+ if ( cab )
+ {
+ ab[0] = y[0];
+ }
+
+ if ( cb )
+ {
+ if ( a[0+0*lda] == 0.0 )
+ {
+ info = 1;
+ }
+ else
+ {
+ b[0] = y[0] / a[0+0*lda];
+ }
+ }
+
+ if ( cr )
+ {
+ rsd[0] = 0.0;
+ }
+ return info;
+ }
+/*
+ Set up to compute QY or QTY.
+*/
+ if ( cqy )
+ {
+ for ( i = 1; i <= n; i++ )
+ {
+ qy[i-1] = y[i-1];
+ }
+ }
+
+ if ( cqty )
+ {
+ for ( i = 1; i <= n; i++ )
+ {
+ qty[i-1] = y[i-1];
+ }
+ }
+/*
+ Compute QY.
+*/
+ if ( cqy )
+ {
+ for ( jj = 1; jj <= ju; jj++ )
+ {
+ j = ju - jj + 1;
+
+ if ( qraux[j-1] != 0.0 )
+ {
+ temp = a[j-1+(j-1)*lda];
+ a[j-1+(j-1)*lda] = qraux[j-1];
+ t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qy+j-1, 1 ) / a[j-1+(j-1)*lda];
+ daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qy+j-1, 1 );
+ a[j-1+(j-1)*lda] = temp;
+ }
+ }
+ }
+/*
+ Compute Q'*Y.
+*/
+ if ( cqty )
+ {
+ for ( j = 1; j <= ju; j++ )
+ {
+ if ( qraux[j-1] != 0.0 )
+ {
+ temp = a[j-1+(j-1)*lda];
+ a[j-1+(j-1)*lda] = qraux[j-1];
+ t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qty+j-1, 1 ) / a[j-1+(j-1)*lda];
+ daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qty+j-1, 1 );
+ a[j-1+(j-1)*lda] = temp;
+ }
+ }
+ }
+/*
+ Set up to compute B, RSD, or AB.
+*/
+ if ( cb )
+ {
+ for ( i = 1; i <= k; i++ )
+ {
+ b[i-1] = qty[i-1];
+ }
+ }
+
+ if ( cab )
+ {
+ for ( i = 1; i <= k; i++ )
+ {
+ ab[i-1] = qty[i-1];
+ }
+ }
+
+ if ( cr && k < n )
+ {
+ for ( i = k+1; i <= n; i++ )
+ {
+ rsd[i-1] = qty[i-1];
+ }
+ }
+
+ if ( cab && k+1 <= n )
+ {
+ for ( i = k+1; i <= n; i++ )
+ {
+ ab[i-1] = 0.0;
+ }
+ }
+
+ if ( cr )
+ {
+ for ( i = 1; i <= k; i++ )
+ {
+ rsd[i-1] = 0.0;
+ }
+ }
+/*
+ Compute B.
+*/
+ if ( cb )
+ {
+ for ( jj = 1; jj <= k; jj++ )
+ {
+ j = k - jj + 1;
+
+ if ( a[j-1+(j-1)*lda] == 0.0 )
+ {
+ info = j;
+ break;
+ }
+
+ b[j-1] = b[j-1] / a[j-1+(j-1)*lda];
+
+ if ( j != 1 )
+ {
+ t = -b[j-1];
+ daxpy ( j-1, t, a+0+(j-1)*lda, 1, b, 1 );
+ }
+ }
+ }
+/*
+ Compute RSD or AB as required.
+*/
+ if ( cr || cab )
+ {
+ for ( jj = 1; jj <= ju; jj++ )
+ {
+ j = ju - jj + 1;
+
+ if ( qraux[j-1] != 0.0 )
+ {
+ temp = a[j-1+(j-1)*lda];
+ a[j-1+(j-1)*lda] = qraux[j-1];
+
+ if ( cr )
+ {
+ t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 )
+ / a[j-1+(j-1)*lda];
+ daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 );
+ }
+
+ if ( cab )
+ {
+ t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, ab+j-1, 1 )
+ / a[j-1+(j-1)*lda];
+ daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, ab+j-1, 1 );
+ }
+ a[j-1+(j-1)*lda] = temp;
+ }
+ }
+ }
+
+ return info;
+}
+/******************************************************************************/
+
+void dscal ( int n, float sa, float x[], int incx )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DSCAL scales a vector by a constant.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 30 March 2007
+
+ Author:
+
+ C version by John Burkardt
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979.
+
+ Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+ Basic Linear Algebra Subprograms for Fortran Usage,
+ Algorithm 539,
+ ACM Transactions on Mathematical Software,
+ Volume 5, Number 3, September 1979, pages 308-323.
+
+ Parameters:
+
+ Input, int N, the number of entries in the vector.
+
+ Input, float SA, the multiplier.
+
+ Input/output, float X[*], the vector to be scaled.
+
+ Input, int INCX, the increment between successive entries of X.
+*/
+{
+ int i;
+ int ix;
+ int m;
+
+ if ( n <= 0 )
+ {
+ }
+ else if ( incx == 1 )
+ {
+ m = n % 5;
+
+ for ( i = 0; i < m; i++ )
+ {
+ x[i] = sa * x[i];
+ }
+
+ for ( i = m; i < n; i = i + 5 )
+ {
+ x[i] = sa * x[i];
+ x[i+1] = sa * x[i+1];
+ x[i+2] = sa * x[i+2];
+ x[i+3] = sa * x[i+3];
+ x[i+4] = sa * x[i+4];
+ }
+ }
+ else
+ {
+ if ( 0 <= incx )
+ {
+ ix = 0;
+ }
+ else
+ {
+ ix = ( - n + 1 ) * incx;
+ }
+
+ for ( i = 0; i < n; i++ )
+ {
+ x[ix] = sa * x[ix];
+ ix = ix + incx;
+ }
+ }
+ return;
+}
+/******************************************************************************/
+
+void dswap ( int n, float x[], int incx, float y[], int incy )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ DSWAP interchanges two vectors.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 30 March 2007
+
+ Author:
+
+ C version by John Burkardt
+
+ Reference:
+
+ Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+ LINPACK User's Guide,
+ SIAM, 1979.
+
+ Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+ Basic Linear Algebra Subprograms for Fortran Usage,
+ Algorithm 539,
+ ACM Transactions on Mathematical Software,
+ Volume 5, Number 3, September 1979, pages 308-323.
+
+ Parameters:
+
+ Input, int N, the number of entries in the vectors.
+
+ Input/output, float X[*], one of the vectors to swap.
+
+ Input, int INCX, the increment between successive entries of X.
+
+ Input/output, float Y[*], one of the vectors to swap.
+
+ Input, int INCY, the increment between successive elements of Y.
+*/
+{
+ int i;
+ int ix;
+ int iy;
+ int m;
+ float temp;
+
+ if ( n <= 0 )
+ {
+ }
+ else if ( incx == 1 && incy == 1 )
+ {
+ m = n % 3;
+
+ for ( i = 0; i < m; i++ )
+ {
+ temp = x[i];
+ x[i] = y[i];
+ y[i] = temp;
+ }
+
+ for ( i = m; i < n; i = i + 3 )
+ {
+ temp = x[i];
+ x[i] = y[i];
+ y[i] = temp;
+
+ temp = x[i+1];
+ x[i+1] = y[i+1];
+ y[i+1] = temp;
+
+ temp = x[i+2];
+ x[i+2] = y[i+2];
+ y[i+2] = temp;
+ }
+ }
+ else
+ {
+ if ( 0 <= incx )
+ {
+ ix = 0;
+ }
+ else
+ {
+ ix = ( - n + 1 ) * incx;
+ }
+
+ if ( 0 <= incy )
+ {
+ iy = 0;
+ }
+ else
+ {
+ iy = ( - n + 1 ) * incy;
+ }
+
+ for ( i = 0; i < n; i++ )
+ {
+ temp = x[ix];
+ x[ix] = y[iy];
+ y[iy] = temp;
+ ix = ix + incx;
+ iy = iy + incy;
+ }
+
+ }
+
+ return;
+}
+/******************************************************************************/
+
+void qr_solve ( int m, int n, float a[], float b[], float x[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ QR_SOLVE solves a linear system in the least squares sense.
+
+ Discussion:
+
+ If the matrix A has full column rank, then the solution X should be the
+ unique vector that minimizes the Euclidean norm of the residual.
+
+ If the matrix A does not have full column rank, then the solution is
+ not unique; the vector X will minimize the residual norm, but so will
+ various other vectors.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 11 September 2012
+
+ Author:
+
+ John Burkardt
+
+ Reference:
+
+ David Kahaner, Cleve Moler, Steven Nash,
+ Numerical Methods and Software,
+ Prentice Hall, 1989,
+ ISBN: 0-13-627258-4,
+ LC: TA345.K34.
+
+ Parameters:
+
+ Input, int M, the number of rows of A.
+
+ Input, int N, the number of columns of A.
+
+ Input, float A[M*N], the matrix.
+
+ Input, float B[M], the right hand side.
+
+ Output, float QR_SOLVE[N], the least squares solution.
+*/
+{
+ int ind UNUSED;
+ int itask;
+ int kr;
+ int lda;
+ float tol;
+
+ float a_qr[m*n];
+ r8mat_copy_new ( m, n, a, a_qr );
+ lda = m;
+ tol = r8_epsilon ( ) / r8mat_amax ( m, n, a_qr );
+ int jpvt[n];
+ float qraux[n];
+ float r[m];
+ itask = 1;
+
+ ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask );
+}
+/******************************************************************************/
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/qr_solve.h b/sw/airborne/firmwares/rotorcraft/stabilization/wls/qr_solve.h
new file mode 100644
index 0000000000..5e54b6c9e5
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/wls/qr_solve.h
@@ -0,0 +1,27 @@
+/*
+ * This is part of the qr_solve library from John Burkardt.
+ * http://people.sc.fsu.edu/~jburkardt/c_src/qr_solve/qr_solve.html
+ *
+ * It is slightly modified to make it compile on simple microprocessors,
+ * and to remove all dynamic memory.
+ *
+ * This code is distributed under the GNU LGPL license.
+ */
+
+void daxpy ( int n, float da, float dx[], int incx, float dy[], int incy );
+float ddot ( int n, float dx[], int incx, float dy[], int incy );
+float dnrm2 ( int n, float x[], int incx );
+void dqrank ( float a[], int lda, int m, int n, float tol, int *kr,
+ int jpvt[], float qraux[] );
+void dqrdc ( float a[], int lda, int n, int p, float qraux[], int jpvt[],
+ float work[], int job );
+int dqrls ( float a[], int lda, int m, int n, float tol, int *kr, float b[],
+ float x[], float rsd[], int jpvt[], float qraux[], int itask );
+void dqrlss ( float a[], int lda, int m, int n, int kr, float b[], float x[],
+ float rsd[], int jpvt[], float qraux[] );
+int dqrsl ( float a[], int lda, int n, int k, float qraux[], float y[],
+ float qy[], float qty[], float b[], float rsd[], float ab[], int job );
+void drotg ( float *sa, float *sb, float *c, float *s );
+void dscal ( int n, float sa, float x[], int incx );
+void dswap ( int n, float x[], int incx, float y[], int incy );
+void qr_solve ( int m, int n, float a[], float b[], float x[] );
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.c b/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.c
new file mode 100644
index 0000000000..bd2ff3998b
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.c
@@ -0,0 +1,554 @@
+/*
+ * This file is a modified subset of the R8lib from John Burkardt.
+ * http://people.sc.fsu.edu/~jburkardt/c_src/r8lib/r8lib.html
+ *
+ * It is the minimal set of functions from r8lib needed to use qr_solve.
+ *
+ * This code is distributed under the GNU LGPL license.
+ */
+
+# include "r8lib_min.h"
+#include "std.h"
+# include
+# include
+
+#define DEBUG_FPRINTF(...)
+#define DEBUG_PRINT(...)
+#define DEBUG_EXIT(...)
+
+void r8mat_copy_new ( int m, int n, float a1[], float a2[])
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
+
+ Discussion:
+
+ An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+ in column-major order.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 26 July 2008
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int M, N, the number of rows and columns.
+
+ Input, float A1[M*N], the matrix to be copied.
+
+ Output, float R8MAT_COPY_NEW[M*N], the copy of A1.
+*/
+{
+ int i;
+ int j;
+
+ /*a2 = ( float * ) malloc ( m * n * sizeof ( float ) );*/
+
+ for ( j = 0; j < n; j++ )
+ {
+ for ( i = 0; i < m; i++ )
+ {
+ a2[i+j*m] = a1[i+j*m];
+ }
+ }
+}
+/******************************************************************************/
+
+float r8_epsilon ( void )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8_EPSILON returns the R8 round off unit.
+
+ Discussion:
+
+ R8_EPSILON is a number R which is a power of 2 with the property that,
+ to the precision of the computer's arithmetic,
+ 1 < 1 + R
+ but
+ 1 = ( 1 + R / 2 )
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 01 September 2012
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Output, float R8_EPSILON, the R8 round-off unit.
+*/
+{
+ const float value = 1.192092896E-7;
+
+ return value;
+}
+/******************************************************************************/
+
+float r8mat_amax ( int m, int n, float a[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
+
+ Discussion:
+
+ An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+ in column-major order.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 07 September 2012
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int M, the number of rows in A.
+
+ Input, int N, the number of columns in A.
+
+ Input, float A[M*N], the M by N matrix.
+
+ Output, float R8MAT_AMAX, the maximum absolute value entry of A.
+*/
+{
+ int i;
+ int j;
+ float value;
+
+ value = fabs ( a[0+0*m] );
+
+ for ( j = 0; j < n; j++ )
+ {
+ for ( i = 0; i < m; i++ )
+ {
+ if ( value < fabs ( a[i+j*m] ) )
+ {
+ value = fabs ( a[i+j*m] );
+ }
+ }
+ }
+ return value;
+}
+/******************************************************************************/
+
+float r8_sign ( float x )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8_SIGN returns the sign of an R8.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 08 May 2006
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, float X, the number whose sign is desired.
+
+ Output, float R8_SIGN, the sign of X.
+*/
+{
+ float value;
+
+ if ( x < 0.0 )
+ {
+ value = - 1.0;
+ }
+ else
+ {
+ value = + 1.0;
+ }
+ return value;
+}
+/******************************************************************************/
+
+float r8_max ( float x, float y )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8_MAX returns the maximum of two R8's.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 07 May 2006
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, float X, Y, the quantities to compare.
+
+ Output, float R8_MAX, the maximum of X and Y.
+*/
+{
+ float value;
+
+ if ( y < x )
+ {
+ value = x;
+ }
+ else
+ {
+ value = y;
+ }
+ return value;
+}
+/******************************************************************************/
+
+float *r8mat_l_solve ( int n, float a[], float b[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8MAT_L_SOLVE solves a lower triangular linear system.
+
+ Discussion:
+
+ An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+ in column-major order.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 07 June 2008
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int N, the number of rows and columns of
+ the matrix A.
+
+ Input, float A[N*N], the N by N lower triangular matrix.
+
+ Input, float B[N], the right hand side of the linear system.
+
+ Output, float R8MAT_L_SOLVE[N], the solution of the linear system.
+*/
+{
+ float dot;
+ int i;
+ int j;
+ float *x;
+
+ x = ( float * ) malloc ( n * sizeof ( float ) );
+/*
+ Solve L * x = b.
+*/
+ for ( i = 0; i < n; i++ )
+ {
+ dot = 0.0;
+ for ( j = 0; j < i; j++ )
+ {
+ dot = dot + a[i+j*n] * x[j];
+ }
+ x[i] = ( b[i] - dot ) / a[i+i*n];
+ }
+
+ return x;
+}
+/******************************************************************************/
+
+float *r8mat_lt_solve ( int n, float a[], float b[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8MAT_LT_SOLVE solves a transposed lower triangular linear system.
+
+ Discussion:
+
+ An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+ in column-major order.
+
+ Given the lower triangular matrix A, the linear system to be solved is:
+
+ A' * x = b
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 08 April 2009
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int N, the number of rows and columns of the matrix A.
+
+ Input, float A[N*N], the N by N lower triangular matrix.
+
+ Input, float B[N], the right hand side of the linear system.
+
+ Output, float R8MAT_LT_SOLVE[N], the solution of the linear system.
+*/
+{
+ int i;
+ int j;
+ float *x;
+
+ x = ( float * ) malloc ( n * sizeof ( float ) );
+
+ for ( j = n-1; 0 <= j; j-- )
+ {
+ x[j] = b[j];
+ for ( i = j+1; i < n; i++ )
+ {
+ x[j] = x[j] - x[i] * a[i+j*n];
+ }
+ x[j] = x[j] / a[j+j*n];
+ }
+
+ return x;
+}
+/******************************************************************************/
+
+float *r8mat_mtv_new ( int m, int n, float a[], float x[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8MAT_MTV_NEW multiplies a transposed matrix times a vector.
+
+ Discussion:
+
+ An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+ in column-major order.
+
+ For this routine, the result is returned as the function value.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 26 August 2011
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int M, N, the number of rows and columns of the matrix.
+
+ Input, float A[M,N], the M by N matrix.
+
+ Input, float X[M], the vector to be multiplied by A.
+
+ Output, float R8MAT_MTV_NEW[N], the product A'*X.
+*/
+{
+ int i;
+ int j;
+ float *y;
+
+ y = ( float * ) malloc ( n * sizeof ( float ) );
+
+ for ( j = 0; j < n; j++ )
+ {
+ y[j] = 0.0;
+ for ( i = 0; i < m; i++ )
+ {
+ y[j] = y[j] + a[i+j*m] * x[i];
+ }
+ }
+
+ return y;
+}
+/******************************************************************************/
+
+float r8vec_max ( int n, float r8vec[] )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ R8VEC_MAX returns the value of the maximum element in a R8VEC.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 05 May 2006
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int N, the number of entries in the array.
+
+ Input, float R8VEC[N], a pointer to the first entry of the array.
+
+ Output, float R8VEC_MAX, the value of the maximum element. This
+ is set to 0.0 if N <= 0.
+*/
+{
+ int i;
+ float value;
+
+ if ( n <= 0 )
+ {
+ value = 0.0;
+ return value;
+ }
+
+ value = r8vec[0];
+
+ for ( i = 1; i < n; i++ )
+ {
+ if ( value < r8vec[i] )
+ {
+ value = r8vec[i];
+ }
+ }
+ return value;
+}
+/******************************************************************************/
+
+int i4_min ( int i1, int i2 )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ I4_MIN returns the smaller of two I4's.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 29 August 2006
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int I1, I2, two integers to be compared.
+
+ Output, int I4_MIN, the smaller of I1 and I2.
+*/
+{
+ int value;
+
+ if ( i1 < i2 )
+ {
+ value = i1;
+ }
+ else
+ {
+ value = i2;
+ }
+ return value;
+}
+/******************************************************************************/
+
+int i4_max ( int i1, int i2 )
+
+/******************************************************************************/
+/*
+ Purpose:
+
+ I4_MAX returns the maximum of two I4's.
+
+ Licensing:
+
+ This code is distributed under the GNU LGPL license.
+
+ Modified:
+
+ 29 August 2006
+
+ Author:
+
+ John Burkardt
+
+ Parameters:
+
+ Input, int I1, I2, are two integers to be compared.
+
+ Output, int I4_MAX, the larger of I1 and I2.
+*/
+{
+ int value;
+
+ if ( i2 < i1 )
+ {
+ value = i1;
+ }
+ else
+ {
+ value = i2;
+ }
+ return value;
+}
+/******************************************************************************/
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.h b/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.h
new file mode 100644
index 0000000000..72e96552be
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.h
@@ -0,0 +1,25 @@
+/*
+ * This file is a modified subset of the R8lib from John Burkardt.
+ * http://people.sc.fsu.edu/~jburkardt/c_src/r8lib/r8lib.html
+ *
+ * It is the minimal set of functions from r8lib needed to use qr_solve.
+ *
+ * This code is distributed under the GNU LGPL license.
+ */
+
+void r8mat_copy_new ( int m, int n, float a1[], float a2[] );
+float r8_epsilon ( void );
+float r8mat_amax ( int m, int n, float a[] );
+float r8_sign ( float x );
+float r8_max ( float x, float y );
+float *r8mat_transpose_new ( int m, int n, float a[] );
+float *r8mat_mm_new ( int n1, int n2, int n3, float a[], float b[] );
+float *r8mat_cholesky_factor ( int n, float a[], int *flag );
+float *r8mat_mv_new ( int m, int n, float a[], float x[] );
+float *r8mat_cholesky_solve ( int n, float l[], float b[] );
+float *r8mat_l_solve ( int n, float a[], float b[] );
+float *r8mat_lt_solve ( int n, float a[], float b[] );
+float *r8mat_mtv_new ( int m, int n, float a[], float x[] );
+float r8vec_max ( int n, float r8vec[] );
+int i4_min ( int i1, int i2 );
+int i4_max ( int i1, int i2 );
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c b/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
new file mode 100644
index 0000000000..e5d04dcb28
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
@@ -0,0 +1,347 @@
+/*
+ * Copyright (C) Anton Naruta && Daniel Hoppener
+ * MAVLab Delft University of Technology
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file wls_alloc.c
+ * @brief This is an active set algorithm for WLS control allocation
+ *
+ * This algorithm will find the optimal inputs to produce the least error wrt
+ * the control objective, taking into account the weighting matrices on the
+ * control objective and the control effort.
+ *
+ * The algorithm is described in:
+ * Prioritized Control Allocation for Quadrotors Subject to Saturation -
+ * E.J.J. Smeur, D.C. Höppener, C. de Wagter. Submitted to IMAV 2017
+ *
+ * written by Anton Naruta && Daniel Hoppener 2016
+ * MAVLab Delft University of Technology
+ */
+
+#include "wls_alloc.h"
+#include
+#include "std.h"
+
+void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
+void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
+
+// provide loop feedback
+#define WLS_VERBOSE FALSE
+
+// the wrapper can use any solver function
+void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
+ float in[m * n];
+ // convert A to 1d array
+ int k = 0;
+ for (int j = 0; j < n; j++) {
+ for (int i = 0; i < m; i++) {
+ in[k++] = A[i][j];
+ }
+ }
+ // use solver
+ qr_solve(m, n, in, b, x);
+}
+
+/**
+ * @brief active set algorithm for control allocation
+ *
+ * Takes the control objective and max and min inputs from pprz and calculates
+ * the inputs that will satisfy most of the control objective, subject to the
+ * weighting matrices Wv and Wu
+ *
+ * @param u The control output vector
+ * @param v The control objective
+ * @param umin The minimum u vector
+ * @param umax The maximum u vector
+ * @param B The control effectiveness matrix
+ * @param n_u Length of u
+ * @param n_v Lenght of v
+ * @param u_guess Initial value for u
+ * @param W_init Initial working set, if known
+ * @param Wv Weighting on different control objectives
+ * @param Wu Weighting on different controls
+ * @param up Preferred control vector
+ * @param gamma_sq Preference of satisfying control objective over desired
+ * control vector (sqare root of gamma)
+ * @param imax Max number of iterations
+ *
+ * @return Number of iterations, -1 upon failure
+ */
+int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
+ int n_u, int n_v, float* u_guess, float* W_init, float* Wv,
+ float* Wu, float* up, float gamma_sq, int imax) {
+ // allocate variables, use defaults where parameters are set to 0
+ if(!gamma_sq) gamma_sq = 100000;
+ if(!imax) imax = 100;
+ int n_c = n_u + n_v;
+
+ float A[n_c][n_u];
+ float A_free[n_c][n_u];
+
+ // Create a pointer array to the rows of A_free
+ // such that we can pass it to a function
+ float * A_free_ptr[n_c];
+ for(int i = 0; i < n_c; i++)
+ A_free_ptr[i] = A_free[i];
+
+ float b[n_c];
+ float d[n_c];
+
+ int free_index[n_u];
+ int free_index_lookup[n_u];
+ int n_free = 0;
+ int free_chk = -1;
+
+ int iter = 0;
+ float p_free[n_u];
+ float p[n_u];
+ float u_opt[n_u];
+ int infeasible_index[n_u] UNUSED;
+ int n_infeasible = 0;
+ float lambda[n_u];
+ float W[n_u];
+
+ // Initialize u and the working set, if provided from input
+ if (!u_guess) {
+ for (int i = 0; i < n_u; i++) {
+ u[i] = (umax[i] + umin[i]) * 0.5;
+ }
+ } else {
+ for (int i = 0; i < n_u; i++) {
+ u[i] = u_guess[i];
+ }
+ }
+ W_init ? memcpy(W, W_init, n_u * sizeof(float))
+ : memset(W, 0, n_u * sizeof(float));
+
+ memset(free_index_lookup, -1, n_u * sizeof(float));
+
+
+ // find free indices
+ for (int i = 0; i < n_u; i++) {
+ if (W[i] == 0) {
+ free_index_lookup[i] = n_free;
+ free_index[n_free++] = i;
+ }
+ }
+
+ // fill up A, A_free, b and d
+ for (int i = 0; i < n_v; i++) {
+ // If Wv is a NULL pointer, use Wv = identity
+ b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
+ d[i] = b[i];
+ for (int j = 0; j < n_u; j++) {
+ // If Wv is a NULL pointer, use Wv = identity
+ A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
+ d[i] -= A[i][j] * u[j];
+ }
+ }
+ for (int i = n_v; i < n_c; i++) {
+ memset(A[i], 0, n_u * sizeof(float));
+ A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
+ b[i] = up ? (Wu ? Wu[i] * up[i] : up[i]) : 0;
+ d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
+ }
+
+ // -------------- Start loop ------------
+ while (iter++ < imax) {
+ // clear p, copy u to u_opt
+ memset(p, 0, n_u * sizeof(float));
+ memcpy(u_opt, u, n_u * sizeof(float));
+
+ // Construct a matrix with the free columns of A
+ if (free_chk != n_free) {
+ for (int i = 0; i < n_c; i++) {
+ for (int j = 0; j < n_free; j++) {
+ A_free[i][j] = A[i][free_index[j]];
+ }
+ }
+ free_chk = n_free;
+ }
+
+
+ if (n_free) {
+ // Still free variables left, calculate corresponding solution
+
+ // use a solver to find the solution to A_free*p_free = d
+ qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
+
+ //print results current step
+#if WLS_VERBOSE
+ print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
+#endif
+
+ }
+
+ // Set the nonzero values of p and add to u_opt
+ for (int i = 0; i < n_free; i++) {
+ p[free_index[i]] = p_free[i];
+ u_opt[free_index[i]] += p_free[i];
+ }
+ // check limits
+ n_infeasible = 0;
+ for (int i = 0; i < n_u; i++) {
+ if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
+ infeasible_index[n_infeasible++] = i;
+ }
+ }
+
+ // Check feasibility of the solution
+ if (n_infeasible == 0) {
+ // all variables are within limits
+ memcpy(u, u_opt, n_u * sizeof(float));
+ memset(lambda, 0, n_u * sizeof(float));
+
+ // d = d + A_free*p_free; lambda = A*d;
+ for (int i = 0; i < n_c; i++) {
+ for (int k = 0; k < n_free; k++) {
+ d[i] -= A_free[i][k] * p_free[k];
+ }
+ for (int k = 0; k < n_u; k++) {
+ lambda[k] += A[i][k] * d[i];
+ }
+ }
+ bool break_flag = true;
+
+ // lambda = lambda x W;
+ for (int i = 0; i < n_u; i++) {
+ lambda[i] *= W[i];
+ // if any lambdas are negative, keep looking for solution
+ if (lambda[i] < -FLT_EPSILON) {
+ break_flag = false;
+ W[i] = 0;
+ // add a free index
+ if (free_index_lookup[i] < 0) {
+ free_index_lookup[i] = n_free;
+ free_index[n_free++] = i;
+ }
+ }
+ }
+ if (break_flag) {
+
+#if WLS_VERBOSE
+ print_final_values(1, n_u, n_v, u, B, v, umin, umax);
+#endif
+
+ // if solution is found, return number of iterations
+ return iter;
+ }
+ } else {
+ float alpha = INFINITY;
+ float alpha_tmp;
+ int id_alpha = 0;
+
+ // find the lowest distance from the limit among the free variables
+ for (int i = 0; i < n_free; i++) {
+ int id = free_index[i];
+ if(fabs(p[id]) > FLT_EPSILON) {
+ alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
+ : (umax[id] - u[id]) / p[id];
+ } else {
+ alpha_tmp = INFINITY;
+ }
+ if (alpha_tmp < alpha) {
+ alpha = alpha_tmp;
+ id_alpha = id;
+ }
+ }
+
+ // update input u = u + alpha*p
+ for (int i = 0; i < n_u; i++) {
+ u[i] += alpha * p[i];
+ }
+ // update d = d-alpha*A*p_free
+ for (int i = 0; i < n_c; i++) {
+ for (int k = 0; k < n_free; k++) {
+ d[i] -= A_free[i][k] * alpha * p_free[k];
+ }
+ }
+ // get rid of a free index
+ W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
+
+ free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
+ free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
+ free_index_lookup[id_alpha];
+ free_index_lookup[id_alpha] = -1;
+ }
+ }
+ // solution failed, return negative one to indicate failure
+ return -1;
+}
+
+void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
+
+ printf("n_c = %d n_free = %d\n", n_c, n_free);
+
+ printf("A_free =\n");
+ for(int i = 0; i < n_c; i++) {
+ for (int j = 0; j < n_free; j++) {
+ printf("%f ", A_free_ptr[i][j]);
+ }
+ printf("\n");
+ }
+
+ printf("d = ");
+ for (int j = 0; j < n_c; j++) {
+ printf("%f ", d[j]);
+ }
+
+ printf("\noutput = ");
+ for (int j = 0; j < n_free; j++) {
+ printf("%f ", p_free[j]);
+ }
+ printf("\n\n");
+}
+
+void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
+ printf("n_u = %d n_v = %d\n", n_u, n_v);
+
+ printf("B =\n");
+ for(int i = 0; i < n_v; i++) {
+ for (int j = 0; j < n_u; j++) {
+ printf("%f ", B[i][j]);
+ }
+ printf("\n");
+ }
+
+ printf("v = ");
+ for (int j = 0; j < n_v; j++) {
+ printf("%f ", v[j]);
+ }
+
+ printf("\nu = ");
+ for (int j = 0; j < n_u; j++) {
+ printf("%f ", u[j]);
+ }
+ printf("\n");
+
+ printf("\numin = ");
+ for (int j = 0; j < n_u; j++) {
+ printf("%f ", umin[j]);
+ }
+ printf("\n");
+
+ printf("\numax = ");
+ for (int j = 0; j < n_u; j++) {
+ printf("%f ", umax[j]);
+ }
+ printf("\n\n");
+
+}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h b/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
new file mode 100644
index 0000000000..20a9919e3a
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
@@ -0,0 +1,21 @@
+
+//#include
+//#include
+//#include
+//#include
+#include
+#include
+#include
+#include
+#include
+#include "qr_solve.h"
+#include "r8lib_min.h"
+#ifndef DBL_EPSILON
+#define DBL_EPSILON 2.2204460492503131e-16
+#endif
+
+void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
+
+int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
+ int n_u, int n_w, float* u_guess, float* W_init, float* Wv,
+ float* Wu, float* ud, float gamma, int imax);
diff --git a/sw/airborne/test/Makefile b/sw/airborne/test/Makefile
index a65340b354..a92ba60897 100644
--- a/sw/airborne/test/Makefile
+++ b/sw/airborne/test/Makefile
@@ -21,8 +21,11 @@ test_algebra: test_algebra.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c
test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+test_alloc: test_alloc.c ../firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../firmwares/rotorcraft/stabilization/wls/r8lib_min.c ../firmwares/rotorcraft/stabilization/wls/qr_solve.c
+ $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
%.exe : %.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean:
- $(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe
+ $(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla test_alloc *.exe
diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c
new file mode 100644
index 0000000000..f9ebb55a75
--- /dev/null
+++ b/sw/airborne/test/test_alloc.c
@@ -0,0 +1,46 @@
+#include
+#include
+#include
+
+#include "std.h"
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_algebra_int.h"
+#include "pprz_algebra_print.h"
+#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
+
+#define INDI_OUTPUTS 4
+#define INDI_NUM_ACT 4
+
+int main(int argc, char **argv)
+{
+ float u_min[4] = {-107, -19093, 0, -95, };
+ float u_max[4] = {19093, 107, 4600, 4505, };
+
+ float g1g2[4][4] =
+ {{ 0, 0, -0.0105, 0.0107016},
+ {-0.0030044, 0.0030044, 0.035, 0.035},
+ {-0.004856, -0.004856, 0, 0},
+ { 0, 0, -0.0011, -0.0011}};
+
+ //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
+ static float Wv[INDI_OUTPUTS] = {100, 1000, 0.1, 10};
+ /*static float Wv[INDI_OUTPUTS] = {10, 10, 0.1, 1};*/
+
+ // The control objective in array format
+ float indi_v[4] = {10.8487, -10.5658, 6.8383, 1.8532};
+ float indi_du[4];
+
+ // Initialize the array of pointers to the rows of g1g2
+ float* Bwls[4];
+ uint8_t i;
+ for(i=0; i