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