strap_down_inertial_navigation.c

捷联系统C语言实现算法

#include "stdio.h"
#include "conio.h"
#include "stdlib.h"
#include "math.h"
#define pi  3.141592654
#define T   0.025         /*陀螺采样周期,SI units(s) ==================================*/
#define Tk  0.0125        /*加表采样周期,SI units(s)===================================*/
#define e   0.003352813   /*地球扁率,SI units(1)=======================================*/
#define Re  6378137.0     /*长半轴,SI units(m)=========================================*/
#define Wie 0.00007292115855   /*地球自转角速度,SI units(rad/s)=============================*/
double  C[3][3];          /*位置矩阵===================================================*/
double  P[4];             /*四元数=====================================================*/
double  TT[3][3];         /*捷联矩阵===================================================*/
double   faiG;            /*网格航向角,SI units(rad)===================================*/
double   theta;           /*俯仰角,SI units(rad)=======================================*/
double   gama;            /*倾斜角,SI units(rad)=======================================*/
double  Wibb[3][3];       /*陀螺输出,数组行为连续三个时刻采样,列为X,Y,Z方向输出,SI units(rad/s)*/
double  Wepp[3][2];       /*数组行为连续三个时刻采样,列为X,Y,Z方向输出,SI units(rad/s)=*/
double  Wpbb[3][3];
double  fb[7][3];         /*加速度计输出,数组行为连续三个时刻采样,列为X,Y,Z方向输出,SI units(m/s2)*/
double  fp[7][3];         /*转换到导航坐标系,SI units(m/s2)============================*/
double  VV1[3][3];        /*行为连续三个时刻速度，列为0东向E,1北向N,2天向速度U,SI units(m/s)*/
double  level_s[3];       /*水平速度,一次输出连续三个时刻,SI units(m/s)================*/
double  L;                /*纬度,SI units(rad)=========================================*/
double  lamed;            /*经度,SI units(rad)=========================================*/
double  alpha;            /*游移方位角,SI units(rad)===================================*/
double  h;                /*高度,SI units(m)===========================================*/
double  g=9.8;
/*{{-0.00015705,0.000051562,0.000051562},{-0.00015705,0.000051562,0.000051562},
{-0.00015705,0.000051562,0.000051562}};*/
/*{{0,0,9.8},{0,0,9.8},{0,0,9.8},{0,0,9.8},{0,0,9.8},{0,0,9.8},{0,0,9.8},
{0,0,9.8},{0,0,9.8}};*/
/*={{0,1000,0},{0,1000,0},{0,1000,0}} */


void initial(double *L,double *lamed,double *h ,double *alpha,double  TT[][3],
     double  P[4],double *faiG,double *theta,double *gama,double  C[][3],
     double  Wibb[][3],double  fb[][3])
{
     *L=pi/4;                   /*纬度      */
     *lamed=pi/4;               /*经度      */
     *h=0;                      /*高度      */
     /*初始速度。行为连续三个时刻速度，列为0东向E,1北向N,2天向速度U*/
     *alpha=0;                  /*游移方位角*/
     /*初始捷联矩阵----------------------------------------------------------*/
     TT[0][0]=(Wibb[0][2]*fb[0][1]-Wibb[0][1]*fb[0][2])/(g*Wie*cos(*L));
     TT[0][1]=(Wibb[0][2]*fb[0][2]-Wibb[0][2]*fb[0][0])/(g*Wie*cos(*L));
     TT[0][2]=(Wibb[0][1]*fb[0][0]-Wibb[0][0]*fb[0][1])/(g*Wie*cos(*L));
     TT[1][0]=fb[0][0]*tan(*L)/g+Wibb[0][0]/(cos(*L)*Wie);
     TT[1][1]=fb[0][1]*tan(*L)/g+Wibb[0][1]/(cos(*L)*Wie);
     TT[1][2]=fb[0][2]*tan(*L)/g+Wibb[0][2]/(cos(*L)*Wie);
     TT[2][0]=-fb[0][0]/g;
     TT[2][1]=-fb[0][1]/g;
     TT[2][2]=-fb[0][2]/g;
     /*初始姿态角------------------------------------------------------------*/
     *theta=asin(TT[2][1]);                 /*俯仰角*/
     *gama=atan(-(TT[2][0]/TT[2][2]));      /*gama主值倾斜角*/
     *faiG=atan(-(TT[0][1]/TT[1][1]));      /*fai主值网格航向角*/
     if((TT[2][2]<0)&&(*gama<0))          *gama=*gama+pi;
       else if((TT[2][2]<0)&&(*gama>0))   *gama=*gama-pi;
     if((TT[1][1]>0)&&(*faiG<0))          *faiG=*faiG+2*pi;
       else if(TT[1][1]<0)                *faiG=*faiG+pi;
     /*初始四元数计算--------------------------------------------------------*/
     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];
     /*位置矩阵初值----------------------------------------------------------*/
     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);
      /*--------------------------------------------------------------------*/
}
 /* VV1[3][3]连续三个时刻速度输出，3*3数组行为连续三个时刻采样，列为X,Y,Z方向输出。
                           位置矩阵在这三个时刻内不更新*/
