WebSVN - FlightCtrl - Diff - Rev 966 and 968 - /branches/KalmanFilter MikeW/matmatrix.h


/*  matmatrix.c
 
The following functions are based on the Numerical Recipes.
 
*/
 
#ifndef __MATMATH_H__
#define __MATMATH_H__
 
/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "math.h"
#include "float.h"
 
/*****************************************************************************
  TYPEDEFS
*****************************************************************************/
#define f32_t float 
#define ui32_t unsigned int 
#define i32_t int 
 
#define PI 3.1415926535897932384626433832795
 
#define matSetFull(matM,Row,Column,X)               matM[Row+1][Column+1] = (X)
#define matSetDiagonal(matM,Row,Column,X)           matM[Row+1][Column+1] = (X)
#define matGetFull(matM,Row,Column,X)              *(X) = matM[Row+1][Column+1]
#define matGetDiagonal(matM,Row,Column,X)          *(X) = matM[Row+1][Column+1]
 
 
/* B = A */
extern void matrix_copy( f32_t **A, f32_t **B, int rows,
                        int cols );
 
/* C = A + B */
extern void matrix_add( f32_t **A, f32_t **B, f32_t **C, int m, int n );
 
/* C = A - B */
extern void matrix_sub( f32_t **A, f32_t **B, f32_t **C, int m, int n );
 
/*   C =  A x  B , A(a_rows x a_cols), B(a_cols x b_cols) */
extern void matrix_mult( f32_t **A, f32_t **B, f32_t **C,
                        int a_rows, int a_cols, int b_cols );
 
/*  C =  A x B^T, A(a_rows x a_cols), B(b_cols x a_cols) */
extern void matrix_mult_transpose( f32_t **A, f32_t **B, f32_t **C,
                                  int a_rows, int a_cols, int b_cols );
 
/* C = A^T x B, A(a_cols x a_rows), B(a_cols x b_cols) */
extern void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C,
                                  int a_rows, int a_cols, int b_cols );
 
/* B = A^-1 */
extern void take_inverse( f32_t **A, f32_t **B, int size);
extern void gaussj( f32_t **A, int n, f32_t **B);
 
/* Matrix allocation routines */
extern f32_t **matrix(long row_low, long row_high, long column_low, long column_high);
 
/* Deallocation routines */
extern void free_matrix(f32_t **m, long row_low, long row_high, long column_low, long column_high);
#endif
 

Rev 966	Rev 968
-		1	/* matmatrix.c
-		2
-		3	The following functions are based on the Numerical Recipes.
-		4
-		5	*/
-		6
1	#ifndef __MATMATH_H__	7	#ifndef __MATMATH_H__
2	#define __MATMATH_H__	8	#define __MATMATH_H__
3		9
4	/*****************************************************************************	10	/*****************************************************************************
5	INCLUDES	11	INCLUDES
6	**************************************************************************** */	12	**************************************************************************** */
7	#include "math.h"	13	#include "math.h"
8	#include "float.h"	14	#include "float.h"
9		15
10	/*****************************************************************************	16	/*****************************************************************************
11	TYPEDEFS	17	TYPEDEFS
12	*****************************************************************************/	18	*****************************************************************************/
13	#define f32_t float	19	#define f32_t float
14	#define ui32_t unsigned int	20	#define ui32_t unsigned int
15	#define i32_t int	21	#define i32_t int
16		22
17	#define PI 3.1415926535897932384626433832795	23	#define PI 3.1415926535897932384626433832795
18		24
19	#define matSetFull(matM,Row,Column,X) matM[Row+1][Column+1] = (X)	25	#define matSetFull(matM,Row,Column,X) matM[Row+1][Column+1] = (X)
20	#define matSetDiagonal(matM,Row,Column,X) matM[Row+1][Column+1] = (X)	26	#define matSetDiagonal(matM,Row,Column,X) matM[Row+1][Column+1] = (X)
21	#define matGetFull(matM,Row,Column,X) *(X) = matM[Row+1][Column+1]	27	#define matGetFull(matM,Row,Column,X) *(X) = matM[Row+1][Column+1]
22	#define matGetDiagonal(matM,Row,Column,X) *(X) = matM[Row+1][Column+1]	28	#define matGetDiagonal(matM,Row,Column,X) *(X) = matM[Row+1][Column+1]
23		29
24		30
25	/* B = A */	31	/* B = A */
26	extern void matrix_copy( f32_t A, f32_t B, int rows,	32	extern void matrix_copy( f32_t A, f32_t B, int rows,
27	int cols );	33	int cols );
28		34
29	/* C = A + B */	35	/* C = A + B */
30	extern void matrix_add( f32_t A, f32_t B, f32_t **C, int m, int n );	36	extern void matrix_add( f32_t A, f32_t B, f32_t **C, int m, int n );
31		37
32	/* C = A - B */	38	/* C = A - B */
33	extern void matrix_sub( f32_t A, f32_t B, f32_t **C, int m, int n );	39	extern void matrix_sub( f32_t A, f32_t B, f32_t **C, int m, int n );
34		40
35	/* C = A x B , A(a_rows x a_cols), B(a_cols x b_cols) */	41	/* C = A x B , A(a_rows x a_cols), B(a_cols x b_cols) */
36	extern void matrix_mult( f32_t A, f32_t B, f32_t **C,	42	extern void matrix_mult( f32_t A, f32_t B, f32_t **C,
37	int a_rows, int a_cols, int b_cols );	43	int a_rows, int a_cols, int b_cols );
38		44
39	/* C = A x B^T, A(a_rows x a_cols), B(b_cols x a_cols) */	45	/* C = A x B^T, A(a_rows x a_cols), B(b_cols x a_cols) */
40	extern void matrix_mult_transpose( f32_t A, f32_t B, f32_t **C,	46	extern void matrix_mult_transpose( f32_t A, f32_t B, f32_t **C,
41	int a_rows, int a_cols, int b_cols );	47	int a_rows, int a_cols, int b_cols );
42		48
43	/* C = A^T x B, A(a_cols x a_rows), B(a_cols x b_cols) */	49	/* C = A^T x B, A(a_cols x a_rows), B(a_cols x b_cols) */
44	extern void matrix_transpose_mult( f32_t A, f32_t B, f32_t **C,	50	extern void matrix_transpose_mult( f32_t A, f32_t B, f32_t **C,
45	int a_rows, int a_cols, int b_cols );	51	int a_rows, int a_cols, int b_cols );
46		52
47	/* B = A^-1 */	53	/* B = A^-1 */
48	extern void take_inverse( f32_t A, f32_t B, int size);	54	extern void take_inverse( f32_t A, f32_t B, int size);
49	extern void gaussj( f32_t A, int n, f32_t B);	55	extern void gaussj( f32_t A, int n, f32_t B);
50		56
51	/* Matrix allocation routines */	57	/* Matrix allocation routines */
52	extern f32_t **matrix(long row_low, long row_high, long column_low, long column_high);	58	extern f32_t **matrix(long row_low, long row_high, long column_low, long column_high);
53		59
54	/* Deallocation routines */	60	/* Deallocation routines */
55	extern void free_matrix(f32_t **m, long row_low, long row_high, long column_low, long column_high);	61	extern void free_matrix(f32_t **m, long row_low, long row_high, long column_low, long column_high);
56	#endif	62	#endif
57		63

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/matmatrix.h @ 1040 - Rev 966 → 968