diff --git a/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml b/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
index 5eb1f770e4..1ff6bb8f71 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
@@ -100,12 +100,30 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -123,6 +141,9 @@
+
+
+
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
index 81eb0ca0d5..6a66ba2921 100644
--- a/conf/modules/stabilization_indi.xml
+++ b/conf/modules/stabilization_indi.xml
@@ -11,12 +11,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
+
+
+
+
@@ -26,10 +39,9 @@
-
-
-
-
+
+
+
@@ -57,9 +69,6 @@
-
-
-
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index f95caaa286..9a28c09e4d 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -42,15 +42,6 @@
#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);
@@ -94,14 +85,6 @@ 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)*/
@@ -210,12 +193,6 @@ 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
deleted file mode 100644
index 5e54b6c9e5..0000000000
--- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/qr_solve.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * 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
deleted file mode 100644
index bd2ff3998b..0000000000
--- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.c
+++ /dev/null
@@ -1,554 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 72e96552be..0000000000
--- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/r8lib_min.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * 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
deleted file mode 100644
index e5d04dcb28..0000000000
--- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 20a9919e3a..0000000000
--- a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
+++ /dev/null
@@ -1,21 +0,0 @@
-
-//#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 a92ba60897..a65340b354 100644
--- a/sw/airborne/test/Makefile
+++ b/sw/airborne/test/Makefile
@@ -21,11 +21,8 @@ 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 test_alloc *.exe
+ $(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe
diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c
deleted file mode 100644
index f9ebb55a75..0000000000
--- a/sw/airborne/test/test_alloc.c
+++ /dev/null
@@ -1,46 +0,0 @@
-#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