//捷聯慣導
#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<