捷联惯导c算法

//捷联惯导
#include 
#include 
#include 

#define PI 3.1415926
#define WIE 7.2921158e-5//弧度每秒
#define RAD PI/180
#define G 9.8
#define Re 6.378393e6
#define dRe 0.15678e-6
#define e 0.3367e-2

//void Initial（）;
void Initial（double C[3][3]double T[3][3]double Q[4]double POS[3]double V[2]double Angl[3]double Wiep[3]double WEPP[2]）;
void INITCMAT（double C[3][3]double POS[3]）;
void INITQ（double T[3][3]double Q[4]）;
void INITWIEP（double C[3][3]）;
void INITWEPP（double C[3][3] double V[2]）;
void calcG（double C[3][3]double h）;
void calcV（double V[2]）;
void calcWPBB（double Wpbb[3]double Wibb[3] double T[3][3]double Wepp[3] double Wiep[3]double WEPP[2]）;
//void updConvQ（double Wpbb[3]）;
void updConvQ（double Wpbb[3]double  Conv_Q[4][4]）;
void updateQ（double Wpbb1[3]double Wpbb2[3]double Wpbb3[3]double Q[4]double t1）;
void unitQ（double Q[4]）;
void calaT（double Q[4]double T[3][3]）;
void convacce（double T[3][3]double fp[3]double fb[3]）;
void updateVp（double fp[3]double Wiep[3]double Wepp[3]double V[3]double t2）;
//void updateVp（double fp[3]double Wiep[3]double Wepp[3]double Init_V[3]double t2）;
void calaWEPP（double C[3][3] double V[2]double WEPP[2]）;
void updateC（double C[3][3]double WEPP[2]double t2）;
//void updateC（double Init_C[3][3]double C[3][3]double Wepp[2]double t2）;
void calaWIEP（double Wiep[3]double C[3][3]double Wie[3]）;
void calapos（double POS[3]double C[3][3]）;
void calczitai（double Angl[3]double T[3][3]double alpha）;

void mulit_MAT_VEC1（double mulit[2]double a[2][2]double b[2]）;
void mulit_MAT_VEC2（double mulit[3]double a[3][3]double b[3]）;
void mulit_MAT_VEC3（double mulit[4]double a[4][4]double b[4]）;
void mulit_NUM_VEC1（double mulit[4]double ndouble a[4]）;
void mulit_NUM_VEC2（double mulit[3]double ndouble a[3]）;
void mulit_NUM_MAT1（double mulit[3][3]double ndouble a[3][3]）;
void add_VECTOR1（double sum[3]double a[3]double b[3]）;
void add_VECTOR2（double sum[4]double a[4]double b[4]）;
void add_mVECTOR（double sum[4]double a[4]double b[4]double c[4]double d[4]double e1[4]）;
void sub_VECTOR1（double diff[3]double a[3]double b[3]）;
void trans_MATRIX1（double a[3][3]double a_trans[3][3]）;
void mulit_MATRIX（double mulit[3][3]double a[3][3]double b[3][3]）;
void add_MATRIX（double sum[3][3]double a[3][3]double b[3][3]）;
  
void main（）

{
	double LAT=45*RAD;
    double t1=0.01;
	double t2=0.04;
	int I=0;
    int mn;
	double T[3][3];
    double Wepp[3];
    double WEPP[2];
	double Wiep[3];
    double Q[4];
    double fp[3];    
	double C[3][3];
    double V[2];
	double Wie[3];
	double h=0;
	double Angl[3];
	double alpha=0;
	double POS[3];   
	double x=0;
    double Wf[18];
    double Wibb1[3]Wibb2[3]Wibb3[3]fb[3];
    double Wpbb1[3]Wpbb2[3]Wpbb3[3];
	  
	  //Initial（）;
    Initial（CTQPOSVAnglWiepWEPP）;
	cout<    cout<