慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
Diff: main.cpp
- Revision:
- 0:6a28eb668082
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jan 30 11:39:03 2019 +0000 @@ -0,0 +1,162 @@ +#include "mbed.h" +#include "Vector.h" +#include "Matrix.h" +#include "Vector_Matrix_operator.h" + +/*---------------------------定数---------------------------*/ +#define PI 3.141592 +#define OMEGA 7.2921159/100000//地球の自転角速度 rad/s +/*---------------------------------------------------------*/ +DigitalOut myled(LED1); + +int second;//軌道からの経過時間[s] +double theta;//ECIとECEFのずれ[rad] + +double lon,lat; // lon 経度 lat 緯度 [rad] +double lon_degree,lat_degree;// lon_degree 経度 lat_degree 緯度 [degree] +double el,roll,az; //ピッチ,ロール,アジマス(rad) +double pitch_degree,roll_degree,azimaith_degree; + //ピッチ,ロール,アジマス(10進数表記) + +Serial pc(USBTX, USBRX); // Teratermとの接続 tx, rx + +/*--------------------------行列、ベクトル-----------------------------*/ +Matrix Q1(3, 3), Q2(3, 3),Q3(3,3); //検算の行列 +Matrix ECI_to_ECEF(3,3),ECEF_to_NED(3,3),NED_to_BODY(3,3);//変換行列 +Vector ECI(3),ECEF(3), NED(3),BODY(3); //座標系 +/*-------------------------------------------------------------------*/ + +/*--------------------プロトタイプ宣言----------------------*/ +void toString(Matrix& m); // 行列デバッグ用 +void toString_V(Vector& v); // ベクトルデバッグ用 +double deg_to_rad(double); // 度数単位からrad単位への変換 +/*--------------------------------------------------------*/ + +/* ------------------------------ デバッグ用関数 ------------------------------ */ + +void toString(Matrix& m) +{ + + for(int i=0; i<m.GetRow(); i++) { + for(int j=0; j<m.GetCol(); j++) { + pc.printf("%.6f\t", m.GetComp(i+1, j+1)); + } + pc.printf("\r\n"); + } + +} + +void toString_V(Vector& v) +{ + + for(int i=0; i<v.GetDim(); i++) { + pc.printf("%.6f\t", v.GetComp(i+1)); + } + pc.printf("\r\n"); + +} +/*-------------------------------------------------------------------------*/ +/*---------------------度数単位からラジアン単位に変換する------------*/ +double deg_to_rad(double degree) +{ double rad; + rad=degree*PI/180; + return rad; + } +/*---------------------------------------------------------------*/ + +int main() { + + /* ----------------Q1の要素値を設定----------------------*/ + float q1[9] = { + 1.0f, 2.0f, 3.0f, + 4.0f, 5.0f, 6.0f, + 7.0f, 8.0f, 9.0f + }; + + Q1.SetComps(q1); +/*-----------------------------------------------------*/ +/* ----------------Q2の要素値を設定----------------------*/ + float q2[9] = { + 1.0f, 2.0f, 3.0f, + 4.0f, 5.0f, 6.0f, + 7.0f, 8.0f, 9.0f + }; + + Q2.SetComps(q2); +/*-----------------------------------------------------*/ +/* ----------------ECIの要素値を設定----------------------*/ + float eci[3] = { + 4.0f, + 1.0f, + 0.0f + }; + + ECI.SetComps(eci); +/*-----------------------------------------------------*/ + +/*=================ECI座標系からECEF座標系への変換行列=====================*/ + +second=3600*6;//1時間は3600秒 +theta=OMEGA*second; +float eci_to_ecef[9]={ + cos(theta),sin(theta),0.0, + -sin(theta),cos(theta),0.0, + 0.0, 0.0 ,0.0 + }; + + ECI_to_ECEF.SetComps(eci_to_ecef); +/*====================================================================*/ + +/*=================ECEF座標系からNED座標系への変換行列=====================*/ + +lon_degree=0.0; //経度 +lat_degree=0.0; //緯度 +lon=deg_to_rad(lon_degree); +lat=deg_to_rad(lat_degree); + +float ecef_to_ned[9]={ + -sin(lat)*cos(lon),-sin(lat)*sin(lon) ,cos(lat), + -sin(lon) ,cos(lon) ,0.0, + -cos(lat)*cos(lon) , -cos(lat)*sin(lon) ,-sin(lat) + }; + + ECEF_to_NED.SetComps(ecef_to_ned); +/*====================================================================*/ + +/*=================NED座標系から機体座標系への変換行列=====================*/ + +pitch_degree=0;//ピッチ(10進数表記) +roll_degree=0;//ロール(10進数表記) +azimaith_degree=0;//アジマス(10進数表記) + +el=deg_to_rad(pitch_degree);//ピッチ(rad) +roll=deg_to_rad(roll_degree);//ロール(rad) +az=deg_to_rad(azimaith_degree);//アジマス(rad) + +float ned_to_body[9]={-cos(el)*cos(az) , cos(el)*sin(az) ,-sin(el), + + cos(roll)*sin(az)+sin(roll)*sin(el)*cos(az), + cos(az)*cos(roll)+sin(roll)*sin(el)*sin(az), + sin(roll)*cos(el), + + -sin(roll)*sin(az)+cos(roll)*sin(el)*cos(az), + -sin(roll)*cos(az)+cos(roll)*sin(el)*sin(az), + cos(roll)*cos(el) + }; + + NED_to_BODY.SetComps(ned_to_body); +/*--------------------------------------------------------------------*/ +/*-------------------計算を行う-----------------------*/ + Q3 = Q1 * Q2; + ECEF = ECI_to_ECEF*ECI; + NED = ECEF_to_NED*ECEF; + BODY = NED_to_BODY*NED; +/*-----------------------------------------------------*/ +/*------------Tera Termで計算結果を表示する--------------------*/ + toString(Q3); + toString_V(ECEF); + toString_V(NED); + toString_V(BODY); + //pc.printf("%.3f\r\n",sin(deg_to_rad(30))); +/*-----------------------------------------------------*/ +} \ No newline at end of file