/**********************main()***********************/
void main()
{
fp_err=fopen("纯捷联err.txt","w");
//fp_V=fopen("V.txt","w");
//fp_tV=fopen("tV.txt","w");
//初始化导航参数
initial(); //初始化理论值和实际值
ctCn2b(); //姿态角计算姿态矩阵（理论）
ini_Cn2b();//姿态角计算姿态矩阵（实际）
ini_q(); //四元数初始化（实际值）
/*Mk0=gyro_ran;
Nk0=acce_ran;*/
for(number=0;number<Num;number++)
{
noise=white();
/*Mk=-number*sampletime/1800*Mk0;
Nk=-number*sampletime/1800*Nk0;
printf("%le\t %le\t\n",Mk,Nk);*/
D_gyro[0]=(gyro_ran*noise+gyro_con0)*dh_hs;//+Mk
D_gyro[1]=(gyro_ran*noise+gyro_con1)*dh_hs;//+Mk
D_gyro[2]=(gyro_ran*noise+gyro_con2)*dh_hs;//+Mk
D_acce[0]=(acce_ran*noise+acce_con0)*g/1000.0;//+Nk
D_acce[1]=(acce_ran*noise+acce_con1)*g/1000.0;//+Nk+Nk
D_acce[2]=(acce_ran*noise+acce_con2)*g/1000.0;//
t_renew();//导航参数理论值更新函数
ctCn2b(); //由理论值姿态角解算姿态矩阵的函数
out_Wib_b();//导入陀螺数据
cWnb_b();//真实值计算
q_renew();//真实值计算
cCn2b();//真实值，从四元数中计算出来
out_f(); //导入加速度计数据
renew();//理论值
if((number%Tfnum==0)&&(number!=0)) // 1s
{
fprintf(fp_err,"%e\t %e\t %e\t %e\t %e\t %e\t %e\t\n",(P-tP)*H_d*60,(R-tR)*H_d*60,(H-tH)*H_d*60,
V[0]-tV[0],V[1]-tV[1],(latit-tlatit)*Rn,(longi-tlongi)*Re*cos(tlatit));
}
}
fclose(fp_err);
// fclose(fp_V);
// fclose(fp_tV);
}
void
tunDeviceRecForceXValuators (TunDevicePtr tun, int n)
{
if (n >= tun->nof_xvaluators)
{
int i;
tun->xval_to_lval_tbl = tun->xval_to_lval_tbl
? t_renew (TunValuatorPtr, tun->xval_to_lval_tbl, n)
: t_new (TunValuatorPtr, n);
for (i = tun->nof_xvaluators; i < n; i++)
tun->xval_to_lval_tbl [i] = NULL;
tun->nof_xvaluators = n;
}
}
