Atsumi Toda / Mbed 2 deprecated Matrix_calculation_test

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

Dependencies:   mbed

Vector/Vector_Matrix_operator.cpp@0:6a28eb668082, 2019-01-30 (annotated)

Committer:
Joeatsumi
Date:
Wed Jan 30 11:39:03 2019 +0000
Revision:
0:6a28eb668082
Direction cosine matrix and it's calculation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:6a28eb668082 1 #include "Vector_Matrix_operator.h"
Joeatsumi 0:6a28eb668082 2
Joeatsumi 0:6a28eb668082 3 Vector operator*(const Matrix& lhm, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 4 if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention");
Joeatsumi 0:6a28eb668082 5 Vector retVec(lhm.GetRow());
Joeatsumi 0:6a28eb668082 6
Joeatsumi 0:6a28eb668082 7 for (int i = 1; i <= lhm.GetRow(); i++) {
Joeatsumi 0:6a28eb668082 8 float temp = 0.0f;
Joeatsumi 0:6a28eb668082 9 for (int j = 1; j <= rhv.GetDim(); j++) {
Joeatsumi 0:6a28eb668082 10 temp += lhm.GetComp(i, j)*rhv.GetComp(j);
Joeatsumi 0:6a28eb668082 11 }
Joeatsumi 0:6a28eb668082 12 retVec.SetComp(i, temp);
Joeatsumi 0:6a28eb668082 13 }
Joeatsumi 0:6a28eb668082 14
Joeatsumi 0:6a28eb668082 15 retVec.CleanUp();
Joeatsumi 0:6a28eb668082 16
Joeatsumi 0:6a28eb668082 17 return retVec;
Joeatsumi 0:6a28eb668082 18 }
Joeatsumi 0:6a28eb668082 19
Joeatsumi 0:6a28eb668082 20 Vector operator*(const Vector& lhv, const Matrix& rhm) {
Joeatsumi 0:6a28eb668082 21 if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention");
Joeatsumi 0:6a28eb668082 22 Vector retVec(rhm.GetCol());
Joeatsumi 0:6a28eb668082 23
Joeatsumi 0:6a28eb668082 24 for (int i = 1; i <= rhm.GetCol(); i++) {
Joeatsumi 0:6a28eb668082 25 float temp = 0.0f;
Joeatsumi 0:6a28eb668082 26 for (int j = 1; j <= lhv.GetDim(); j++) {
Joeatsumi 0:6a28eb668082 27 temp += lhv.GetComp(j) * rhm.GetComp(j, i);
Joeatsumi 0:6a28eb668082 28 }
Joeatsumi 0:6a28eb668082 29 retVec.SetComp(i, temp);
Joeatsumi 0:6a28eb668082 30 }
Joeatsumi 0:6a28eb668082 31
Joeatsumi 0:6a28eb668082 32 retVec.CleanUp();
Joeatsumi 0:6a28eb668082 33
Joeatsumi 0:6a28eb668082 34 return retVec;
Joeatsumi 0:6a28eb668082 35 }