慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
Joeatsumi
Date:
Wed Jan 30 11:39:03 2019 +0000
Commit message:
Direction cosine matrix and it's calculation.

Changed in this revision

Matrix/Matrix.cpp Show annotated file Show diff for this revision Revisions of this file
Matrix/Matrix.h Show annotated file Show diff for this revision Revisions of this file
Vector/Vector.cpp Show annotated file Show diff for this revision Revisions of this file
Vector/Vector.h Show annotated file Show diff for this revision Revisions of this file
Vector/Vector_Matrix_operator.cpp Show annotated file Show diff for this revision Revisions of this file
Vector/Vector_Matrix_operator.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
myConstants.h Show annotated file Show diff for this revision Revisions of this file
--- /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
--- /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 行番号1
+             rowNo2 行番号2
+    */
+    void SwapRow(int rowNo1, int rowNo2);
+    /*
+        行列の列の入れ替えを行う
+        引数:colNo1 列番号1
+             colNo2 列番号2
+    */
+    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);
+
--- /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;
+}
--- /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);
+
--- /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
--- /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
--- /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;//1時間は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
--- /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
--- /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