Atsumi Toda / Mbed 2 deprecated Matrix_calculation_test

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

Dependencies:   mbed

main.cpp

Committer:
Joeatsumi
Date:
2019-01-30
Revision:
0:6a28eb668082

File content as of revision 0:6a28eb668082:

#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;//１時間は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)));
/*-----------------------------------------------------*/
}