Atsumi Toda / Mbed 2 deprecated Matrix_calculation_test

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

Dependencies:   mbed

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?

UserRevisionLine numberNew 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;//１時間は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 }