慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
Diff: Vector/Vector_Matrix_operator.cpp
- Revision:
- 0:6a28eb668082
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Vector/Vector_Matrix_operator.cpp Wed Jan 30 11:39:03 2019 +0000 @@ -0,0 +1,35 @@ +#include "Vector_Matrix_operator.h" + +Vector operator*(const Matrix& lhm, const Vector& rhv) { + if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention"); + Vector retVec(lhm.GetRow()); + + for (int i = 1; i <= lhm.GetRow(); i++) { + float temp = 0.0f; + for (int j = 1; j <= rhv.GetDim(); j++) { + temp += lhm.GetComp(i, j)*rhv.GetComp(j); + } + retVec.SetComp(i, temp); + } + + retVec.CleanUp(); + + return retVec; +} + +Vector operator*(const Vector& lhv, const Matrix& rhm) { + if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention"); + Vector retVec(rhm.GetCol()); + + for (int i = 1; i <= rhm.GetCol(); i++) { + float temp = 0.0f; + for (int j = 1; j <= lhv.GetDim(); j++) { + temp += lhv.GetComp(j) * rhm.GetComp(j, i); + } + retVec.SetComp(i, temp); + } + + retVec.CleanUp(); + + return retVec; +} \ No newline at end of file