diff options
Diffstat (limited to 'apps/px4/attitude_estimator_bm/matrix.h')
-rw-r--r-- | apps/px4/attitude_estimator_bm/matrix.h | 156 |
1 files changed, 156 insertions, 0 deletions
diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h new file mode 100644 index 000000000..613a2d081 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/matrix.h @@ -0,0 +1,156 @@ +/* + * matrix.h + * + * Created on: 18.11.2010 + * Author: Laurens Mackay + */ + +#ifndef MATRIX_H_ +#define MATRIX_H_ + +typedef float m_elem; + +typedef struct { + int rows; + int cols; + m_elem *a; +} matrix_t; + +typedef struct { + float x; + float y; + float z; +} float_vect3; + +#define M(m,i,j) m.a[m.cols*i+j] + +///* This is the datatype used for the math and non-type specific ops. */ +// +//matrix_t matrix_create(const int rows, const int cols, m_elem * a); +///* matrix C = matrix A + matrix B , both of size m x n */ +//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A - matrix B , all of size m x n */ +//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ +//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); +// +//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); +// +//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = A*B'*/ +//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); + + +static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) +{ + matrix_t ret; + ret.rows = rows; + ret.cols = cols; + ret.a = a; + return ret; +} + +static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_add: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) + M(b, i, j); + } + + } +} + +static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_sub: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) - M(b, i, j); + } + + } +} + +static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, k, j); + } + } + + } +} + +static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) +{ + + if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, j, k); + } + } + + } + +} + +static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols) { + //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = f * M(a, i, j); + } + + } +} + + +static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_mult_element: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) * M(b, i, j); + } + + } +} + + + +#endif /* MATRIX_H_ */ |