Revision 0:6a28eb668082, committed 2019-01-30

Comitter:

Joeatsumi

Date:

Wed Jan 30 11:39:03 2019 +0000

Commit message:

Direction cosine matrix and it's calculation.

Changed in this revision

diff -r 000000000000 -r 6a28eb668082 Matrix/Matrix.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.cpp	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,388 @@
+#include "myConstants.h"
+#include "Matrix.h"
+
+
+
+Matrix::Matrix(int row, int col) : row(row), col(col), components(0) {
+    components = new float[row*col];
+    if (!components) error("Memory Allocation Error");
+    for(int i=0; i<row*col; i++) components[i] = 0.0f;
+    if (row == col) {
+        for (int i = 0; i < row; i++) {
+            components[i * col + i] = 1.0f;
+        }
+    }
+}
+
+Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) {
+    components = new float[row*col];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, comps, sizeof(float)*row*col);
+}
+
+
+Matrix::~Matrix() {
+    delete[] components;
+}
+
+Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) {
+    components = new float[row*col];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
+}
+
+Matrix Matrix::operator-() const{
+    Matrix retMat(*this);
+
+    for (int i = 0; i < row * col; i++) {
+        retMat.components[i] = - this->components[i];
+    }
+
+    return retMat;
+}
+
+Matrix& Matrix::operator=(const Matrix& m) {
+    if (this == &m) return *this;
+    row = m.row;
+    col = m.col;
+    delete[] components;
+    components = new float[row*col];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
+
+    return *this;
+}
+
+Matrix& Matrix::operator+=(const Matrix& m) {
+    if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
+    
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i * col + j] += m.components[i * col + j];
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Matrix& Matrix::operator-=(const Matrix& m) {
+    if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
+
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i * col + j] -= m.components[i * col + j];
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+/*
+Matrix& Matrix::operator*=(const Matrix& m) {
+    if (col != m.GetRow()) error("Irregular Dimention");
+    Matrix temp = Matrix(*this);
+    
+    col = m.GetCol();
+    delete[] components;
+    components = new float[row*col];
+
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] = 0.0f;
+            for (int k = 0; k < m.GetRow(); k++) {
+                components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j];
+            }
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+*/
+
+Matrix& Matrix::operator*=(float c) {
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] *= c;
+        }
+    }
+
+    return *this;
+}
+
+Matrix& Matrix::operator/=(float c) {
+    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] /= c;
+        }
+    }
+
+    return *this;
+}
+
+void Matrix::SetComp(int rowNo, int colNo, float val) {
+    if (rowNo > row || colNo > col) error("Index Out of Bounds Error");
+    components[(rowNo-1)*col + (colNo-1)] = val;
+}
+
+void Matrix::SetComps(float* pComps) {
+    memcpy(components, pComps, sizeof(float) * row * col);
+}
+
+float Matrix::Determinant() const{
+    if (row != col) error("failed to calculate det. : matrix is not square");
+    int decSign = 0;
+    float retVal = 1.0f;
+
+    // 行列のLU分解
+    Matrix LU(this->LU_Decompose(&decSign));
+
+    for (int i = 0; i < LU.row; i++) {
+        retVal *= LU.components[i * LU.col + i];
+    }
+
+    return retVal*decSign;
+}
+
+float Matrix::det() const {
+    if (row != col) error("failed to calculate det : matrix is not square");
+    
+    Matrix temp(*this);
+    int decSign = 1;
+
+    for (int j = 0; j < col - 1; j++) {
+
+        // 列内のみで最大の要素を探す
+        int maxNo = j;
+        for (int k = j; k < row; k++) {
+            if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k;
+        }
+        if (maxNo != j) {
+            temp.SwapRow(j + 1, maxNo + 1);
+            decSign *= -1;
+        }
+        // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す
+        if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
+            maxNo = j;
+            for (int k = j; k < col; k++) {
+                if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k;
+            }
+            if (maxNo != j) {
+                temp.SwapCol(j + 1, maxNo + 1);
+                decSign *= -1;
+            }
+
+            // 列内、行内の最大要素を選んでも小さすぎる場合はエラー
+            if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
+                if (row != col) error("failed to calculate det : Division by Zero");
+            }
+        }
+
+        float c1 = 1.0f / temp.components[j * col + j];
+
+        for (int i = j + 1; i < row; i++) {
+            float c2 = temp.components[i * col + j] * c1;
+            for (int k = j; k < col; k++) {
+                temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k];
+            }
+        }
+        
+    }
+
+    if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f;
+
+    float retVal = 1.0f;
+    for (int i = 0; i < row; i++) {
+        retVal *= temp.components[i * col + i];
+    }
+
+    return retVal * decSign;
+}
+
+Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{
+    if (row != col) error("failed to LU decomposition: matrix is not square");
+    if (sign != 0) *sign = 1;
+    if (p != 0) {
+        if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect");
+        // 置換行列は最初に単位行列にしておく
+        memset(p->components, 0, sizeof(float) * row * col);
+        for (int i = 0; i < row; i++) {
+            p->components[i * col + i] = 1.0f;
+        }
+    }
+    Matrix retVal(*this);
+
+    for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う
+        // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する
+        int maxNo = d;
+        for (int i = d; i < row; i++) {
+            if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i;
+        }
+        if (maxNo != d) {
+            retVal.SwapRow(d + 1, maxNo + 1);
+            if (sign != 0) *sign *= -1;
+            if (p != 0) {
+                p->SwapRow(d + 1, maxNo + 1);
+            }
+        }
+        float c = retVal.components[d * col + d];
+        if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero");
+
+        // d行d列目以降の行列について計算
+        for (int i = d+1; i < row; i++) {
+            retVal.components[i * col + d] /= c;
+            for (int j = d+1; j < col; j++) {
+                retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d];
+            }
+        }
+    }
+
+    retVal.CleanUp();
+
+    return retVal;
+}
+
+float Matrix::Inverse(Matrix& invm) const{
+    if (row != col) error("failed to get Inv. : matrix is not square");
+
+    Matrix P(*this);
+    Matrix LU(LU_Decompose(0, &P));
+
+    // 分解した行列の対角成分の積から行列式を求める
+    // det = 0　ならfalse
+    float det = 1.0f;
+    for (int i = 0; i < row; i++) {
+        det *= LU.components[i * col + i];
+    }
+    if (fabs(det) < NEARLY_ZERO) {
+        return fabs(det);
+    }
+
+    // U、Lそれぞれの逆行列を計算する
+    Matrix U_inv = Matrix(row, col);
+    Matrix L_inv = Matrix(row, col);
+
+    for (int j = 0; j < col; j++) {
+        for (int i = 0; i <= j; i++) {
+            int i_U = j - i;        // U行列の逆行列は対角成分から上へ向かって
+                                    // 左から順番に値を計算する
+
+            int j_L = col - 1 - j;  // L行列の逆行列は右から順番に
+            int i_L = j_L + i;      // 対角成分から下へ向かって計算する
+
+            if (i_U != j) { // 非対角成分
+                float temp_U = 0.0f;
+                float temp_L = 0.0f;
+
+                for (int k = 0; k < i; k++) {
+
+                    temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)];
+                    
+                    if (k == 0) {
+                        temp_L -= LU.components[i_L * col + j_L];
+                    } else {
+                        temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k];
+                    }
+                    
+                }
+
+                U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U];
+                L_inv.components[i_L * col + j_L] = temp_L;
+
+            } else {    // 対角成分
+                if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) {
+                    U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U];
+                }
+            }
+        }
+    }
+
+    invm = U_inv * L_inv * P;
+
+    return -1.0f;
+}
+
+Matrix Matrix::Transpose() const{
+    //if (row != col) error("failed to get Trans. : matrix is not square");
+    Matrix retVal(col, row);
+
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            retVal.components[j * row + i] = this->components[i * col + j];        
+        }
+    }
+
+    return retVal;
+}
+
+Matrix operator+(const Matrix& lhm, const Matrix& rhm) {
+    Matrix temp = Matrix(lhm);
+    temp += rhm;
+    return temp;
+}
+
+Matrix operator-(const Matrix& lhm, const Matrix& rhm) {
+    Matrix temp = Matrix(lhm);
+    temp -= rhm;
+    return temp;
+}
+
+Matrix operator*(const Matrix& lhm, const Matrix& rhm) {
+    if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention.");
+    int row = lhm.GetRow();
+    int col = rhm.GetCol();
+    int sum = lhm.GetCol();
+    Matrix temp(row, col);
+    
+    for (int i = 1; i <= row; i++) {
+        for (int j = 1; j <= col; j++) {
+            float temp_c = 0.0f;
+            for (int k = 1; k <= sum; k++) {
+                temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j);
+            }
+            temp.SetComp(i, j, temp_c);
+        }
+    }
+    
+    return temp;
+}
+
+void Matrix::CleanUp() {
+    int num = row*col;
+    float maxComp = 0.0f;
+    for (int i = 0; i < num; i++) {
+        if (maxComp < fabs(components[i])) maxComp = fabs(components[i]);
+    }
+    if (maxComp > NEARLY_ZERO) {
+        for (int i = 0; i < num; i++) {
+            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
+        }
+    }
+}
+
+void Matrix::SwapRow(int rowNo1, int rowNo2) {
+    if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!");
+    float* temp = new float[col];
+
+    memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col);
+    memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col);
+    memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col);
+
+    delete[] temp;
+}
+
+void Matrix::SwapCol(int colNo1, int colNo2) {
+    if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!");
+    float temp = 0.0f;
+
+    for (int i = 0; i < row; i++) {
+        temp = components[i * col + colNo1];
+        components[i * col + colNo1] = components[i * col + colNo2];
+        components[i * col + colNo2] = temp;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 6a28eb668082 Matrix/Matrix.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.h	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,115 @@
+#pragma once
+#include "mbed.h"
+
+class Matrix
+{
+public:
+    /********** コンストラクタ　デストラクタ **********/
+    Matrix(int row, int col);
+    Matrix(int row, int col, float* comps);
+    ~Matrix();
+    Matrix(const Matrix& m);
+
+    /********** メンバ演算子 **********/
+    Matrix operator-() const;
+    Matrix& operator=(const Matrix& m);
+    Matrix& operator+=(const Matrix& m);
+    Matrix& operator-=(const Matrix& m);
+    //Matrix& operator*=(const Matrix& m);
+    Matrix& operator*=(float c);
+    Matrix& operator/=(float c);
+
+    /********** その他関数 **********/
+    /*
+        行列の成分を設定
+        引数：rowNo 行番号
+              colNo 列番号
+              val 設定値
+    */
+    void SetComp(int rowNo, int colNo, float val);
+
+    /*
+        行列の成分を全て設定。全成分を一度に指定する必要がある。
+        引数：pComps 設定値の入ったfloat配列。
+    */
+    void SetComps(float* pComps);
+    /*
+        行列式を計算する。行列が正方行列で無い場合にはエラー。
+    */
+    float Determinant() const;
+    
+    /*
+        行列式を計算する。行列が正方行列で無い場合にはエラー。
+    */
+    float det() const;
+
+    /*
+        行列をLU分解する
+        引数：sign (省略可)置換操作の符号を格納するポインタ
+             p (省略可)置換行列を格納する行列のポインタ。（分解する行列と同じ列数の正方行列)
+        返り値：LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
+    */
+    Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
+    
+    /*
+        逆行列を生成する
+        返り値で逆行列が存在するか否かを判断
+        引数：逆行列を格納する行列
+        返り値：逆行列が存在するか否か
+    */
+    float Inverse(Matrix& invm) const;
+    
+    /*
+        転置行列を生成する
+        返り値：転置行列
+    */
+    Matrix Transpose() const;
+    
+    /*
+        行列の行の入れ替えを行う
+        引数：rowNo1　行番号１
+             rowNo2　行番号２
+    */
+    void SwapRow(int rowNo1, int rowNo2);
+    /*
+        行列の列の入れ替えを行う
+        引数：colNo1　列番号１
+             colNo2　列番号２
+    */
+    void SwapCol(int colNo1, int colNo2);
+    
+    /********** インライン関数 **********/
+    inline int GetRow() const {
+        return row;
+    }
+
+    inline int GetCol() const {
+        return col;
+    }
+
+    inline const float* GetpComponents() const {
+        return (const float*)components;
+    }
+
+    inline float GetComp(int rowNo, int colNo) const {
+        if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!");
+        return components[(rowNo-1)*col + (colNo-1)];
+    }
+
+private:
+    int row;
+    int col;
+    float* components;
+
+    /*
+        行列の成分の中で無視できるほど小さい値を0と置き換える（掃除する）
+    */
+    void CleanUp();
+
+};
+
+// グローバル演算子
+Matrix operator+(const Matrix& lhm, const Matrix& rhm);
+Matrix operator-(const Matrix& lhm, const Matrix& rhm);
+Matrix operator*(const Matrix& lhm, const Matrix& rhm);
+

diff -r 000000000000 -r 6a28eb668082 Vector/Vector.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector.cpp	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,177 @@
+#include "myConstants.h"
+#include "Vector.h"
+
+
+Vector::Vector(int dim) : dim(dim), components(0){
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    for(int i=0; i<dim; i++) components[i] = 0.0f;
+}
+
+
+Vector::~Vector() {
+    delete[] components;
+}
+
+Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+}
+
+Vector& Vector::operator=(const Vector& v) {
+    if (this == &v) return *this;
+    dim = v.dim;
+    delete[] components;
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+
+    return *this;
+}
+
+Vector Vector::operator+() {
+    return *this;
+}
+
+Vector Vector::operator-() {
+    Vector retVec(*this);
+    retVec *= -1;
+    return retVec;
+}
+
+Vector& Vector::operator*=(float c) {
+    for (int i = 0; i < dim; i++) {
+        components[i] *= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator/=(float c) {
+    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
+    for (int i = 0; i < dim; i++) {
+        components[i] /= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator+=(const Vector& v) {
+    if (dim != v.dim) error("failed to add: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] += v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Vector& Vector::operator-=(const Vector& v) {
+    if (dim != v.dim) error("failed to subtract: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] -= v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+void Vector::SetComp(int dimNo, float val) {
+    if (dimNo > dim) error("Index Out of Bounds Error");
+    components[dimNo-1] = val;
+}
+
+void Vector::SetComps(float* pComps) {
+    memcpy(components, pComps, sizeof(float) * dim);
+}
+
+float Vector::GetNorm() const {
+    float norm = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        norm += components[i] * components[i];
+    }
+    return sqrt(norm);
+}
+
+Vector Vector::Normalize() const {
+    float norm = GetNorm();
+    Vector temp(*this);
+    for (int i = 0; i < dim; i++) {
+        temp.components[i] /= norm;
+    }
+    temp.CleanUp();
+    return temp;
+}
+
+Vector Vector::GetParaCompTo(Vector v) {
+    Vector norm_v = v.Normalize();
+    return (*this * norm_v) * norm_v;
+}
+
+Vector Vector::GetPerpCompTo(Vector v) {
+    return (*this - this->GetParaCompTo(v));
+}
+
+void Vector::CleanUp() {
+    float maxComp = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
+    }
+    if (maxComp > NEARLY_ZERO) {
+        for (int i = 0; i < dim; i++) {
+            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
+        }
+    }
+}
+
+Vector operator+(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec += rhv;
+    return retVec;
+}
+
+Vector operator-(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec -= rhv;
+    return retVec;
+}
+
+Vector Cross(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
+    if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");
+
+    Vector retVec(lhv.GetDim());
+
+    for (int i = 0; i < lhv.GetDim(); i++) {
+        retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
+            - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
+    }
+
+    return retVec;
+}
+
+Vector operator*(const float c, const Vector& rhv) {
+    Vector retVec(rhv);
+    retVec *= c;
+    return retVec;
+}
+
+Vector operator*(const Vector& lhv, const float c) {
+    Vector retVec(lhv);
+    retVec *= c;
+    return retVec;
+}
+
+float operator*(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
+    float retVal = 0.0f;
+
+    for (int i = 1; i <= lhv.GetDim(); i++) {
+        retVal += lhv.GetComp(i) * rhv.GetComp(i);
+    }
+
+    return retVal;
+}

diff -r 000000000000 -r 6a28eb668082 Vector/Vector.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector.h	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,56 @@
+#pragma once
+#include "mbed.h"
+
+class Matrix;
+
+class Vector {
+public:
+    Vector(int dim);
+    ~Vector();
+    Vector(const Vector& v);
+
+    Vector& operator=(const Vector& v);
+    Vector operator+();
+    Vector operator-();
+    Vector& operator*=(float c);
+    Vector& operator/=(float c);
+    Vector& operator+=(const Vector& v);
+    Vector& operator-=(const Vector& v);
+
+    void SetComp(int dimNo, float val);
+    void SetComps(float* vals);
+    float GetNorm() const;
+    Vector Normalize() const;
+    Vector GetParaCompTo(Vector v);
+    Vector GetPerpCompTo(Vector v);
+
+    inline int GetDim() const {
+        return dim;
+    }
+
+    inline const float* GetpComponents() const {
+        return (const float*)components;
+    }
+
+    inline float GetComp(int dimNo) const {
+        if (dimNo > dim) error("Index Out of Bounds Error !!");
+        return components[dimNo-1];
+    }
+
+    void CleanUp();
+
+private:
+    int dim;
+    float* components;
+
+    Vector& operator*=(const Matrix& m);
+    Vector& operator*=(const Vector& m);
+};
+
+Vector operator+(const Vector& lhv, const Vector& rhv);
+Vector operator-(const Vector& lhv, const Vector& rhv);
+Vector Cross(const Vector& lhv, const Vector& rhv);
+Vector operator*(const float c, const Vector& rhv);
+Vector operator*(const Vector& lhv, const float c);
+float operator*(const Vector& lhv, const Vector& rhv);
+

diff -r 000000000000 -r 6a28eb668082 Vector/Vector_Matrix_operator.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector_Matrix_operator.cpp	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,35 @@
+#include "Vector_Matrix_operator.h"
+
+Vector operator*(const Matrix& lhm, const Vector& rhv) {
+    if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention");
+    Vector retVec(lhm.GetRow());
+
+    for (int i = 1; i <= lhm.GetRow(); i++) {
+        float temp = 0.0f;
+        for (int j = 1; j <= rhv.GetDim(); j++) {
+            temp += lhm.GetComp(i, j)*rhv.GetComp(j);
+        }
+        retVec.SetComp(i, temp);
+    }
+
+    retVec.CleanUp();
+
+    return retVec;
+}
+
+Vector operator*(const Vector& lhv, const Matrix& rhm) {
+    if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention");
+    Vector retVec(rhm.GetCol());
+
+    for (int i = 1; i <= rhm.GetCol(); i++) {
+        float temp = 0.0f;
+        for (int j = 1; j <= lhv.GetDim(); j++) {
+            temp += lhv.GetComp(j) * rhm.GetComp(j, i);
+        }
+        retVec.SetComp(i, temp);
+    }
+
+    retVec.CleanUp();
+
+    return retVec;
+}
\ No newline at end of file

diff -r 000000000000 -r 6a28eb668082 Vector/Vector_Matrix_operator.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector_Matrix_operator.h	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,5 @@
+#include "Matrix.h"
+#include "Vector.h"
+
+Vector operator*(const Matrix& lhm, const Vector& rhv);
+Vector operator*(const Vector& lhv, const Matrix& rhm);
\ No newline at end of file

diff -r 000000000000 -r 6a28eb668082 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "Vector.h"
+#include "Matrix.h"
+#include "Vector_Matrix_operator.h"
+
+/*---------------------------定数---------------------------*/
+#define PI 3.141592
+#define OMEGA 7.2921159/100000//地球の自転角速度 rad/s
+/*---------------------------------------------------------*/
+DigitalOut myled(LED1);
+
+int second;//軌道からの経過時間[s]
+double theta;//ECIとECEFのずれ[rad]
+
+double lon,lat;              // lon 経度 lat 緯度 [rad]
+double lon_degree,lat_degree;// lon_degree 経度 lat_degree 緯度 [degree]
+double el,roll,az;           //ピッチ,ロール,アジマス（rad）
+double pitch_degree,roll_degree,azimaith_degree;
+                             //ピッチ,ロール,アジマス（10進数表記）
+
+Serial pc(USBTX, USBRX);     // Teratermとの接続 tx, rx
+
+/*--------------------------行列、ベクトル-----------------------------*/
+Matrix Q1(3, 3), Q2(3, 3),Q3(3,3);                        //検算の行列
+Matrix ECI_to_ECEF(3,3),ECEF_to_NED(3,3),NED_to_BODY(3,3);//変換行列
+Vector ECI(3),ECEF(3), NED(3),BODY(3);                    //座標系
+/*-------------------------------------------------------------------*/
+
+/*--------------------プロトタイプ宣言----------------------*/
+void    toString(Matrix& m);     // 行列デバッグ用
+void    toString_V(Vector& v);   // ベクトルデバッグ用
+double deg_to_rad(double);       // 度数単位からrad単位への変換
+/*--------------------------------------------------------*/
+
+/* ------------------------------ デバッグ用関数 ------------------------------ */
+
+void toString(Matrix& m)
+{
+
+    for(int i=0; i<m.GetRow(); i++) {
+        for(int j=0; j<m.GetCol(); j++) {
+            pc.printf("%.6f\t", m.GetComp(i+1, j+1));
+        }
+        pc.printf("\r\n");
+    }
+
+}
+
+void toString_V(Vector& v)
+{
+
+    for(int i=0; i<v.GetDim(); i++) {
+        pc.printf("%.6f\t", v.GetComp(i+1));
+    }
+    pc.printf("\r\n");
+
+}
+/*-------------------------------------------------------------------------*/
+/*---------------------度数単位からラジアン単位に変換する------------*/
+double deg_to_rad(double degree)
+{   double rad;
+    rad=degree*PI/180;
+    return rad;     
+    }
+/*---------------------------------------------------------------*/
+
+int main() {
+
+ /* ----------------Q1の要素値を設定----------------------*/
+        float q1[9] = {
+            1.0f, 2.0f, 3.0f,  
+            4.0f, 5.0f, 6.0f, 
+            7.0f, 8.0f, 9.0f
+        };
+            
+         Q1.SetComps(q1);
+/*-----------------------------------------------------*/
+/* ----------------Q2の要素値を設定----------------------*/
+        float q2[9] = {
+            1.0f, 2.0f, 3.0f,  
+            4.0f, 5.0f, 6.0f, 
+            7.0f, 8.0f, 9.0f
+        };
+            
+         Q2.SetComps(q2);
+/*-----------------------------------------------------*/
+/* ----------------ECIの要素値を設定----------------------*/
+        float eci[3] = {
+            4.0f,   
+            1.0f, 
+            0.0f
+        };
+            
+         ECI.SetComps(eci);
+/*-----------------------------------------------------*/
+
+/*=================ECI座標系からECEF座標系への変換行列=====================*/
+
+second=3600*6;//１時間は3600秒
+theta=OMEGA*second;
+float eci_to_ecef[9]={
+            cos(theta),sin(theta),0.0,
+            -sin(theta),cos(theta),0.0,
+            0.0,       0.0        ,0.0    
+    }; 
+    
+        ECI_to_ECEF.SetComps(eci_to_ecef);
+/*====================================================================*/
+
+/*=================ECEF座標系からNED座標系への変換行列=====================*/    
+
+lon_degree=0.0;  //経度
+lat_degree=0.0;  //緯度
+lon=deg_to_rad(lon_degree);
+lat=deg_to_rad(lat_degree);
+
+float ecef_to_ned[9]={
+            -sin(lat)*cos(lon),-sin(lat)*sin(lon)    ,cos(lat),
+               -sin(lon)             ,cos(lon)     ,0.0,
+            -cos(lat)*cos(lon)    ,  -cos(lat)*sin(lon) ,-sin(lat)
+    }; 
+    
+        ECEF_to_NED.SetComps(ecef_to_ned);
+/*====================================================================*/
+
+/*=================NED座標系から機体座標系への変換行列=====================*/  
+
+pitch_degree=0;//ピッチ（10進数表記）
+roll_degree=0;//ロール（10進数表記）
+azimaith_degree=0;//アジマス（10進数表記）
+
+el=deg_to_rad(pitch_degree);//ピッチ（rad）
+roll=deg_to_rad(roll_degree);//ロール（rad）
+az=deg_to_rad(azimaith_degree);//アジマス（rad）
+
+float ned_to_body[9]={-cos(el)*cos(az) , cos(el)*sin(az)    ,-sin(el),
+                      
+                      cos(roll)*sin(az)+sin(roll)*sin(el)*cos(az),
+                      cos(az)*cos(roll)+sin(roll)*sin(el)*sin(az),                  
+                      sin(roll)*cos(el),
+                      
+                      -sin(roll)*sin(az)+cos(roll)*sin(el)*cos(az),
+                      -sin(roll)*cos(az)+cos(roll)*sin(el)*sin(az),                  
+                        cos(roll)*cos(el)
+        };
+        
+        NED_to_BODY.SetComps(ned_to_body);    
+/*--------------------------------------------------------------------*/
+/*-------------------計算を行う-----------------------*/
+        Q3 = Q1 * Q2;    
+        ECEF = ECI_to_ECEF*ECI;
+        NED  = ECEF_to_NED*ECEF;
+        BODY = NED_to_BODY*NED;
+/*-----------------------------------------------------*/
+/*------------Tera Termで計算結果を表示する--------------------*/
+    toString(Q3);
+    toString_V(ECEF);
+    toString_V(NED);
+    toString_V(BODY);
+    //pc.printf("%.3f\r\n",sin(deg_to_rad(30)));
+/*-----------------------------------------------------*/
+}
\ No newline at end of file

diff -r 000000000000 -r 6a28eb668082 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file

diff -r 000000000000 -r 6a28eb668082 myConstants.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/myConstants.h	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,37 @@
+#pragma once
+
+/* Math Constants */
+#define NEARLY_ZERO         0.000000001f
+#define ZERO_TOLERANCE      0.000001f
+#define RAD_TO_DEG          57.2957795f             // 180 / π
+#define DEG_TO_RAD          0.0174532925f           // π / 180
+
+/* Accelerometer */
+#define ACC_LSB_TO_G        0.0000610351562f        // g/LSB (1/2^14
+#define G_TO_MPSS           9.8f                    // (m/s^2)/g
+
+/* Gyro Sensor */
+//#define GYRO_LSB_TO_DEG     0.0304878048f           // deg/LSB (1/32.8
+#define GYRO_LSB_TO_DEG     0.0152671755f           // deg/LSB (1/65.5
+//#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB (1/131
+
+/* Pressure Sensor */
+#define PRES_LSB_TO_HPA     0.000244140625f         // hPa/LSB (1/4096
+
+inline float TempLsbToDeg(short int temp) {
+    return (42.5f + (float)temp * 0.00208333333f);  // degree_C = 42.5 + temp / 480;
+}
+
+/* GPS */
+#define GPS_SQ_E                0.00669437999f      // （第一離心率）^2
+#define GPS_A                   6378137.0f          // 長半径(赤道半径)(m)
+#define GPS_B                   6356752.3f          // 短半径(極半径)(m)
+
+/* Geomagnetic Sensor */
+#define MAG_LSB_TO_GAUSS    0.00092f                // Gauss/LSB
+#define MAG_MAGNITUDE       0.46f                   // Magnitude of GeoMagnetism (Gauss)
+#define MAG_SIN             -0.754709580f           // Sin-Value of Inclination
+#define MAG_DECLINATION     7.5f                    // declination (deg)
+
+/* ADC */
+#define ADC_LSB_TO_V        0.000050354f            // 3.3(V)/65535(LSB)
\ No newline at end of file