慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
Matrix.h
00001 #pragma once 00002 #include "mbed.h" 00003 00004 class Matrix 00005 { 00006 public: 00007 /********** コンストラクタ デストラクタ **********/ 00008 Matrix(int row, int col); 00009 Matrix(int row, int col, float* comps); 00010 ~Matrix(); 00011 Matrix(const Matrix& m); 00012 00013 /********** メンバ演算子 **********/ 00014 Matrix operator-() const; 00015 Matrix& operator=(const Matrix& m); 00016 Matrix& operator+=(const Matrix& m); 00017 Matrix& operator-=(const Matrix& m); 00018 //Matrix& operator*=(const Matrix& m); 00019 Matrix& operator*=(float c); 00020 Matrix& operator/=(float c); 00021 00022 /********** その他関数 **********/ 00023 /* 00024 行列の成分を設定 00025 引数:rowNo 行番号 00026 colNo 列番号 00027 val 設定値 00028 */ 00029 void SetComp(int rowNo, int colNo, float val); 00030 00031 /* 00032 行列の成分を全て設定。全成分を一度に指定する必要がある。 00033 引数:pComps 設定値の入ったfloat配列。 00034 */ 00035 void SetComps(float* pComps); 00036 /* 00037 行列式を計算する。行列が正方行列で無い場合にはエラー。 00038 */ 00039 float Determinant() const; 00040 00041 /* 00042 行列式を計算する。行列が正方行列で無い場合にはエラー。 00043 */ 00044 float det() const; 00045 00046 /* 00047 行列をLU分解する 00048 引数:sign (省略可)置換操作の符号を格納するポインタ 00049 p (省略可)置換行列を格納する行列のポインタ。(分解する行列と同じ列数の正方行列) 00050 返り値:LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。 00051 */ 00052 Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const; 00053 00054 /* 00055 逆行列を生成する 00056 返り値で逆行列が存在するか否かを判断 00057 引数:逆行列を格納する行列 00058 返り値:逆行列が存在するか否か 00059 */ 00060 float Inverse(Matrix& invm) const; 00061 00062 /* 00063 転置行列を生成する 00064 返り値:転置行列 00065 */ 00066 Matrix Transpose() const; 00067 00068 /* 00069 行列の行の入れ替えを行う 00070 引数:rowNo1 行番号1 00071 rowNo2 行番号2 00072 */ 00073 void SwapRow(int rowNo1, int rowNo2); 00074 /* 00075 行列の列の入れ替えを行う 00076 引数:colNo1 列番号1 00077 colNo2 列番号2 00078 */ 00079 void SwapCol(int colNo1, int colNo2); 00080 00081 /********** インライン関数 **********/ 00082 inline int GetRow() const { 00083 return row; 00084 } 00085 00086 inline int GetCol() const { 00087 return col; 00088 } 00089 00090 inline const float* GetpComponents() const { 00091 return (const float*)components; 00092 } 00093 00094 inline float GetComp(int rowNo, int colNo) const { 00095 if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!"); 00096 return components[(rowNo-1)*col + (colNo-1)]; 00097 } 00098 00099 private: 00100 int row; 00101 int col; 00102 float* components; 00103 00104 /* 00105 行列の成分の中で無視できるほど小さい値を0と置き換える(掃除する) 00106 */ 00107 void CleanUp(); 00108 00109 }; 00110 00111 // グローバル演算子 00112 Matrix operator+(const Matrix& lhm, const Matrix& rhm); 00113 Matrix operator-(const Matrix& lhm, const Matrix& rhm); 00114 Matrix operator*(const Matrix& lhm, const Matrix& rhm); 00115
Generated on Wed Jul 20 2022 18:18:03 by 1.7.2