void updateWepp(double C[3][3],double  Wepp[3][2],double  VV1[3][3])
{
double  Ryp,Rxp, ta;
   Ryp=(1-e*C[2][2]*C[2][2]+2*e*C[1][2]*C[1][2])/Re;       /*自由方位等效曲率半径，倒数*/
   Rxp=(1+2*e*C[0][2]*C[0][2]-e*C[2][2]*C[2][2])/Re;       /*自由方位等效曲率半径，倒数*/
    ta=2*e*C[0][2]*C[1][2]/Re;                             /*扭曲率，倒数              */
   Wepp[0][0]=(-ta*VV1[0][0])+(-Ryp*VV1[0][1]);
   Wepp[0][1]=Rxp*VV1[0][0]+ta*VV1[0][1];

   Wepp[1][0]=(-ta*VV1[1][0])+(-Ryp*VV1[1][1]);
   Wepp[1][1]=Rxp*VV1[1][0]+ta*VV1[1][1];

   Wepp[2][0]=(-ta*VV1[2][0])+(-Ryp*VV1[2][1]);
   Wepp[2][1]=Rxp*VV1[2][0]+ta*VV1[2][1];
}
void updateWpbb(double Wepp[3][2],double Wibb[3][3],double Wpbb[3][3],double TT[3][3])
{
double  Wipp[3][3];

        Wipp[0][0]= Wepp[0][0]+Wie*C[0][2];
        Wipp[0][1]= Wepp[0][1]+Wie*C[1][2];
        Wipp[0][2]= Wie*C[2][2];
        Wpbb[0][0]= Wibb[0][0]-(TT[0][0]*Wipp[0][0]+TT[1][0]*Wipp[0][1]+TT[2][0]*Wipp[0][2]);
        Wpbb[0][1]= Wibb[0][1]-(TT[0][1]*Wipp[0][0]+TT[1][1]*Wipp[0][1]+TT[2][1]*Wipp[0][2]);
        Wpbb[0][2]= Wibb[0][2]-(TT[0][2]*Wipp[0][0]+TT[1][2]*Wipp[0][1]+TT[2][2]*Wipp[0][2]);

        Wipp[1][0]= Wepp[1][0]+Wie*C[0][2];
        Wipp[1][1]= Wepp[1][1]+Wie*C[1][2];
        Wipp[1][2]= Wie*C[2][2];
        Wpbb[1][0]= Wibb[1][0]-(TT[0][0]*Wipp[1][0]+TT[1][0]*Wipp[1][1]+TT[2][0]*Wipp[1][2]);
        Wpbb[1][1]= Wibb[1][1]-(TT[0][1]*Wipp[1][0]+TT[1][1]*Wipp[1][1]+TT[2][1]*Wipp[1][2]);
        Wpbb[1][2]= Wibb[1][2]-(TT[0][2]*Wipp[1][0]+TT[1][2]*Wipp[1][1]+TT[2][2]*Wipp[1][2]);

        Wipp[2][0]= Wepp[2][0]+Wie*C[0][2];
        Wipp[2][1]= Wepp[2][1]+Wie*C[1][2];
        Wipp[2][2]= Wie*C[2][2];
        Wpbb[2][0]= Wibb[2][0]-(TT[0][0]*Wipp[2][0]+TT[1][0]*Wipp[2][1]+TT[2][0]*Wipp[2][2]);
        Wpbb[2][1]= Wibb[2][1]-(TT[0][1]*Wipp[2][0]+TT[1][1]*Wipp[2][1]+TT[2][1]*Wipp[2][2]);
        Wpbb[2][2]= Wibb[2][2]-(TT[0][2]*Wipp[2][0]+TT[1][2]*Wipp[2][1]+TT[2][2]*Wipp[2][2]);
}
  /*Wpbb[3][3]为机体坐标系相对于导航坐标系在机体坐标系上的投
      影3*3数组行为连续三个时刻采样，列为X,Y,Z方向陀螺经补偿输出。
                  T[3][3]为捷联矩阵。P[4]为四元数向量。T为采样周期*/
