Atsumi Toda / Mbed OS UAV_Logger4

UAV_Logger1から改造。サーボ機構のプログラムを追加

Dependencies:   MPU6050_alter HMC5883L

Matrix/Matrix.h@2:e6496a794bde, 2019-05-24 (annotated)

Committer:
Joeatsumi
Date:
Fri May 24 05:57:12 2019 +0000
Revision:
2:e6496a794bde
mpu6050 and GHC5883L for atitude estimation for UAV.

Who changed what in which revision?

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