LAURUS / Mbed 2 deprecated Laurus_acc_gyro

read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

Dependencies:   mbed

Matrix.h@3:40559ebef0f1, 2015-05-13 (annotated)

Committer:
ojan
Date:
Wed May 13 04:02:27 2015 +0000
Revision:
3:40559ebef0f1
Parent:
2:4a6b46653abf 
estimate gravity vector & geomagnetic vector

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 1:2eca9b376580 1 #pragma once
ojan 1:2eca9b376580 2 #include "ErrorLogger.h"
ojan 1:2eca9b376580 3
ojan 1:2eca9b376580 4 class Matrix
ojan 1:2eca9b376580 5 {
ojan 1:2eca9b376580 6 public:
ojan 1:2eca9b376580 7 /********** コンストラクタ　デストラクタ **********/
ojan 1:2eca9b376580 8 Matrix(int row, int col);
ojan 1:2eca9b376580 9 Matrix(int row, int col, float* comps);
ojan 1:2eca9b376580 10 ~Matrix();
ojan 1:2eca9b376580 11 Matrix(const Matrix& m);
ojan 1:2eca9b376580 12
ojan 1:2eca9b376580 13 /********** メンバ演算子 **********/
ojan 2:4a6b46653abf 14 Matrix operator-() const;
ojan 1:2eca9b376580 15 Matrix& operator=(const Matrix& m);
ojan 1:2eca9b376580 16 Matrix& operator+=(const Matrix& m);
ojan 1:2eca9b376580 17 Matrix& operator-=(const Matrix& m);
ojan 1:2eca9b376580 18 Matrix& operator*=(const Matrix& m);
ojan 1:2eca9b376580 19 Matrix& operator*=(float c);
ojan 1:2eca9b376580 20 Matrix& operator/=(float c);
ojan 1:2eca9b376580 21
ojan 1:2eca9b376580 22 /********** その他関数 **********/
ojan 1:2eca9b376580 23 /*
ojan 1:2eca9b376580 24 行列の成分を設定
ojan 1:2eca9b376580 25 引数：rowNo 行番号
ojan 1:2eca9b376580 26 colNo 列番号
ojan 1:2eca9b376580 27 val 設定値
ojan 1:2eca9b376580 28 */
ojan 1:2eca9b376580 29 void SetComp(int rowNo, int colNo, float val);
ojan 1:2eca9b376580 30
ojan 1:2eca9b376580 31 /*
ojan 1:2eca9b376580 32 行列の成分を全て設定。全成分を一度に指定する必要がある。
ojan 1:2eca9b376580 33 引数：pComps 設定値の入ったfloat配列。
ojan 1:2eca9b376580 34 */
ojan 1:2eca9b376580 35 void SetComps(float* pComps);
ojan 1:2eca9b376580 36 /*
ojan 1:2eca9b376580 37 行列式を計算する。行列が正方行列で無い場合にはエラー。
ojan 1:2eca9b376580 38 */
ojan 1:2eca9b376580 39 float Determinant() const;
ojan 2:4a6b46653abf 40
ojan 2:4a6b46653abf 41 /*
ojan 2:4a6b46653abf 42 行列式を計算する。行列が正方行列で無い場合にはエラー。
ojan 2:4a6b46653abf 43 */
ojan 2:4a6b46653abf 44 float det() const;
ojan 1:2eca9b376580 45
ojan 1:2eca9b376580 46 /*
ojan 1:2eca9b376580 47 行列をLU分解する
ojan 1:2eca9b376580 48 引数：sign (省略可)置換操作の符号を格納するポインタ
ojan 1:2eca9b376580 49 p (省略可)置換行列を格納する行列のポインタ。（分解する行列と同じ列数の正方行列)
ojan 1:2eca9b376580 50 返り値：LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
ojan 1:2eca9b376580 51 */
ojan 1:2eca9b376580 52 Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
ojan 1:2eca9b376580 53
ojan 1:2eca9b376580 54 /*
ojan 1:2eca9b376580 55 逆行列を生成する
ojan 1:2eca9b376580 56 返り値で逆行列が存在するか否かを判断
ojan 1:2eca9b376580 57 引数：逆行列を格納する行列
ojan 1:2eca9b376580 58 返り値：逆行列が存在するか否か
ojan 1:2eca9b376580 59 */
ojan 1:2eca9b376580 60 bool Inverse(Matrix& invm) const;
ojan 1:2eca9b376580 61
ojan 1:2eca9b376580 62 /*
ojan 1:2eca9b376580 63 転置行列を生成する
ojan 1:2eca9b376580 64 返り値：転置行列
ojan 1:2eca9b376580 65 */
ojan 1:2eca9b376580 66 Matrix Transpose() const;
ojan 1:2eca9b376580 67
ojan 1:2eca9b376580 68 /*
ojan 1:2eca9b376580 69 行列の行の入れ替えを行う
ojan 1:2eca9b376580 70 引数：rowNo1　行番号１
ojan 1:2eca9b376580 71 rowNo2　行番号２
ojan 1:2eca9b376580 72 */
ojan 1:2eca9b376580 73 void SwapRow(int rowNo1, int rowNo2);
ojan 2:4a6b46653abf 74 /*
ojan 2:4a6b46653abf 75 行列の列の入れ替えを行う
ojan 2:4a6b46653abf 76 引数：colNo1　列番号１
ojan 2:4a6b46653abf 77 colNo2　列番号２
ojan 2:4a6b46653abf 78 */
ojan 2:4a6b46653abf 79 void SwapCol(int colNo1, int colNo2);
ojan 2:4a6b46653abf 80
ojan 1:2eca9b376580 81 /********** インライン関数 **********/
ojan 1:2eca9b376580 82 inline int GetRow() const {
ojan 1:2eca9b376580 83 return row;
ojan 1:2eca9b376580 84 }
ojan 1:2eca9b376580 85
ojan 1:2eca9b376580 86 inline int GetCol() const {
ojan 1:2eca9b376580 87 return col;
ojan 1:2eca9b376580 88 }
ojan 1:2eca9b376580 89
ojan 1:2eca9b376580 90 inline const float* GetpComponents() const {
ojan 1:2eca9b376580 91 return (const float*)components;
ojan 1:2eca9b376580 92 }
ojan 1:2eca9b376580 93
ojan 1:2eca9b376580 94 inline float GetComp(int rowNo, int colNo) const {
ojan 1:2eca9b376580 95 if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error !!");
ojan 1:2eca9b376580 96 return components[(rowNo-1)*col + (colNo-1)];
ojan 1:2eca9b376580 97 }
ojan 1:2eca9b376580 98
ojan 1:2eca9b376580 99 private:
ojan 1:2eca9b376580 100 int row;
ojan 1:2eca9b376580 101 int col;
ojan 1:2eca9b376580 102 float* components;
ojan 1:2eca9b376580 103
ojan 1:2eca9b376580 104 /*
ojan 1:2eca9b376580 105 行列の成分の中で無視できるほど小さい値を0と置き換える（掃除する）
ojan 1:2eca9b376580 106 */
ojan 1:2eca9b376580 107 void CleanUp();
ojan 1:2eca9b376580 108
ojan 1:2eca9b376580 109 };
ojan 1:2eca9b376580 110
ojan 1:2eca9b376580 111 // グローバル演算子
ojan 1:2eca9b376580 112 Matrix operator+(const Matrix& lhm, const Matrix& rhm);
ojan 1:2eca9b376580 113 Matrix operator-(const Matrix& lhm, const Matrix& rhm);
ojan 1:2eca9b376580 114 Matrix operator*(const Matrix& lhm, const Matrix& rhm);
ojan 1:2eca9b376580 115