基于vc6.0----纯惯性技术仿真程序

最近一直没有写博客是，有原因的，事情多，再加上程序大，需要慢慢理解，但我还是会努力写博客的。

//********************* 捷联惯导静基座（或匀速）考虑陀螺漂移和加表偏置时系统误差 ********************//

    //********************* 坐标系为：东北天 *******************//    //前缀t表示理论值，不加t表示实际值;H表示（航向角）,P表示（纵摇角）,R表示（横摇角）#include <math.h>#include <stdio.h>#include <stdlib.h>       #define  sampletime  0.01#define  Tfnum  100   #define  Num  100*60*125   //仿真时间2h#define  Ri  6.378140e+6             //地球半径#define  g  9.8                      //重力加速度#define  e  1/298.257                //偏心率#define  PI  3.14159265358979323846  //圆周率#define  Wie  7.292125241172469e-5   //地球自转角速率#define  D_h  PI/180.0               //度到弧度的单位转换#define  H_d  180.0/PI               //弧度到度的单位转换#define  dh_hs  PI/180.0/3600.0      //度每小时到弧度每秒的单位转换#define  hs_dh  180.0*3600.0/PI      //弧度每秒到度每小时的单位转换#define  D_con D_h*D_h/3600/3600     //度每小时的平方到弧度每秒的平方的单位转换#define  G_con 9.8*9.8               //重力加速度的平方#define  H0  60.0*D_h //初始航向角#define  P0  0.0*D_h   //初始纵摇角#define  R0  0.0*D_h   //初始横摇角#define  datH0  0.05*D_h //初始航向角误差#define  datP0  0.02*D_h //初始纵摇角误差#define  datR0  0.02*D_h //初始横摇角误差#define  A_H  5*D_h  //航向角摇摆的幅度0.0#define  A_P  5*D_h  //纵摇角摇摆的幅度#define  A_R  5*D_h  //横摇角摇摆的幅度#define  W_H  2.0*PI/5.0  //航向角摇摆的频率#define  W_P  2.0*PI/5.0  //纵摇角摇摆的频率#define  W_R  2.0*PI/5.0  //横向角摇摆的频率#define  longi0  118.0*D_h //初始经度#define  latit0  32.0*D_h  //初始纬度#define  Velo  8.0      //初始速度#define  datVelo  0.1   //初始速度误差/* int  AF=0;       //加速标志double ATS=30.0;//加速起始时间double ATE=60.0;//加速结束时间double AV=0.5;  //加速度大小double ATC=0.0; //加速度实际时间 int  RF=1;      //拐弯标志double RTS=30.0;//拐弯起始时间double RA=30.0; //蛇形拐弯机动角度double RV=1.0;  //拐弯角速度度大小double RTC=0.0; //拐弯实际时间   */#define  gyro_con0   0.05 //东向陀螺的常值漂移 度/h#define  gyro_con1   0.05 //北向陀螺的常值漂移 度/h#define  gyro_con2   0.05 //天向陀螺的常值漂移 度/h#define  gyro_ran    0.05 //陀螺的随机漂移    度/h#define  acce_con0   0.2 //东向加表的常值偏置  mg#define  acce_con1   0.2 //北向加表的常值偏置  mg#define  acce_con2   0.2 //北向加表的常值偏置  mg#define  acce_ran    0.2 //加表的随机偏置long int number=0;//循环次数double noise;//double Mk,Nk,Mk0,Nk0; //马尔科夫!!!!double tWin_n[3];//（理论值）导航坐标系相对地球惯性坐标系的角速率在导航坐标系上的投影double tWin_b[3];//（理论值）导航坐标系相对地球惯性坐标系的角速率在船体坐标系上的投影double tWnb_b[3];//（理论值）船体坐标系相对导航坐标系的角速率在船体坐标系上的投影double tWib_b[3];//（理论值）船体坐标系相对地球惯性坐标系的角速率在船体坐标系上的投影double tCn2b[3][3];//（理论值）导航坐标系到船体坐标系的姿态矩阵double tH,tP,tR,dtH,dtP,dtR;//理论的航向、纵摇、横摇以及航向微分、纵摇微分、横摇微分double tlongi,tlatit,tRe,tRn;//理论的经度、纬度、与子午面垂直的法线平面的曲率半径 、子午面曲率半径double tV[3],dtV[3],tf[3]  ,tf_n[3],tf_b[3];//理论的速度、速度微分、比力、导航坐标系下的比力、船体的坐标系下的比力double temp1,temp2;//中间变量double D_gyro[3];//陀螺漂移值double D_acce[3];//加表偏置值double Wib_b[3];double f_b[3];//船体坐标系下加速度计输出的比力 /////////////////////////SINS解算///////////////////////////double Re,Rn;double Win_n[3],Win_b[3],Wnb_b[3];double Cn2b[3][3];double H,P,R;double V[3];double Vt[3];double q[4];double q_old[4];double D_sita[4][4];double vD_sita;double f_n[3];double longi;double latit;FILE *fp_err,*fp_V,*fp_tV; //定义文件//white noisedouble 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;  int i;  for(i=0;i<12;i++)   {r0=w0*r0+v0;m0=r0/s0;r0=r0-m0*s0;    t0=t0+r0/s0;  }   EGx=t0-6.0;  return(EGx);}/******************导航参数初始化函数***************/void initial(){tH=H0;//理论的航向角赋初值tP=P0;//理论的纵摇角赋初值tR=R0;//理论的横摇角赋初值           tlongi=longi0;//理论的经度赋初值tlatit=latit0;//理论的纬度赋初值tRe=Ri*(e*sin(tlatit)*sin(tlatit)+1);//理论的与子午面垂直的法线平面的曲率半径赋初值tRn=Ri*(3.0*e*sin(tlatit)*sin(tlatit)-2.0*e+1); //理论的子午面曲率半径赋初值    tV[0]=-Velo*sin(tH);//理论的东向速度赋初值    tV[1]=Velo*cos(tH);    tV[2]=0.0;H=H0+datH0;//实际的航向角赋初值P=P0+datP0;//实际的纵摇角赋初值R=R0+datR0;//实际的航向角赋初值longi=tlongi;//实际的经度赋初值latit=tlatit;//实际的纬度赋初值Re=tRe;//法线平面的曲率半径Rn=tRn;//子午面曲率半径V[0]=-(Velo+datVelo)*sin(H);//实际的东向速度赋初值V[1]= (Velo+datVelo)*cos(H);    V[2]= 0.0;}/****************由姿态角解算姿态矩阵的函数*************/void ctCn2b(){ tCn2b[0][0]=cos(tR)*cos(tH)-sin(tR)*sin(tP)*sin(tH);tCn2b[0][1]=cos(tR)*sin(tH)+sin(tR)*sin(tP)*cos(tH);tCn2b[0][2]=-sin(tR)*cos(tP);    tCn2b[1][0]=-cos(tP)*sin(tH);tCn2b[1][1]=cos(tP)*cos(tH);tCn2b[1][2]=sin(tP);tCn2b[2][0]=sin(tR)*cos(tH)+cos(tR)*sin(tP)*sin(tH);tCn2b[2][1]=sin(tR)*sin(tH)-cos(tR)*sin(tP)*cos(tH);tCn2b[2][2]=cos(tR)*cos(tP);}/**************初始化姿态矩阵函数***************/void ini_Cn2b(){Cn2b[0][0]=cos(R)*cos(H)-sin(R)*sin(P)*sin(H);Cn2b[0][1]=cos(R)*sin(H)+sin(R)*sin(P)*cos(H);Cn2b[0][2]=-sin(R)*cos(P);    Cn2b[1][0]=-cos(P)*sin(H);Cn2b[1][1]=cos(P)*cos(H);Cn2b[1][2]=sin(P);Cn2b[2][0]=sin(R)*cos(H)+cos(R)*sin(P)*sin(H);Cn2b[2][1]=sin(R)*sin(H)-cos(R)*sin(P)*cos(H);Cn2b[2][2]=cos(R)*cos(P);}/***************四元数初始化函数****************/void ini_q(){q_old[0]=cos(H/2.0)*cos(P/2.0)*cos(R/2.0)-sin(H/2.0)*sin(P/2.0)*sin(R/2.0);q_old[1]=cos(H/2.0)*sin(P/2.0)*cos(R/2.0)-sin(H/2.0)*cos(P/2.0)*sin(R/2.0);    q_old[2]=cos(H/2.0)*cos(P/2.0)*sin(R/2.0)+sin(H/2.0)*sin(P/2.0)*cos(R/2.0);q_old[3]=sin(H/2.0)*cos(P/2.0)*cos(R/2.0)+cos(H/2.0)*sin(P/2.0)*sin(R/2.0);}/***************导航参数理论值更新函数*************/void t_renew(){      tH=H0+A_H*sin(W_H*number*sampletime);//理论的航向角更新，通过正弦函数来实现tP=P0+A_P*sin(W_P*number*sampletime);//理论的纵摇角更新，通过正弦函数来实现    tR=R0+A_R*sin(W_R*number*sampletime);//理论的纵摇角更新，通过正弦函数来实现dtH=A_H*W_H*cos(W_H*number*sampletime);//理论的航向角微分dtP=A_P*W_P*cos(W_P*number*sampletime);//理论的纵摇角微分 dtR=A_R*W_R*cos(W_R*number*sampletime);//理论的横摇角微分    tRe=Ri*(e*sin(tlatit)*sin(tlatit)+1);//理论的与子午面垂直的法线平面的曲率半径更新tRn=Ri*(3.0*e*sin(tlatit)*sin(tlatit)-2.0*e+1);//理论的子午面曲率半径更新tV[0]=-Velo*sin(tH);dtV[0]=-Velo*cos(tH)*dtH;tV[1]=Velo*cos(tH);dtV[1]=-Velo*sin(tH)*dtH;tV[2]=0.0;dtV[2]=0.0;tlatit=tlatit+tV[1]*sampletime/tRn;//理论的纬度更新tlongi=tlongi+tV[0]*sampletime/(tRe*cos(tlatit));//理论的经度更新 /*if(RF){if(RTC<=RTS){tH=H0+A_H*sin(W_H*number*sampletime);dtH=A_H*W_H*cos(W_H*number*sampletime);tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}if((RTC>RTS)&&(RTC<=(RTS+RA/RV))){tH=H0+A_H*sin(W_H*number*sampletime)+RV*(RTC-RTS)*D_h;dtH=A_H*W_H*cos(W_H*number*sampletime)+RV*D_h;tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}if((RTC>(RTS+RA/RV))&&(RTC<=(RTS+3.0*RA/RV))){tH=H0+A_H*sin(W_H*number*sampletime)+RA*D_h-RV*(RTC-RTS-RA/RV)*D_h;dtH=A_H*W_H*cos(W_H*number*sampletime)-RV*D_h;tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}if((RTC>(RTS+3.0*RA/RV))&&(RTC<=(RTS+4.0*RA/RV))){tH=H0+A_H*sin(W_H*number*sampletime)-RA*D_h+RV*(RTC-RTS-3.0*RA/RV)*D_h;dtH=A_H*W_H*cos(W_H*number*sampletime)+RV*D_h;tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}if(RTC>(RTS+4.0*RA/RV)){tH=H0+A_H*sin(W_H*number*sampletime);dtH=A_H*W_H*cos(W_H*number*sampletime);tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}RTC=RTC+sampletime;}else{    tH=H0+A_H*sin(W_H*number*sampletime);dtH=A_H*W_H*cos(W_H*number*sampletime);    tP=P0+A_P*sin(W_P*number*sampletime);dtP=A_P*W_P*cos(W_P*number*sampletime);        tR=R0+A_R*sin(W_R*number*sampletime);dtR=A_R*W_R*cos(W_R*number*sampletime);}tRe=Ri*(e*sin(tlatit)*sin(tlatit)+1);tRn=Ri*(3.0*e*sin(tlatit)*sin(tlatit)-2.0*e+1);if(AF){if(ATC<=ATS){tV[0]=-Velo*sin(tH);dtV[0]=-Velo*cos(tH)*dtH;tV[1]=Velo*cos(tH);dtV[1]=-Velo*sin(tH)*dtH;tV[2]=0.0;        dtV[2]=0.0;}if((ATC>ATS)&&(ATC<=ATE)){tV[0]=-(Velo+AV*(ATC-ATS))*sin(tH);dtV[0]=-(Velo+AV*(ATC-ATS))*cos(tH)*dtH-AV*sin(tH);tV[1]=(Velo+AV*(ATC-ATS))*cos(tH);dtV[1]=-(Velo+AV*(ATC-ATS))*sin(tH)*dtH+AV*cos(tH);tV[2]=0.0;        dtV[2]=0.0;}if(ATC>ATE){tV[0]=-(Velo+AV*(ATE-ATS))*sin(tH);dtV[0]=-(Velo+AV*(ATE-ATS))*cos(tH)*dtH;tV[1]=(Velo+AV*(ATE-ATS))*cos(tH);dtV[1]=-(Velo+AV*(ATE-ATS))*sin(tH)*dtH;tV[2]=0.0;        dtV[2]=0.0;}ATC=ATC+sampletime;}else{        tV[0]=-Velo*sin(tH);    dtV[0]=-Velo*cos(tH)*dtH;    tV[1]=Velo*cos(tH);    dtV[1]=-Velo*sin(tH)*dtH;    tV[2]=0.0;    dtV[2]=0.0;}tlatit=tlatit+tV[1]*sampletime/tRn;tlongi=tlongi+tV[0]*sampletime/(tRe*cos(tlatit));*/}/*************陀螺仪数据发生函数***************/void out_Wib_b(){tWin_n[0]=-tV[1]/tRn;tWin_n[1]=Wie*cos(tlatit)+tV[0]/tRe;tWin_n[2]=Wie*sin(tlatit)+tV[0]*tan(tlatit)/tRe;tWin_b[0]=tCn2b[0][0]*tWin_n[0]+tCn2b[0][1]*tWin_n[1]+tCn2b[0][2]*tWin_n[2];tWin_b[1]=tCn2b[1][0]*tWin_n[0]+tCn2b[1][1]*tWin_n[1]+tCn2b[1][2]*tWin_n[2];tWin_b[2]=tCn2b[2][0]*tWin_n[0]+tCn2b[2][1]*tWin_n[1]+tCn2b[2][2]*tWin_n[2];tWnb_b[0]=-sin(tR)*cos(tP)*dtH+cos(tR)*dtP;tWnb_b[1]=sin(tP)*dtH+dtR;tWnb_b[2]=cos(tR)*cos(tP)*dtH+sin(tR)*dtP;for(int i=0;i<3;i++){tWib_b[i]=tWin_b[i]+tWnb_b[i];    Wib_b[i]=tWib_b[i]+D_gyro[i];//模拟陀螺仪输出，叠加了误差}}/**************船体坐标系相对导航坐标系的角速率在船体坐标系上的投影的计算函数**************/void cWnb_b(){Win_n[0]=-V[1]/Rn;Win_n[1]=Wie*cos(latit)+V[0]/Re;Win_n[2]=Wie*sin(latit)+V[0]*tan(latit)/Re;Win_b[0]=Cn2b[0][0]*Win_n[0]+Cn2b[0][1]*Win_n[1]+Cn2b[0][2]*Win_n[2];Win_b[1]=Cn2b[1][0]*Win_n[0]+Cn2b[1][1]*Win_n[1]+Cn2b[1][2]*Win_n[2];Win_b[2]=Cn2b[2][0]*Win_n[0]+Cn2b[2][1]*Win_n[1]+Cn2b[2][2]*Win_n[2];Wnb_b[0]=Wib_b[0]-Win_b[0];Wnb_b[1]=Wib_b[1]-Win_b[1];Wnb_b[2]=Wib_b[2]-Win_b[2];}void q_renew(){D_sita[1][0]=Wnb_b[0]*sampletime;D_sita[2][0]=Wnb_b[1]*sampletime;D_sita[3][0]=Wnb_b[2]*sampletime;D_sita[0][0]=0.0;D_sita[1][1]=0.0;D_sita[2][2]=0.0;D_sita[3][3]=0.0;D_sita[0][1]=-D_sita[1][0];D_sita[0][2]=-D_sita[2][0];D_sita[0][3]=-D_sita[3][0];D_sita[1][2]=D_sita[3][0];D_sita[1][3]=-D_sita[2][0];D_sita[2][1]=-D_sita[3][0];D_sita[2][3]=D_sita[1][0];D_sita[3][1]=D_sita[2][0];D_sita[3][2]=-D_sita[1][0];int i;vD_sita=0.0;for(i=1;i<=3;i++)vD_sita+=D_sita[i][0]*D_sita[i][0];vD_sita=sqrt(vD_sita);temp1=cos(vD_sita/2.0);if (vD_sita!=0.0)temp2=sin(vD_sita/2.0)/vD_sita;else temp2=0.5;q[0]=temp1*q_old[0]+temp2*D_sita[0][1]*q_old[1]+temp2*D_sita[0][2]*q_old[2]+temp2*D_sita[0][3]*q_old[3];q[1]=temp2*D_sita[1][0]*q_old[0]+temp1*q_old[1]+temp2*D_sita[1][2]*q_old[2]+temp2*D_sita[1][3]*q_old[3];q[2]=temp2*D_sita[2][0]*q_old[0]+temp2*D_sita[2][1]*q_old[1]+temp1*q_old[2]+temp2*D_sita[2][3]*q_old[3];q[3]=temp2*D_sita[3][0]*q_old[0]+temp2*D_sita[3][1]*q_old[1]+temp2*D_sita[3][2]*q_old[2]+temp1*q_old[3];temp1=0.0;////////////////四元数规范化处理for(i=0;i<=3;i++)temp1+=q[i]*q[i];temp1=sqrt(temp1);q[0]=q[0]/temp1;q[1]=q[1]/temp1;q[2]=q[2]/temp1;q[3]=q[3]/temp1;}/*************姿态矩阵和姿态角计算函数*************/void cCn2b(){Cn2b[0][0]=q[0]*q[0]+q[1]*q[1]-q[2]*q[2]-q[3]*q[3];Cn2b[0][1]=2.0*(q[1]*q[2]+q[0]*q[3]);Cn2b[0][2]=2.0*(q[1]*q[3]-q[0]*q[2]);Cn2b[1][0]=2.0*(q[1]*q[2]-q[0]*q[3]);Cn2b[1][1]=q[0]*q[0]-q[1]*q[1]+q[2]*q[2]-q[3]*q[3];Cn2b[1][2]=2.0*(q[2]*q[3]+q[0]*q[1]);Cn2b[2][0]=2.0*(q[1]*q[3]+q[0]*q[2]);Cn2b[2][1]=2.0*(q[2]*q[3]-q[0]*q[1]);Cn2b[2][2]=q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3];H=atan2(-Cn2b[1][0],Cn2b[1][1]);/////////////////注意负号；P=asin(Cn2b[1][2]);R=atan2(-Cn2b[0][2],Cn2b[2][2]);}//**************加速度计数据发生函数***************/void out_f(){   temp1=tV[1]/tRn;//dlatittemp2=2.0*Wie+tV[0]/(tRe*cos(tlatit));//2Wie+dlongitf_n[0]=dtV[0]-temp2*sin(tlatit)*tV[1]+temp2*cos(tlatit)*tV[2];//////////改了个符号tf_n[1]=dtV[1]+temp2*sin(tlatit)*tV[0]+temp1*tV[2];tf_n[2]=dtV[2]+g-temp2*cos(tlatit)*tV[0]-temp1*tV[1];    //比力由导航坐标系变换到船体坐标系tf_b[0]=tCn2b[0][0]*tf_n[0]+tCn2b[0][1]*tf_n[1]+tCn2b[0][2]*tf_n[2];tf_b[1]=tCn2b[1][0]*tf_n[0]+tCn2b[1][1]*tf_n[1]+tCn2b[1][2]*tf_n[2];tf_b[2]=tCn2b[2][0]*tf_n[0]+tCn2b[2][1]*tf_n[1]+tCn2b[2][2]*tf_n[2];for(int i=0;i<3;i++){f_b[i]=tf_b[i]+D_acce[i];//模拟加表输出，叠加了误差}}void renew(){f_n[0]=Cn2b[0][0]*f_b[0]+Cn2b[1][0]*f_b[1]+Cn2b[2][0]*f_b[2];f_n[1]=Cn2b[0][1]*f_b[0]+Cn2b[1][1]*f_b[1]+Cn2b[2][1]*f_b[2];f_n[2]=Cn2b[0][2]*f_b[0]+Cn2b[1][2]*f_b[1]+Cn2b[2][2]*f_b[2];Re=Ri*(e*sin(latit)*sin(latit)+1);Rn=Ri*(3.0*e*sin(latit)*sin(latit)-2.0*e+1);temp1=V[1]/Rn;temp2=2.0*Wie+V[0]/(Re*cos(latit));Vt[0]=V[0]+(f_n[0]+temp2*sin(latit)*V[1]-temp2*cos(latit)*V[2])*sampletime;Vt[1]=V[1]+(f_n[1]-temp2*sin(latit)*V[0]-temp1*V[2])*sampletime;Vt[2]=0.0;latit=latit+V[1]*sampletime/Rn;longi=longi+V[0]*sampletime/(Re*cos(latit));int i;for(i=0;i<3;i++)V[i]=Vt[i];for(i=0;i<4;i++)q_old[i]=q[i];}/**********************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;//+NkD_acce[1]=(acce_ran*noise+acce_con1)*g/1000.0;//+Nk+NkD_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",(H-tH)*H_d*60,(P-tP)*H_d*60,(R-tR)*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);}