#include?
#include?
#include?
#define?step?0.01
#define?N?6000
#define?pi?3.141592653
#define?deg_rad?pi/180


void?MatrixMultiplication（int?mint?nint?pdouble?*M1double?*M2double?*M3）;
void?MatrixTranspose（int?mdouble?*adouble?*b）;
void?VectorPlus（int?m?double?*a?double?*b?double?*c）;
void?VectorMultiScaler（int?m?double?a?double?*b?double?*c）;
double?VectorNorm（int?m?double?*a?double?*b）;
void?MatrixDiagonal（int?m?double?*adouble?b）;
void?MatrixMultiplicationScaler（int?m?int?n?double?*a?double?b）;
void?MatrixPlus（int?m?int?n?double?*a?double?*b?double?*c）;
void?ode_vel（double?*wtei?double?*wtte?double?*vt?double?*fn?double?*dvt?double?g）;

int?main（）
{
????FILE?*imuout*ins_pos*pos_vel*ins_atti;
????int?ij;
????double?wbbi[3]fb[3]fn[3]q[4]atti[3]pos[3]vt[3]dvt[3];
????double?cbt[3][3]ctb[3][3]cte[3][3]cet[3][3];
????double?wtei[3]weei?=?0.7292115147e-4;
????double?Re=6378137RMRN;
????double?e?=?0.0033528106647474807;
????double?g0?=?9.7803266714g;
????double?wtte[3]wbbt[3]wbti[3]wtti[3];
????double?tempdeltheta[4][4]delthetasqdelthetaxdelthetaydelthetaz;
????double?IIII[4][4];
????double?temp_q[4];
????double?RK_K[4][3]RK_temp[3];

????imuout?=?fopen（“G:\\imu.txt““r“）;
????pos_vel?=?fopen（“G:\\pos_vel.txt““r“）;
????ins_pos?=?fopen（“G:\\ins_pos.txt““w“）;
????ins_atti?=?fopen（“G:\\ins_atti.txt““w“）;

????if（imuout?==?NULL）
????{
????????printf（“null\n“）;
????????exit（0）;
????}

????pos[0]?=?48*deg_rad;
????pos[1]?=?125*deg_rad;
????pos[2]?=?5000;

????vt[0]?=?0;
????vt[1]?=?0;
????vt[2]?=?0;

????atti[0]?=?5*deg_rad;
????atti[1]?=?5*deg_rad;
????atti[2]?=?5*deg_rad;

????cbt[0][0]?=?cos（atti[2]）*cos（atti[1]）+sin（atti[2]）*sin（atti[0]）*sin（atti[1]）;
????cbt[0][1]?=?-sin（atti[2]）*cos（atti[1]）+cos（atti[2]）*sin（atti[1]）*sin（atti[0]）;
????cbt[0][2]?=?-sin（atti[1]）*cos（atti[0]）;
????cbt[1][0]?=?sin（atti[2]）*cos（atti[0]）;
????cbt[1][1]?=?cos（atti[2]）*cos（atti[0]）;ins_pos?=?fopen（“G:\\ins_pos.txt““w“）;
????cbt[1][2]?=?sin（atti[0]）;
????cbt[2][0]?=?cos（atti[2]）*sin（atti[1]）-sin（atti[2]）*sin（atti[0]）*cos（atti[1]）;
????cbt[2][1]?=?-sin（atti[2]）*sin（atti[1]）-cos（atti[2]）*sin（atti[0]）*cos（atti[1]）;
????cbt[2][2]?=?cos（atti[0]）*cos（atti[1]）;
????MatrixTranspose（3cbtctb）;
????/*-------------------姿態(tài)矩陣算四元數(shù)--------------------------*/
????q[1]?=?sqrt（fabs（1?+?cbt[0][0]?-?cbt[1][1]?-?cbt[2][2]））/2;
????q[2]?=?（cbt[0][1]?+?cbt[1][0]）/（4*q[1]）;
????q[3]?=?（cbt[0][2]?+?cbt[2][0]）/（4*q[1]）;
????q[0]?=?（cbt[1][2]?-?cbt[2][1]）/（4*q[1]）;


????/*-------------------姿態(tài)矩陣算四元數(shù)--------------------------*/

????cte[0][0]?=?-sin（pos[1]）;
????cte[0][1]?=?cos（pos[1]）;
????cte[0][2]?=?0;
????cte[1][0]?=?-sin（pos[0]）*cos（pos[1]）;
????cte[1][1]?=?-sin（pos[0]）*sin（pos[1]）;
????cte[1][2]?=?cos（pos[0]）;
????cte[2][0]?=?cos（pos[0]）*cos（pos[1]）;
????cte[2][1]?=?cos（pos[0]）*sin（pos[1]）;
????cte[2][2]?=?sin（pos[0]）;
????