捷联惯导c语言仿真

#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）;
    /*-------------------姿态矩阵算四元数--------------------------*/
    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]）;


    /*-------------------姿态矩阵算四元数--------------------------*/

    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]）;