慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
main.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
Joeatsumi | 0:6a28eb668082 | 1 | #include "mbed.h" |
Joeatsumi | 0:6a28eb668082 | 2 | #include "Vector.h" |
Joeatsumi | 0:6a28eb668082 | 3 | #include "Matrix.h" |
Joeatsumi | 0:6a28eb668082 | 4 | #include "Vector_Matrix_operator.h" |
Joeatsumi | 0:6a28eb668082 | 5 | |
Joeatsumi | 0:6a28eb668082 | 6 | /*---------------------------定数---------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 7 | #define PI 3.141592 |
Joeatsumi | 0:6a28eb668082 | 8 | #define OMEGA 7.2921159/100000//地球の自転角速度 rad/s |
Joeatsumi | 0:6a28eb668082 | 9 | /*---------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 10 | DigitalOut myled(LED1); |
Joeatsumi | 0:6a28eb668082 | 11 | |
Joeatsumi | 0:6a28eb668082 | 12 | int second;//軌道からの経過時間[s] |
Joeatsumi | 0:6a28eb668082 | 13 | double theta;//ECIとECEFのずれ[rad] |
Joeatsumi | 0:6a28eb668082 | 14 | |
Joeatsumi | 0:6a28eb668082 | 15 | double lon,lat; // lon 経度 lat 緯度 [rad] |
Joeatsumi | 0:6a28eb668082 | 16 | double lon_degree,lat_degree;// lon_degree 経度 lat_degree 緯度 [degree] |
Joeatsumi | 0:6a28eb668082 | 17 | double el,roll,az; //ピッチ,ロール,アジマス(rad) |
Joeatsumi | 0:6a28eb668082 | 18 | double pitch_degree,roll_degree,azimaith_degree; |
Joeatsumi | 0:6a28eb668082 | 19 | //ピッチ,ロール,アジマス(10進数表記) |
Joeatsumi | 0:6a28eb668082 | 20 | |
Joeatsumi | 0:6a28eb668082 | 21 | Serial pc(USBTX, USBRX); // Teratermとの接続 tx, rx |
Joeatsumi | 0:6a28eb668082 | 22 | |
Joeatsumi | 0:6a28eb668082 | 23 | /*--------------------------行列、ベクトル-----------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 24 | Matrix Q1(3, 3), Q2(3, 3),Q3(3,3); //検算の行列 |
Joeatsumi | 0:6a28eb668082 | 25 | Matrix ECI_to_ECEF(3,3),ECEF_to_NED(3,3),NED_to_BODY(3,3);//変換行列 |
Joeatsumi | 0:6a28eb668082 | 26 | Vector ECI(3),ECEF(3), NED(3),BODY(3); //座標系 |
Joeatsumi | 0:6a28eb668082 | 27 | /*-------------------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 28 | |
Joeatsumi | 0:6a28eb668082 | 29 | /*--------------------プロトタイプ宣言----------------------*/ |
Joeatsumi | 0:6a28eb668082 | 30 | void toString(Matrix& m); // 行列デバッグ用 |
Joeatsumi | 0:6a28eb668082 | 31 | void toString_V(Vector& v); // ベクトルデバッグ用 |
Joeatsumi | 0:6a28eb668082 | 32 | double deg_to_rad(double); // 度数単位からrad単位への変換 |
Joeatsumi | 0:6a28eb668082 | 33 | /*--------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 34 | |
Joeatsumi | 0:6a28eb668082 | 35 | /* ------------------------------ デバッグ用関数 ------------------------------ */ |
Joeatsumi | 0:6a28eb668082 | 36 | |
Joeatsumi | 0:6a28eb668082 | 37 | void toString(Matrix& m) |
Joeatsumi | 0:6a28eb668082 | 38 | { |
Joeatsumi | 0:6a28eb668082 | 39 | |
Joeatsumi | 0:6a28eb668082 | 40 | for(int i=0; i<m.GetRow(); i++) { |
Joeatsumi | 0:6a28eb668082 | 41 | for(int j=0; j<m.GetCol(); j++) { |
Joeatsumi | 0:6a28eb668082 | 42 | pc.printf("%.6f\t", m.GetComp(i+1, j+1)); |
Joeatsumi | 0:6a28eb668082 | 43 | } |
Joeatsumi | 0:6a28eb668082 | 44 | pc.printf("\r\n"); |
Joeatsumi | 0:6a28eb668082 | 45 | } |
Joeatsumi | 0:6a28eb668082 | 46 | |
Joeatsumi | 0:6a28eb668082 | 47 | } |
Joeatsumi | 0:6a28eb668082 | 48 | |
Joeatsumi | 0:6a28eb668082 | 49 | void toString_V(Vector& v) |
Joeatsumi | 0:6a28eb668082 | 50 | { |
Joeatsumi | 0:6a28eb668082 | 51 | |
Joeatsumi | 0:6a28eb668082 | 52 | for(int i=0; i<v.GetDim(); i++) { |
Joeatsumi | 0:6a28eb668082 | 53 | pc.printf("%.6f\t", v.GetComp(i+1)); |
Joeatsumi | 0:6a28eb668082 | 54 | } |
Joeatsumi | 0:6a28eb668082 | 55 | pc.printf("\r\n"); |
Joeatsumi | 0:6a28eb668082 | 56 | |
Joeatsumi | 0:6a28eb668082 | 57 | } |
Joeatsumi | 0:6a28eb668082 | 58 | /*-------------------------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 59 | /*---------------------度数単位からラジアン単位に変換する------------*/ |
Joeatsumi | 0:6a28eb668082 | 60 | double deg_to_rad(double degree) |
Joeatsumi | 0:6a28eb668082 | 61 | { double rad; |
Joeatsumi | 0:6a28eb668082 | 62 | rad=degree*PI/180; |
Joeatsumi | 0:6a28eb668082 | 63 | return rad; |
Joeatsumi | 0:6a28eb668082 | 64 | } |
Joeatsumi | 0:6a28eb668082 | 65 | /*---------------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 66 | |
Joeatsumi | 0:6a28eb668082 | 67 | int main() { |
Joeatsumi | 0:6a28eb668082 | 68 | |
Joeatsumi | 0:6a28eb668082 | 69 | /* ----------------Q1の要素値を設定----------------------*/ |
Joeatsumi | 0:6a28eb668082 | 70 | float q1[9] = { |
Joeatsumi | 0:6a28eb668082 | 71 | 1.0f, 2.0f, 3.0f, |
Joeatsumi | 0:6a28eb668082 | 72 | 4.0f, 5.0f, 6.0f, |
Joeatsumi | 0:6a28eb668082 | 73 | 7.0f, 8.0f, 9.0f |
Joeatsumi | 0:6a28eb668082 | 74 | }; |
Joeatsumi | 0:6a28eb668082 | 75 | |
Joeatsumi | 0:6a28eb668082 | 76 | Q1.SetComps(q1); |
Joeatsumi | 0:6a28eb668082 | 77 | /*-----------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 78 | /* ----------------Q2の要素値を設定----------------------*/ |
Joeatsumi | 0:6a28eb668082 | 79 | float q2[9] = { |
Joeatsumi | 0:6a28eb668082 | 80 | 1.0f, 2.0f, 3.0f, |
Joeatsumi | 0:6a28eb668082 | 81 | 4.0f, 5.0f, 6.0f, |
Joeatsumi | 0:6a28eb668082 | 82 | 7.0f, 8.0f, 9.0f |
Joeatsumi | 0:6a28eb668082 | 83 | }; |
Joeatsumi | 0:6a28eb668082 | 84 | |
Joeatsumi | 0:6a28eb668082 | 85 | Q2.SetComps(q2); |
Joeatsumi | 0:6a28eb668082 | 86 | /*-----------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 87 | /* ----------------ECIの要素値を設定----------------------*/ |
Joeatsumi | 0:6a28eb668082 | 88 | float eci[3] = { |
Joeatsumi | 0:6a28eb668082 | 89 | 4.0f, |
Joeatsumi | 0:6a28eb668082 | 90 | 1.0f, |
Joeatsumi | 0:6a28eb668082 | 91 | 0.0f |
Joeatsumi | 0:6a28eb668082 | 92 | }; |
Joeatsumi | 0:6a28eb668082 | 93 | |
Joeatsumi | 0:6a28eb668082 | 94 | ECI.SetComps(eci); |
Joeatsumi | 0:6a28eb668082 | 95 | /*-----------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 96 | |
Joeatsumi | 0:6a28eb668082 | 97 | /*=================ECI座標系からECEF座標系への変換行列=====================*/ |
Joeatsumi | 0:6a28eb668082 | 98 | |
Joeatsumi | 0:6a28eb668082 | 99 | second=3600*6;//1時間は3600秒 |
Joeatsumi | 0:6a28eb668082 | 100 | theta=OMEGA*second; |
Joeatsumi | 0:6a28eb668082 | 101 | float eci_to_ecef[9]={ |
Joeatsumi | 0:6a28eb668082 | 102 | cos(theta),sin(theta),0.0, |
Joeatsumi | 0:6a28eb668082 | 103 | -sin(theta),cos(theta),0.0, |
Joeatsumi | 0:6a28eb668082 | 104 | 0.0, 0.0 ,0.0 |
Joeatsumi | 0:6a28eb668082 | 105 | }; |
Joeatsumi | 0:6a28eb668082 | 106 | |
Joeatsumi | 0:6a28eb668082 | 107 | ECI_to_ECEF.SetComps(eci_to_ecef); |
Joeatsumi | 0:6a28eb668082 | 108 | /*====================================================================*/ |
Joeatsumi | 0:6a28eb668082 | 109 | |
Joeatsumi | 0:6a28eb668082 | 110 | /*=================ECEF座標系からNED座標系への変換行列=====================*/ |
Joeatsumi | 0:6a28eb668082 | 111 | |
Joeatsumi | 0:6a28eb668082 | 112 | lon_degree=0.0; //経度 |
Joeatsumi | 0:6a28eb668082 | 113 | lat_degree=0.0; //緯度 |
Joeatsumi | 0:6a28eb668082 | 114 | lon=deg_to_rad(lon_degree); |
Joeatsumi | 0:6a28eb668082 | 115 | lat=deg_to_rad(lat_degree); |
Joeatsumi | 0:6a28eb668082 | 116 | |
Joeatsumi | 0:6a28eb668082 | 117 | float ecef_to_ned[9]={ |
Joeatsumi | 0:6a28eb668082 | 118 | -sin(lat)*cos(lon),-sin(lat)*sin(lon) ,cos(lat), |
Joeatsumi | 0:6a28eb668082 | 119 | -sin(lon) ,cos(lon) ,0.0, |
Joeatsumi | 0:6a28eb668082 | 120 | -cos(lat)*cos(lon) , -cos(lat)*sin(lon) ,-sin(lat) |
Joeatsumi | 0:6a28eb668082 | 121 | }; |
Joeatsumi | 0:6a28eb668082 | 122 | |
Joeatsumi | 0:6a28eb668082 | 123 | ECEF_to_NED.SetComps(ecef_to_ned); |
Joeatsumi | 0:6a28eb668082 | 124 | /*====================================================================*/ |
Joeatsumi | 0:6a28eb668082 | 125 | |
Joeatsumi | 0:6a28eb668082 | 126 | /*=================NED座標系から機体座標系への変換行列=====================*/ |
Joeatsumi | 0:6a28eb668082 | 127 | |
Joeatsumi | 0:6a28eb668082 | 128 | pitch_degree=0;//ピッチ(10進数表記) |
Joeatsumi | 0:6a28eb668082 | 129 | roll_degree=0;//ロール(10進数表記) |
Joeatsumi | 0:6a28eb668082 | 130 | azimaith_degree=0;//アジマス(10進数表記) |
Joeatsumi | 0:6a28eb668082 | 131 | |
Joeatsumi | 0:6a28eb668082 | 132 | el=deg_to_rad(pitch_degree);//ピッチ(rad) |
Joeatsumi | 0:6a28eb668082 | 133 | roll=deg_to_rad(roll_degree);//ロール(rad) |
Joeatsumi | 0:6a28eb668082 | 134 | az=deg_to_rad(azimaith_degree);//アジマス(rad) |
Joeatsumi | 0:6a28eb668082 | 135 | |
Joeatsumi | 0:6a28eb668082 | 136 | float ned_to_body[9]={-cos(el)*cos(az) , cos(el)*sin(az) ,-sin(el), |
Joeatsumi | 0:6a28eb668082 | 137 | |
Joeatsumi | 0:6a28eb668082 | 138 | cos(roll)*sin(az)+sin(roll)*sin(el)*cos(az), |
Joeatsumi | 0:6a28eb668082 | 139 | cos(az)*cos(roll)+sin(roll)*sin(el)*sin(az), |
Joeatsumi | 0:6a28eb668082 | 140 | sin(roll)*cos(el), |
Joeatsumi | 0:6a28eb668082 | 141 | |
Joeatsumi | 0:6a28eb668082 | 142 | -sin(roll)*sin(az)+cos(roll)*sin(el)*cos(az), |
Joeatsumi | 0:6a28eb668082 | 143 | -sin(roll)*cos(az)+cos(roll)*sin(el)*sin(az), |
Joeatsumi | 0:6a28eb668082 | 144 | cos(roll)*cos(el) |
Joeatsumi | 0:6a28eb668082 | 145 | }; |
Joeatsumi | 0:6a28eb668082 | 146 | |
Joeatsumi | 0:6a28eb668082 | 147 | NED_to_BODY.SetComps(ned_to_body); |
Joeatsumi | 0:6a28eb668082 | 148 | /*--------------------------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 149 | /*-------------------計算を行う-----------------------*/ |
Joeatsumi | 0:6a28eb668082 | 150 | Q3 = Q1 * Q2; |
Joeatsumi | 0:6a28eb668082 | 151 | ECEF = ECI_to_ECEF*ECI; |
Joeatsumi | 0:6a28eb668082 | 152 | NED = ECEF_to_NED*ECEF; |
Joeatsumi | 0:6a28eb668082 | 153 | BODY = NED_to_BODY*NED; |
Joeatsumi | 0:6a28eb668082 | 154 | /*-----------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 155 | /*------------Tera Termで計算結果を表示する--------------------*/ |
Joeatsumi | 0:6a28eb668082 | 156 | toString(Q3); |
Joeatsumi | 0:6a28eb668082 | 157 | toString_V(ECEF); |
Joeatsumi | 0:6a28eb668082 | 158 | toString_V(NED); |
Joeatsumi | 0:6a28eb668082 | 159 | toString_V(BODY); |
Joeatsumi | 0:6a28eb668082 | 160 | //pc.printf("%.3f\r\n",sin(deg_to_rad(30))); |
Joeatsumi | 0:6a28eb668082 | 161 | /*-----------------------------------------------------*/ |
Joeatsumi | 0:6a28eb668082 | 162 | } |