void updateP(double Wpbb[3][3],double TT[3][3],double P[4],double T1)
{
 double mo;
 double K1[4],K2[4],K3[4],K4[4];
                /*四阶龙格库塔法解四元数微分方程*/
   K1[0]=(-Wpbb[0][0]*P[1]-Wpbb[0][1]*P[2]-Wpbb[0][2]*P[3])/2;
   K1[1]=( Wpbb[0][0]*P[0]+Wpbb[0][2]*P[2]-Wpbb[0][1]*P[3])/2;
   K1[2]=( Wpbb[0][1]*P[0]-Wpbb[0][2]*P[1]-Wpbb[0][0]*P[3])/2;
   K1[3]=( Wpbb[0][2]*P[0]+Wpbb[0][1]*P[1]-Wpbb[0][0]*P[2])/2;

   K2[0]=P[0]+K1[0]*T1/2;
   K2[1]=P[1]+K1[1]*T1/2;
   K2[2]=P[2]+K1[2]*T1/2;
   K2[3]=P[3]+K1[3]*T1/2;

   K2[0]=(-Wpbb[1][0]*K2[1]-Wpbb[1][1]*K2[2]-Wpbb[1][2]*K2[3])/2;
   K2[1]=( Wpbb[1][0]*K2[0]+Wpbb[1][2]*K2[2]-Wpbb[1][1]*K2[3])/2;
   K2[2]=( Wpbb[1][1]*K2[0]-Wpbb[1][2]*K2[1]-Wpbb[1][0]*K2[3])/2;
   K2[3]=( Wpbb[1][2]*K2[0]+Wpbb[1][1]*K2[1]-Wpbb[1][0]*K2[2])/2;

   K3[0]=P[0]+K2[0]*T1/2;
   K3[1]=P[1]+K2[1]*T1/2;
   K3[2]=P[2]+K2[2]*T1/2;
   K3[3]=P[3]+K2[3]*T1/2;

   K3[0]=(-Wpbb[1][0]*K3[1]-Wpbb[1][1]*K3[2]-Wpbb[1][2]*K3[3])/2;
   K3[1]=( Wpbb[1][0]*K3[0]+Wpbb[1][2]*K3[2]-Wpbb[1][1]*K3[3])/2;
   K3[2]=( Wpbb[1][1]*K3[0]-Wpbb[1][2]*K3[1]-Wpbb[1][0]*K3[3])/2;
   K3[3]=( Wpbb[1][2]*K3[0]+Wpbb[1][1]*K3[1]-Wpbb[1][0]*K3[2])/2;

   K4[0]=P[0]+K3[0]*T1;
   K4[1]=P[1]+K3[1]*T1;
   K4[2]=P[2]+K3[2]*T1;
   K4[3]=P[3]+K3[3]*T1;

   K4[0]=(-Wpbb[2][0]*K4[1]-Wpbb[2][1]*K4[2]-Wpbb[2][2]*K4[3])/2;
   K4[1]=( Wpbb[2][0]*K4[0]+Wpbb[2][2]*K4[2]-Wpbb[2][1]*K4[3])/2;
   K4[2]=( Wpbb[2][1]*K4[0]-Wpbb[2][2]*K4[1]-Wpbb[2][0]*K4[3])/2;
   K4[3]=( Wpbb[2][2]*K4[0]+Wpbb[2][1]*K4[1]-Wpbb[2][0]*K4[2])/2;

   P[0]=P[0]+(K1[0]+2*K2[0]+2*K3[0]+K4[0])*T1/6;
   P[1]=P[1]+(K1[1]+2*K2[1]+2*K3[1]+K4[1])*T1/6;
   P[2]=P[2]+(K1[2]+2*K2[2]+2*K3[2]+K4[2])*T1/6;
   P[3]=P[3]+(K1[3]+2*K2[3]+2*K3[3]+K4[3])*T1/6;
                  /*归一化*/
   mo=sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]+P[3]*P[3]);
   P[0]=P[0]/mo;
   P[1]=P[1]/mo;
   P[2]=P[2]/mo;
   P[3]=P[3]/mo;
               /*更新捷联矩阵*/
   TT[0][0]=P[0]*P[0]+P[1]*P[1]-P[2]*P[2]-P[3]*P[3];
   TT[0][1]=(P[1]*P[2]-P[0]*P[3])*2;
   TT[0][2]=(P[1]*P[3]+P[0]*P[2])*2;
   TT[1][0]=(P[1]*P[2]+P[0]*P[3])*2;
   TT[1][1]=P[0]*P[0]-P[1]*P[1]+P[2]*P[2]-P[3]*P[3];
   TT[1][2]=(P[2]*P[3]-P[0]*P[1])*2;
   TT[2][0]=(P[1]*P[3]-P[0]*P[2])*2;
   TT[2][1]=(P[2]*P[3]+P[0]*P[1])*2;
   TT[2][2]=P[0]*P[0]-P[1]*P[1]-P[2]*P[2]+P[3]*P[3];
}
void updatezitai(double TT[][3],double *faiG,double *theta,double *gama)   /*姿态角更新*/
{

   *theta=asin(TT[2][1]);                 /*俯仰角*/
   *gama=atan(-(TT[2][0]/TT[2][2]));      /*gama主值倾斜角*/
   *faiG=atan(-(TT[0][1]/TT[1][1]));      /*fai主值网格航向角*/
   
   if((TT[2][2]<0)&&(*gama<0))         *gama=*gama+pi;
     else if((TT[2][2]<0)&&(*gama>0))  *gama=*gama-pi;

   if((TT[1][1]>0)&&(*faiG<0))         *faiG=*faiG+2*pi;
     else if(TT[1][1]<0)               *faiG=*faiG+pi;
   *theta=*theta*180/pi;
   *gama=*gama*180/pi;
   *faiG=*faiG*180/pi;
}
            /*将加速度计输出转换到导航坐标系中*/
