strap_down_inertial_navigation.c

捷联系统C语言实现算法

   VV1[2][1]=VV1[2][1]+(K1[1]+2*K2[1]+2*K3[1]+K4[1])*T1/6;
   level_s[2]=sqrt(VV1[2][0]*VV1[2][0]+VV1[2][1]*VV1[2][1]);

}


/*其中C数组为位置矩阵，Wepp为3*2数组行为连续三个时刻采样，列为X,Y,Z方向陀螺输出。T为采样周期*/
void position(double C[3][3],double Wepp[3][2],double T1)
{
 double K1[6],K2[6],K3[6],K4[6];

   K1[0]=-Wepp[0][1]*C[2][1];
   K1[1]=-Wepp[0][1]*C[2][2];
   K1[2]=Wepp[0][0]*C[2][1];
   K1[3]=Wepp[0][0]*C[2][2];
   K1[4]=Wepp[0][1]*C[0][1]-Wepp[0][0]*C[1][1];
   K1[5]=Wepp[0][1]*C[0][2]-Wepp[0][0]*C[1][2];

   K2[0]=C[0][1]+K1[0]*T1/2;
   K2[1]=C[0][2]+K1[1]*T1/2;
   K2[2]=C[1][1]+K1[2]*T1/2;
   K2[3]=C[1][2]+K1[3]*T1/2;
   K2[4]=C[2][1]+K1[4]*T1/2;
   K2[5]=C[2][2]+K1[5]*T1/2;
   K2[0]=-Wepp[1][1]*K2[4];
   K2[1]=-Wepp[1][1]*K2[5];
   K2[2]=Wepp[1][0]*K2[4];
   K2[3]=Wepp[1][0]*K2[5];
   K2[4]=Wepp[1][1]*K2[0]-Wepp[1][0]*K2[2];
   K2[5]=Wepp[1][1]*K2[1]-Wepp[1][0]*K2[3];

   K3[0]=C[0][1]+K2[0]*T1/2;
   K3[1]=C[0][2]+K2[1]*T1/2;
   K3[2]=C[1][1]+K2[2]*T1/2;
   K3[3]=C[1][2]+K2[3]*T1/2;
   K3[4]=C[2][1]+K2[4]*T1/2;
   K3[5]=C[2][2]+K2[5]*T1/2;
   K3[0]=-Wepp[1][1]*K3[4];
   K3[1]=-Wepp[1][1]*K3[5];
   K3[2]=Wepp[1][0]*K3[4];
   K3[3]=Wepp[1][0]*K3[5];
   K3[4]=Wepp[1][1]*K3[0]-Wepp[1][0]*K3[2];
   K3[5]=Wepp[1][1]*K3[1]-Wepp[1][0]*K3[3];

   K4[0]=C[0][1]+K3[0]*T1;
   K4[1]=C[0][2]+K3[1]*T1;
   K4[2]=C[1][1]+K3[2]*T1;
   K4[3]=C[1][2]+K3[3]*T1;
   K4[4]=C[2][1]+K3[4]*T1;
   K4[5]=C[2][2]+K3[5]*T1;
   K4[0]=-Wepp[2][1]*K4[4];
   K4[1]=-Wepp[2][1]*K4[5];
   K4[2]=Wepp[2][0]*K4[4];
   K4[3]=Wepp[2][0]*K4[5];
   K4[4]=Wepp[2][1]*K4[0]-Wepp[2][0]*K4[2];
   K4[5]=Wepp[2][1]*K4[1]-Wepp[2][0]*K4[3];

   C[0][1]=C[0][1]+(K1[0]+2*K2[0]+2*K3[0]+K4[0])*T1/6;
   C[0][2]=C[0][2]+(K1[1]+2*K2[1]+2*K3[1]+K4[1])*T1/6;
   C[1][1]=C[1][1]+(K1[2]+2*K2[2]+2*K3[2]+K4[2])*T1/6;
   C[1][2]=C[1][2]+(K1[3]+2*K2[3]+2*K3[3]+K4[3])*T1/6;
   C[2][1]=C[2][1]+(K1[4]+2*K2[4]+2*K3[4]+K4[4])*T1/6;
   C[2][2]=C[2][2]+(K1[5]+2*K2[5]+2*K3[5]+K4[5])*T1/6;
   C[0][0]=C[1][1]*C[2][2]-C[1][2]*C[2][1];
   C[1][0]=C[0][1]*C[2][2]-C[0][2]*C[2][1];
   C[2][0]=C[0][1]*C[1][2]-C[1][1]*C[0][2];
} 
void updateposizion(double C[3][3],double *L,double *lamed,double *alpha,double *faiG)   /*位置更新*/
{
   *L=asin(C[2][2])*180/pi;                 /*纬度*/
   *lamed=atan(C[2][1]/C[2][0]);     /*经度主值*/
   *alpha=atan(C[0][2]/C[1][2]);       /*游移方位角主值*/
   if(C[2][0]>0)                       *lamed=*lamed*180/pi;
     else if((C[2][0]<0)&&(*lamed<0))  *lamed=(*lamed*180)/pi+180;
     else if((C[2][0]<0)&&(*lamed>0))  *lamed=(*lamed*180)/pi-180;
   if(C[1][2]<0)                       *alpha=*alpha*180/pi+180;
     else if((C[1][2]>0)&&(*alpha>0))  *alpha=*alpha*180/pi;
     else if((C[1][2]>0)&&(*alpha<0))  *alpha=*alpha*180/pi+360;
   *faiG=*faiG+*alpha;
   if(*faiG>360) *faiG=*faiG-360;
}
main()
{
/*+++++++++++++++++++++++++数据采集++++++++++++++++++++++++++++++++++*/
/*将文件中第一个数赋给Wibb[0][0]，Wibb[0][1]，Wibb[0][2]，fb[0][0]，fb[0][1]，fb[0][2]，
角速度采样顺序为0-1-2-2-3-4-4-5-6-6-7-8。加速度速度采样顺序为0-1-2-3-4-5-6-6-7-8-9-10-11-12
-12-13-14-15-16-17-18*/
int a=0;
int b=0;
int k=0;
double Wibb[3][3],fb[7][3];
FILE  *fpwx,
      *fpwy,
      *fpwz,
      *fpfx,
      *fpfy,
      *fpfz,
      *cfPtr,
      *fpL,
      *fplamed,
      *fpalpha,
      *fpfaiG,
      *fptheta,
      *fpgama,
      *fplevels;
char wx[25], wy[25],  wz[25], ffx[25], ffy[25], ffz[25];
clrscr();


   /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
 L=1.78539816;
 lamed=2.78539816;
 h=3;
 theta=1;
 gama=1;
 faiG=1;
 alpha=0;
 /*位置矩阵初值----------------------------------------------------------*/
     C[0][0]=-sin(alpha)*sin(L)*cos(lamed)-cos(alpha)*sin(lamed);
     C[1][0]=-cos(alpha)*sin(L)*cos(lamed)+sin(alpha)*sin(lamed);
     C[2][0]=cos(L)*cos(lamed);
     C[0][1]=-sin(alpha)*sin(L)*sin(lamed)+cos(alpha)*cos(lamed);
     C[1][1]=-cos(alpha)*sin(L)*sin(lamed)-sin(alpha)*cos(lamed);
     C[2][1]=cos(L)*sin(lamed);
     C[0][2]=sin(alpha)*cos(L);
     C[1][2]=cos(alpha)*cos(L);
     C[2][2]=sin(L);
/*初始捷联矩阵----------------------------------------------------------*/
     TT[0][0]=cos(gama)*cos(faiG)-sin(gama)*sin(theta)*sin(faiG);
     TT[0][1]=-cos(theta)*sin(faiG);
     TT[0][2]=sin(gama)*cos(faiG)+cos(gama)*sin(theta)*sin(faiG);
     TT[1][0]=cos(gama)*sin(faiG)+sin(gama)*sin(theta)*cos(faiG);
     TT[1][1]=cos(theta)*cos(faiG);
     TT[1][2]=sin(gama)*sin(faiG)-cos(gama)*sin(theta)*cos(faiG);
     TT[2][0]=-sin(gama)*cos(theta);
     TT[2][1]= sin(theta);
     TT[2][2]=cos(gama)*cos(theta);
/*初始四元数计算--------------------------------------------------------*/
     P[1]=sqrt(1+TT[0][0]-TT[1][1]-TT[2][2])/2;
     P[2]=sqrt(1-TT[0][0]+TT[1][1]-TT[2][2])/2;
     P[3]=sqrt(1-TT[0][0]-TT[1][1]+TT[2][2])/2;
     P[0]=(1-P[1]*P[1]-P[2]*P[2]-P[3]*P[3]);
     if((TT[2][1]-TT[1][2])<0)   P[1]=-P[1];
     if((TT[0][2]-TT[2][0])<0)   P[2]=-P[2];
     if((TT[1][0]-TT[0][1])<0)   P[3]=-P[3];
/*初始速度--------------------------------------------------------*/
 VV1[3][3]=0;


/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
 
 if((fpwx=fopen ("d:\\WWx.dat","r"))==NULL)
   {printf("cannot open fileWWx\n");getch();exit(0);}
 if((fpwy=fopen ("d:\\WWy.dat","r"))==NULL)
   {printf("cannot open fileWWy\n");getch();exit(0);}
 if((fpwz=fopen ("d:\\WWz.dat","r"))==NULL)
   {printf("cannot open fileWWz\n");getch();exit(0);}
 if((fpfx=fopen ("d:\\fbx.dat","r"))==NULL)
   {printf("cannot open filefx\n");getch();exit(0);}
 if((fpfy=fopen ("d:\\fby.dat","r"))==NULL)
   {printf("cannot open filefy\n");getch();exit(0);}
 if((fpfz=fopen ("d:\\fbz.dat","r"))==NULL)
   {printf("cannot open filefz\n");getch();exit(0);}


 
if((fpL=fopen("d:\\L.dat","w"))==NULL)
{
printf("Cannot open file strike any key exit!");
getch();
exit(1);
}
if((fplevels=fopen("d:\\levels.dat","w"))==NULL)
{
printf("Cannot open file strike any key exit!");
getch();
exit(1);
}
for(k=0;k<1000;k++)
  {
   if(k==0)
   {
    fgets(wx,25,fpwx);Wibb[0][0]=atof(wx);
    fgets(wy,25,fpwy);Wibb[0][1]=atof(wy);
    fgets(wz,25,fpwz);Wibb[0][2]=atof(wz);
    fgets(ffx,25,fpfx);fb[0][0]=atof(ffx);
    fgets(ffy,25,fpfy);fb[0][1]=atof(ffy);
    fgets(ffz,20,fpfz);fb[0][2]=atof(ffz);
    }
 else {Wibb[0][0]=Wibb[2][0];Wibb[0][1]=Wibb[2][1];Wibb[0][2]=Wibb[2][2];
         fb[0][0]=  fb[6][0];  fb[0][1]=  fb[6][1];  fb[0][2]=  fb[6][2];}
          fgets(wx,25,fpwx);Wibb[1][0]=atof(wx);
          fgets(wy,25,fpwy);Wibb[1][1]=atof(wy);
          fgets(wz,25,fpwz);Wibb[1][2]=atof(wz);
          fgets(wx,25,fpwx);Wibb[2][0]=atof(wx);
          fgets(wy,25,fpwy);Wibb[2][1]=atof(wy);
          fgets(wz,25,fpwz);Wibb[2][2]=atof(wz);
          fgets(ffx,25,fpfx);fb[1][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[1][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[1][2]=atof(ffz);
          fgets(ffx,25,fpfx);fb[2][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[2][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[2][2]=atof(ffz);
          fgets(ffx,25,fpfx);fb[3][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[3][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[3][2]=atof(ffz);
          fgets(ffx,25,fpfx);fb[4][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[4][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[4][2]=atof(ffz);
          fgets(ffx,25,fpfx);fb[5][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[5][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[5][2]=atof(ffz);
          fgets(ffx,25,fpfx);fb[6][0]=atof(ffx);
          fgets(ffy,25,fpfy);fb[6][1]=atof(ffy);
          fgets(ffz,25,fpfz);fb[6][2]=atof(ffz);

/*++++++++++++++此时Wibb[3][3],fb[7][3]已经装入采样值+++++++++++++++++++*/
/*+++++++++++++++++++++++++++++捷联解算+++++++++++++++++++++++++++++++++*/
/*假设已经采来连续三个时刻,陀螺和加速度计的输出Wibb[3][3],fb[7][3]并且所有初值已经给定.*/
   /* initial(&L,&lamed,&h ,&alpha,TT,P,&faiG,&theta,&gama,C,Wibb,fb);初始化 */
   updateWepp(C,Wepp,VV1);             /*更新Wepp[3][2]*/

    updateWpbb(Wepp,Wibb,Wpbb,TT);      /*更新Wpbb[3][2]*/
/*    for(a=0;a<3;a++){
    for(b=0;b<3;b++) {
    printf("%f",Wpbb[a][b]);
    }
                     } */ 
    updateP(Wpbb,TT,P,T);             /*更新四元数*/

/*    for(b=0;b<3;b++)
    printf("%f,",P[b]); */
   updatezitai(TT,&faiG,&theta,&gama); /*姿态角更新*/
 /*printf("faiG=%f,",faiG);
 printf("theta=%f,",theta);
 printf("gama=%f,",gama); */

    fbbbb(fb,fp,TT);                   /*将6个时刻的加速度输出采样值转换到导航坐标系中*/

    level_speed(fp,VV1,C,level_s,Tk);   /*计算速度，需要6个时刻的加速度采样值*/
/*for(b=0;b<3;b++)
    printf("level_s=%f\n",level_s[b]); */
    updateWepp(C,Wepp,VV1);             /*更新Wepp[3][2]*/

    position(C,Wepp,T);                /*位置矩阵四阶龙格库塔算法*/

/*    for(a=0;a<3;a++){
    for(b=0;b<3;b++) {
    printf("%f",C[a][b]);
    }
                     }  */
 updateposizion(C,&L,&lamed,&alpha,&faiG); /*位置更新*/
 printf("L=%f,",L);
 /*printf("lamed=%f,",lamed);  */
/* printf("faiG=%f,",faiG);    */
/*++++++++++++++++将结果写入文件++++++++++++++++++++++++++++++++++++++++++++++*/
/*FILE *cfPtr,*fpL,*fplamed,*fpalpha,*fpfaiG,*fptheta*,*fpgama,*fplevels; */
L=L*pi/180;
fprintf(fpL,"%.8f\n",L);
for(a=0;a<3;a++){
fprintf(fplevels,"%.8f\n",level_s[a]); }

}
fclose(fpwx);  /*关闭int文件*/
fclose(fpwy);  /*关闭int文件*/
fclose(fpwz);  /*关闭int文件*/
fclose(fpfx);  /*关闭int文件*/
fclose(fpfy);  /*关闭int文件*/
fclose(fpfz);  /*关闭int文件*/
fclose(fpL);   /*关闭out文件*/
fclose(fplevels);   /*关闭out文件*/
getch ();
}
/*if((cfPtr=fopen("d:\\fb.dat","a"))==NULL){
printf ("Cannot open input file.\n");
printf ("Press any key to quit.\n"); 
getch (); 
exit (0);
} 


for(j=0;j<7;j++){
   printf("%d.fb=%.8f\n", j,fb[j][2]);
   fprintf(cfPtr,"%.8f\n",fb[j][2]);
                }
fclose (cfPtr); */

/*FILE *cfPtr;
if((cfPtr=fopen("D:\L.dat","a"))==NULL)
   printf("File could not de opened\n");
   else{
    fscanf(cfPtr,"%.8f,",&L);       
    }*/