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

Dependencies:   mbed

Matrix/Matrix.h

Committer:
Joeatsumi
Date:
2019-01-30
Revision:
0:6a28eb668082

File content as of revision 0:6a28eb668082:

#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);