慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
Matrix.cpp
00001 #include "myConstants.h" 00002 #include "Matrix.h" 00003 00004 00005 00006 Matrix::Matrix(int row, int col) : row(row), col(col), components(0) { 00007 components = new float[row*col]; 00008 if (!components) error("Memory Allocation Error"); 00009 for(int i=0; i<row*col; i++) components[i] = 0.0f; 00010 if (row == col) { 00011 for (int i = 0; i < row; i++) { 00012 components[i * col + i] = 1.0f; 00013 } 00014 } 00015 } 00016 00017 Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) { 00018 components = new float[row*col]; 00019 if (!components) error("Memory Allocation Error"); 00020 memcpy(components, comps, sizeof(float)*row*col); 00021 } 00022 00023 00024 Matrix::~Matrix() { 00025 delete[] components; 00026 } 00027 00028 Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) { 00029 components = new float[row*col]; 00030 if (!components) error("Memory Allocation Error"); 00031 memcpy(components, m.GetpComponents(), sizeof(float)*row*col); 00032 } 00033 00034 Matrix Matrix::operator-() const{ 00035 Matrix retMat(*this); 00036 00037 for (int i = 0; i < row * col; i++) { 00038 retMat.components[i] = - this->components[i]; 00039 } 00040 00041 return retMat; 00042 } 00043 00044 Matrix& Matrix::operator=(const Matrix& m) { 00045 if (this == &m) return *this; 00046 row = m.row; 00047 col = m.col; 00048 delete[] components; 00049 components = new float[row*col]; 00050 if (!components) error("Memory Allocation Error"); 00051 memcpy(components, m.GetpComponents(), sizeof(float)*row*col); 00052 00053 return *this; 00054 } 00055 00056 Matrix& Matrix::operator+=(const Matrix& m) { 00057 if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); 00058 00059 for (int i = 0; i < row; i++) { 00060 for (int j = 0; j < col; j++) { 00061 components[i * col + j] += m.components[i * col + j]; 00062 } 00063 } 00064 00065 this->CleanUp(); 00066 00067 return *this; 00068 } 00069 00070 Matrix& Matrix::operator-=(const Matrix& m) { 00071 if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); 00072 00073 for (int i = 0; i < row; i++) { 00074 for (int j = 0; j < col; j++) { 00075 components[i * col + j] -= m.components[i * col + j]; 00076 } 00077 } 00078 00079 this->CleanUp(); 00080 00081 return *this; 00082 } 00083 /* 00084 Matrix& Matrix::operator*=(const Matrix& m) { 00085 if (col != m.GetRow()) error("Irregular Dimention"); 00086 Matrix temp = Matrix(*this); 00087 00088 col = m.GetCol(); 00089 delete[] components; 00090 components = new float[row*col]; 00091 00092 for (int i = 0; i < row; i++) { 00093 for (int j = 0; j < col; j++) { 00094 components[i*col + j] = 0.0f; 00095 for (int k = 0; k < m.GetRow(); k++) { 00096 components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j]; 00097 } 00098 } 00099 } 00100 00101 this->CleanUp(); 00102 00103 return *this; 00104 } 00105 */ 00106 00107 Matrix& Matrix::operator*=(float c) { 00108 for (int i = 0; i < row; i++) { 00109 for (int j = 0; j < col; j++) { 00110 components[i*col + j] *= c; 00111 } 00112 } 00113 00114 return *this; 00115 } 00116 00117 Matrix& Matrix::operator/=(float c) { 00118 if (fabs(c) < NEARLY_ZERO) error("Division by Zero"); 00119 for (int i = 0; i < row; i++) { 00120 for (int j = 0; j < col; j++) { 00121 components[i*col + j] /= c; 00122 } 00123 } 00124 00125 return *this; 00126 } 00127 00128 void Matrix::SetComp(int rowNo, int colNo, float val) { 00129 if (rowNo > row || colNo > col) error("Index Out of Bounds Error"); 00130 components[(rowNo-1)*col + (colNo-1)] = val; 00131 } 00132 00133 void Matrix::SetComps(float* pComps) { 00134 memcpy(components, pComps, sizeof(float) * row * col); 00135 } 00136 00137 float Matrix::Determinant() const{ 00138 if (row != col) error("failed to calculate det. : matrix is not square"); 00139 int decSign = 0; 00140 float retVal = 1.0f; 00141 00142 // 行列のLU分解 00143 Matrix LU(this->LU_Decompose(&decSign)); 00144 00145 for (int i = 0; i < LU.row; i++) { 00146 retVal *= LU.components[i * LU.col + i]; 00147 } 00148 00149 return retVal*decSign; 00150 } 00151 00152 float Matrix::det() const { 00153 if (row != col) error("failed to calculate det : matrix is not square"); 00154 00155 Matrix temp(*this); 00156 int decSign = 1; 00157 00158 for (int j = 0; j < col - 1; j++) { 00159 00160 // 列内のみで最大の要素を探す 00161 int maxNo = j; 00162 for (int k = j; k < row; k++) { 00163 if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k; 00164 } 00165 if (maxNo != j) { 00166 temp.SwapRow(j + 1, maxNo + 1); 00167 decSign *= -1; 00168 } 00169 // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す 00170 if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { 00171 maxNo = j; 00172 for (int k = j; k < col; k++) { 00173 if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k; 00174 } 00175 if (maxNo != j) { 00176 temp.SwapCol(j + 1, maxNo + 1); 00177 decSign *= -1; 00178 } 00179 00180 // 列内、行内の最大要素を選んでも小さすぎる場合はエラー 00181 if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { 00182 if (row != col) error("failed to calculate det : Division by Zero"); 00183 } 00184 } 00185 00186 float c1 = 1.0f / temp.components[j * col + j]; 00187 00188 for (int i = j + 1; i < row; i++) { 00189 float c2 = temp.components[i * col + j] * c1; 00190 for (int k = j; k < col; k++) { 00191 temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k]; 00192 } 00193 } 00194 00195 } 00196 00197 if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f; 00198 00199 float retVal = 1.0f; 00200 for (int i = 0; i < row; i++) { 00201 retVal *= temp.components[i * col + i]; 00202 } 00203 00204 return retVal * decSign; 00205 } 00206 00207 Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{ 00208 if (row != col) error("failed to LU decomposition: matrix is not square"); 00209 if (sign != 0) *sign = 1; 00210 if (p != 0) { 00211 if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect"); 00212 // 置換行列は最初に単位行列にしておく 00213 memset(p->components, 0, sizeof(float) * row * col); 00214 for (int i = 0; i < row; i++) { 00215 p->components[i * col + i] = 1.0f; 00216 } 00217 } 00218 Matrix retVal(*this); 00219 00220 for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う 00221 // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する 00222 int maxNo = d; 00223 for (int i = d; i < row; i++) { 00224 if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i; 00225 } 00226 if (maxNo != d) { 00227 retVal.SwapRow(d + 1, maxNo + 1); 00228 if (sign != 0) *sign *= -1; 00229 if (p != 0) { 00230 p->SwapRow(d + 1, maxNo + 1); 00231 } 00232 } 00233 float c = retVal.components[d * col + d]; 00234 if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero"); 00235 00236 // d行d列目以降の行列について計算 00237 for (int i = d+1; i < row; i++) { 00238 retVal.components[i * col + d] /= c; 00239 for (int j = d+1; j < col; j++) { 00240 retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d]; 00241 } 00242 } 00243 } 00244 00245 retVal.CleanUp(); 00246 00247 return retVal; 00248 } 00249 00250 float Matrix::Inverse(Matrix& invm) const{ 00251 if (row != col) error("failed to get Inv. : matrix is not square"); 00252 00253 Matrix P(*this); 00254 Matrix LU(LU_Decompose(0, &P)); 00255 00256 // 分解した行列の対角成分の積から行列式を求める 00257 // det = 0 ならfalse 00258 float det = 1.0f; 00259 for (int i = 0; i < row; i++) { 00260 det *= LU.components[i * col + i]; 00261 } 00262 if (fabs(det) < NEARLY_ZERO) { 00263 return fabs(det); 00264 } 00265 00266 // U、Lそれぞれの逆行列を計算する 00267 Matrix U_inv = Matrix(row, col); 00268 Matrix L_inv = Matrix(row, col); 00269 00270 for (int j = 0; j < col; j++) { 00271 for (int i = 0; i <= j; i++) { 00272 int i_U = j - i; // U行列の逆行列は対角成分から上へ向かって 00273 // 左から順番に値を計算する 00274 00275 int j_L = col - 1 - j; // L行列の逆行列は右から順番に 00276 int i_L = j_L + i; // 対角成分から下へ向かって計算する 00277 00278 if (i_U != j) { // 非対角成分 00279 float temp_U = 0.0f; 00280 float temp_L = 0.0f; 00281 00282 for (int k = 0; k < i; k++) { 00283 00284 temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)]; 00285 00286 if (k == 0) { 00287 temp_L -= LU.components[i_L * col + j_L]; 00288 } else { 00289 temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k]; 00290 } 00291 00292 } 00293 00294 U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U]; 00295 L_inv.components[i_L * col + j_L] = temp_L; 00296 00297 } else { // 対角成分 00298 if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) { 00299 U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U]; 00300 } 00301 } 00302 } 00303 } 00304 00305 invm = U_inv * L_inv * P; 00306 00307 return -1.0f; 00308 } 00309 00310 Matrix Matrix::Transpose() const{ 00311 //if (row != col) error("failed to get Trans. : matrix is not square"); 00312 Matrix retVal(col, row); 00313 00314 for (int i = 0; i < row; i++) { 00315 for (int j = 0; j < col; j++) { 00316 retVal.components[j * row + i] = this->components[i * col + j]; 00317 } 00318 } 00319 00320 return retVal; 00321 } 00322 00323 Matrix operator+(const Matrix& lhm, const Matrix& rhm) { 00324 Matrix temp = Matrix(lhm); 00325 temp += rhm; 00326 return temp; 00327 } 00328 00329 Matrix operator-(const Matrix& lhm, const Matrix& rhm) { 00330 Matrix temp = Matrix(lhm); 00331 temp -= rhm; 00332 return temp; 00333 } 00334 00335 Matrix operator*(const Matrix& lhm, const Matrix& rhm) { 00336 if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention."); 00337 int row = lhm.GetRow(); 00338 int col = rhm.GetCol(); 00339 int sum = lhm.GetCol(); 00340 Matrix temp(row, col); 00341 00342 for (int i = 1; i <= row; i++) { 00343 for (int j = 1; j <= col; j++) { 00344 float temp_c = 0.0f; 00345 for (int k = 1; k <= sum; k++) { 00346 temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j); 00347 } 00348 temp.SetComp(i, j, temp_c); 00349 } 00350 } 00351 00352 return temp; 00353 } 00354 00355 void Matrix::CleanUp() { 00356 int num = row*col; 00357 float maxComp = 0.0f; 00358 for (int i = 0; i < num; i++) { 00359 if (maxComp < fabs(components[i])) maxComp = fabs(components[i]); 00360 } 00361 if (maxComp > NEARLY_ZERO) { 00362 for (int i = 0; i < num; i++) { 00363 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f; 00364 } 00365 } 00366 } 00367 00368 void Matrix::SwapRow(int rowNo1, int rowNo2) { 00369 if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!"); 00370 float* temp = new float[col]; 00371 00372 memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col); 00373 memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col); 00374 memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col); 00375 00376 delete[] temp; 00377 } 00378 00379 void Matrix::SwapCol(int colNo1, int colNo2) { 00380 if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!"); 00381 float temp = 0.0f; 00382 00383 for (int i = 0; i < row; i++) { 00384 temp = components[i * col + colNo1]; 00385 components[i * col + colNo1] = components[i * col + colNo2]; 00386 components[i * col + colNo2] = temp; 00387 } 00388 }
Generated on Wed Jul 20 2022 18:18:03 by 1.7.2