• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/KalmanFilter MikeW/matmatrix.c @ 965

  → /branches/KalmanFilter MikeW/matmatrix.c @ 966

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 965 → Rev 966

/branches/KalmanFilter MikeW/matmatrix.c
1,13 → 1,378
/* matmatrix.c
The following functions are based on the Numerical Recipes.
*/
/*****************************************************************************
INCLUDES
**************************************************************************** */
#include "mat.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "matmatrix.h"
/*****************************************************************************
FUNCTIONS
*****************************************************************************/
/* ****************************************************************************
Functionname: matrix_sub */ /*!
Description: C = A - B
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_sub( f32_t **a, f32_t **b, f32_t **c, int m, int n )
{
int i,j;
f32_t *a_ptr, *b_ptr, *c_ptr;
for( j = 1; j <= m; j++)
{
a_ptr = &a[j][1];
b_ptr = &b[j][1];
c_ptr = &c[j][1];
for( i = 1; i <= n; i++)
{
*c_ptr++ = *a_ptr++ - *b_ptr++;
}
}
}
/* ****************************************************************************
Functionname: matrix_add */ /*!
Description: C = A + B
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_add( f32_t **a, f32_t **b, f32_t **c, int m, int n )
{
int i,j;
f32_t *a_ptr, *b_ptr, *c_ptr;
for( j = 1; j <= m; j++)
{
a_ptr = &a[j][1];
b_ptr = &b[j][1];
c_ptr = &c[j][1];
for( i = 1; i <= n; i++)
{
*c_ptr++ = *a_ptr++ + *b_ptr++;
}
}
}
/* ****************************************************************************
Functionname: matrix_mult */ /*!
Description: C = A x B
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_mult( f32_t **a, f32_t **b, f32_t **c,
int a_rows, int a_cols, int b_cols )
{
int i, j, k;
f32_t *a_ptr;
f32_t temp;
for( i = 1; i <= a_rows; i++)
{
a_ptr = &a[i][0];
for( j = 1; j <= b_cols; j++ )
{
temp = 0.0;
for( k = 1; k <= a_cols; k++ )
{
temp = temp + (a_ptr[k] * b[k][j]);
}
c[i][j] = temp;
}
}
}
/* ****************************************************************************
Functionname: matrix_mult_transpose */ /*!
Description: C = A x B^T
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_mult_transpose( f32_t **a, f32_t **b, f32_t **c,
int a_rows, int a_cols, int b_cols )
{
int i, j, k;
f32_t *a_ptr, *b_ptr;
f32_t temp;
for( i = 1; i <= a_rows; i++)
{
a_ptr = &a[i][0];
for( j = 1; j <= b_cols; j++ )
{
b_ptr = &b[j][1];
temp = (f32_t)0;
for( k = 1; k <= a_cols; k++ )
{
temp += a_ptr[ k ] * *b_ptr++;
}
c[i][j] = temp;
}
}
}
/* ****************************************************************************
Functionname: matrix_mult_transpose */ /*!
Description: C = A^T x B
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C,
int a_rows, int a_cols, int b_cols )
{
int i, j, k;
f32_t temp;
for( i = 1; i <= a_rows; i++)
{
for( j = 1; j <= b_cols; j++ )
{
temp = 0.0;
for( k = 1; k <= a_cols; k++ )
{
temp += A[ k ][ i ] * B[ k ][ j ];
}
C[ i ][ j ] = temp;
}
}
}
/* ****************************************************************************
Functionname: matrix_copy */ /*!
Description: B = A
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void matrix_copy( f32_t **src, f32_t **dst, int num_rows, int num_cols )
{
int i, j;
for( i = 1; i <= num_rows; i++ )
{
for( j = 1; j <= num_cols; j++ )
{
dst[ i ][ j ] = src[ i ][ j ];
}
}
}
/* ****************************************************************************
Functionname: matrix_alloc */ /*!
Description: B = A
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
f32_t **matrix(long row_low, long row_high, long col_low, long col_high)
{
long i, NumOfRows=row_high-row_low+1,NumOfCols=col_high-col_low+1;
f32_t **m;
/* allocate pointers to rows */
m=(f32_t **)malloc((size_t)((NumOfRows+1)
*sizeof(f32_t *)));
m += 1;
m -= row_low;
/* allocate rows and set pointers to them */
m[row_low]=(f32_t *)malloc((size_t)((NumOfRows*NumOfCols+1)
*sizeof(f32_t)));
m[row_low] += 1;
m[row_low] -= col_low;
for(i=row_low+1;i<=row_high;i++) m[i]=m[i-1]+NumOfCols;
{ /* init values with 0.0F */
int x, y;
for(y = 1; y <= NumOfRows; y++)
{
for(x = 1; x <= NumOfCols; x++)
{
m[y][x] = 0.F;
}
}
}
/* return pointer to array of pointers to rows */
return m;
}
/* ****************************************************************************
Functionname: free_matrix */ /*!
Description:
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void free_matrix(f32_t **m, long row_low, long row_high, long col_low, long col_high)
/* free a matrix allocated by matrix() */
{
free((char*) (m[row_low]+col_low-1));
free((char*) (m+row_low-1));
}
/* ****************************************************************************
Functionname: take_inverse */ /*!
Description: B = A^-1
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void take_inverse( f32_t **A, f32_t **B, int size)
{
gaussj( A, size, B);
matrix_copy( A, B, size, size);
}
/* gaussj()
This function performs gauss-jordan elimination to solve a set
of linear equations, at the same time generating the inverse of
the A matrix.
(C) Copr. 1986-92 Numerical Recipes Software `2$m.1.9-153.
*/
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
void gaussj(f32_t **a, int n, f32_t **b)
{
int i,icol,irow,j,k,l,ll;
f32_t big,dum,pivinv,temp;
int indxc[5];
int indxr[5];
int ipiv[5];
irow = 0;
icol = 0;
for (j=1;j<=n;j++)
{
ipiv[j]=0;
}
for (i=1;i<=n;i++)
{
big=0.0;
for (j=1;j<=n;j++)
{
if (ipiv[j] != 1)
{
for (k=1;k<=n;k++)
{
if (ipiv[k] == 0)
{
if (fabs(a[j][k]) >= big)
{
big=fabs(a[j][k]);
irow=j;
icol=k;
}
}
}
}
}
++(ipiv[icol]);
if (irow != icol)
{
for (l=1;l<=n;l++)
{
SWAP(a[irow][l],a[icol][l])
}
}
indxr[i]=irow;
indxc[i]=icol;
pivinv=1.0/a[icol][icol];
a[icol][icol]=1.0;
for (l=1;l<=n;l++)
{
a[icol][l] *= pivinv;
}
for (ll=1;ll<=n;ll++)
{
if (ll != icol)
{
dum=a[ll][icol];
a[ll][icol]=0.0;
for (l=1;l<=n;l++)
{
a[ll][l] -= a[icol][l]*dum;
}
}
}
}
for (l=n;l>=1;l--)
{
if (indxr[l] != indxc[l])
{
for (k=1;k<=n;k++)
{
SWAP(a[k][indxr[l]],a[k][indxc[l]]);
}
}
}
}
#undef SWAP