void fbbbb(double  fb[7][3],double  fp[7][3],double  TT[3][3])
{int i=0;

   for(i=0;i<7;i++)
      {
       fp[i][0]=TT[0][0]*fb[i][0]+TT[0][1]*fb[i][1]+TT[0][2]*fb[i][2];
       fp[i][1]=TT[1][0]*fb[i][0]+TT[1][1]*fb[i][1]+TT[1][2]*fb[i][2];
       fp[i][2]=TT[2][0]*fb[i][0]+TT[2][1]*fb[i][1]+TT[2][2]*fb[i][2];
      }

 }
                  /*水平速度四阶龙格库塔算法*/
/*其中C数组为位置矩阵，angularv为3*3数组行为连续三个时刻采样，
列为X,Y,Z方向加速度计经坐标转换输出。speed[3]为上一时刻T为采样周期*/
void level_speed(double fp[7][3],double VV1[3][3],double C[3][3],double level_s[3],double T1)
{
 double K1[6],K2[6],K3[6],K4[6];

   K1[0]= 2*Wie*C[2][2]*VV1[0][1]+fp[0][0];
   K1[1]=-2*Wie*C[2][2]*VV1[0][0]+fp[0][1];

   K2[0]=VV1[0][0]+K1[0]*T1/2;
   K2[1]=VV1[0][1]+K1[1]*T1/2;

   K2[0]= 2*Wie*C[2][2]*K2[1]+fp[0+1][0];
   K2[1]=-2*Wie*C[2][2]*K2[0]+fp[0+1][1];

   K3[0]=VV1[0][0]+K2[0]*T1/2;
   K3[1]=VV1[0][1]+K2[1]*T1/2;

   K3[0]= 2*Wie*C[2][2]*K3[1]+fp[0+1][0];
   K3[1]=-2*Wie*C[2][2]*K3[0]+fp[0+1][1];

   K4[0]=VV1[0][0]+K3[0]*T1;
   K4[1]=VV1[0][1]+K3[1]*T1;

   K4[0]= 2*Wie*C[2][2]*K4[1]+fp[0+2][0];
   K4[1]=-2*Wie*C[2][2]*K4[0]+fp[0+2][1];

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

   K2[0]=VV1[1][0]+K1[0]*T1/2;
   K2[1]=VV1[1][1]+K1[1]*T1/2;

   K2[0]= 2*Wie*C[2][2]*K2[1]+fp[2+1][0];
   K2[1]=-2*Wie*C[2][2]*K2[0]+fp[2+1][1];

   K3[0]=VV1[1][0]+K2[0]*T1/2;
   K3[1]=VV1[1][1]+K2[1]*T1/2;

   K3[0]= 2*Wie*C[2][2]*K3[1]+fp[2+1][0];
   K3[1]=-2*Wie*C[2][2]*K3[0]+fp[2+1][1];

   K4[0]=VV1[1][0]+K3[0]*T1;
   K4[1]=VV1[1][1]+K3[1]*T1;

   K4[0]= 2*Wie*C[2][2]*K4[1]+fp[2+2][0];
   K4[1]=-2*Wie*C[2][2]*K4[0]+fp[2+2][1];

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

   K2[0]=VV1[2][0]+K1[0]*T1/2;
   K2[1]=VV1[2][1]+K1[1]*T1/2;

   K2[0]= 2*Wie*C[2][2]*K2[1]+fp[5][0];
   K2[1]=-2*Wie*C[2][2]*K2[0]+fp[5][1];

   K3[0]=VV1[2][0]+K2[0]*T1/2;
   K3[1]=VV1[2][1]+K2[1]*T1/2;

   K3[0]= 2*Wie*C[2][2]*K3[1]+fp[5][0];
   K3[1]=-2*Wie*C[2][2]*K3[0]+fp[5][1];

   K4[0]=VV1[2][0]+K3[0]*T1;
   K4[1]=VV1[2][1]+K3[1]*T1;

   K4[0]= 2*Wie*C[2][2]*K4[1]+fp[6][0];
   K4[1]=-2*Wie*C[2][2]*K4[0]+fp[6][1];

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