Atsumi Toda
慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 （行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します）
Diff: Matrix/Matrix.h
- Revision:
- 0:6a28eb668082
--- /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);
+