#include?
#include?
#include?
#include?
#include?
#include?

const?double?GRAVITY?=?9.8;
const?double?PI?=?3.1415926536l;
const?double?RPI?=?PI?/?180.;
const?double?DPI?=?180.?/?PI;
const?double?WIE?=?15.04088?*?RPI?/?3600.;	//單位rad/s
const?double?RE?=?6.378165e6l;	//單位：m
const?double?AEE?=?1.0?/?298.3;
const?double?EI?=?1e-6;

const?int?sta_num?=?12;
const?int?mea_num?=?3;

double?s0?=?65536.0;
double?w0?=?2053.0;
double?v0?=?13849.0;
double?r0?=?0.0;
double?t0?EGx;
int?m0;

double?white（）		//生成白噪聲											
{
	t0?=?0.0;
??????
	for（int?j?=?0;?j?	{
		r0?=?w0?*?r0?+?v0;
		m0?=?（int）（r0?/?s0）;
		r0?=?r0?-?m0?*?s0;
		t0?=?t0?+?r0?/??s0;
	}
	EGx?=?t0?-?6.0;
	return（EGx）;
}
//mxn維矩陣w乘以n維向量b得到m維向量a
void?vecmul（int?m?int?n?double?*a?double?*w?double?*b）
{
	double?aa[20];			//一般維數不超過3維
	int?i?j;
	for（i?=?0;?i?	{
		aa[i]?=?0.0;
		for（j?=?0;?j?			aa[i]?+=?（*（w?+?i*n?+?j））?*?（*（b?+?j））;
	}
	for（i?=?0;?i?		*（a?+?i）?=?aa[i];
}
//m維向量b與c相加得到向量a
void?vecadd（int?m?double?*a?double?*b?double?*c）
{
	int?i;
	for（i?=?0;?i?		*（a?+?i）?=?*（b?+?i）?+?*（c?+?i）;
}
//m維向量b與c相減得到向量a
void?vecsub（int?m?double?*a?double?*b?double?*c）
{
	int?i;
	for（i?=?0;?i?		*（a?+?i）?=?*（b?+?i）?-?*（c?+?i）;
}
//向量b的裝置與向量c相乘得到向量a
//?0???-b2???b1
//?b2???0???-b
//-b1???b????0
void?cvecmul（double?*a?double?*b?double?*c）
{
	double?bb[3][3];
	bb[0][0]?=?0.01;
	bb[0][1]?=?-*（b?+?2）;
	bb[0][2]?=?*（b?+?1）;
	bb[1][0]?=?*（b?+?2）;
	bb[1][1]?=?0.01;
	bb[1][2]?=?-*b;
	bb[2][0]?=?-*（b?+?1）;
	bb[2][1]?=?*b;
	bb[2][2]?=?0.01;
	vecmul（3?3?a?（double?*）bb?c）;
}
//n1xn2矩陣b乘以n2xn3矩陣得到n1xn3矩陣
void?mamul（int?n1?int?n2?int?n3?double?*a?double?*b?double?*c）
{
	double?d[20][20];???//一般矩陣相乘維數不超過3
	int?i?j?k;
	for（i?=?0;?i?	{
		for（j?=?0;?j?		{
			d[i][j]?=?0.0;
			for（k?=?0;?k?				d[i][j]?+=?（*（b?+?i?*?n2?+?k））?*?（*（c?+?k?*?n3?+?j））;
		}
	}
	for（i?=?0;?i?	{
		for（j?=?0;?j?		{
			*（a?+?i?*?n3?+?j）?=?d[i][j];
		}
	}
}
//三個矩陣的維數均為n1xn2
void?maadd（int?n1?int?n2?double?*a?double?*b?double?*c）
{
	int?i?j;
	for（i?=?0;?i?		for（j?=?0;?j?			*（a?+?i?*?n2?+?j）?=?（*（b?+?i?*?n2?+?j））?+?（*（c?+?i?*?n2?+?j））;
}
//三個矩陣的維數均為n1xn2
void?masub（int?n1?int?n2?double?*a?double?*b?double?*c）
{
	int?i?j;
	for（i?=?0;?i?		for（j?=?0;?j?			*（a?+?i?*?n2?+?j）?=?（*（b?+?i?*?n2?+?j））?-?（*（c?+?i?*?n2?+?j））;
}
//向量與常數相乘，得到向量a
void?avecmul（int?n?double?*a?double?*b?double?c）
{
	int?i;
	for（i?=?0;?i?		*（a?+?i）?=?*（b?+?i）?*?c;
}
//矩陣數乘:a==b?（x）?c
void?amamul（int?n1int?n2double?*adouble?*b?double?c）
{
	int?ij;

	for?（i=0;i		for?（j=0;j			*（a+i*n2+j）=*（b+i*n2+j）*c;
}
//矩陣轉置
void?maturn（int?n1int?n2?double?*a?double?*b）
{
	double?d[20][20]