jielian_hao.cpp

最近刚编的C语言的捷联程序。参照陈哲的《惯性系统导航原理》。

	      	T[2][2]=Q[0]*Q[0]-Q[1]*Q[1]-Q[2]*Q[2]+Q[3]*Q[3];
	   }
	   
	   //加速度的坐标转换
	  void convacce(double T[3][3],double fp[3],double fb[3])
	   {
	   		   	
	   	 mulit_MAT_VEC2(fp,T,fb);
	   	}
	   	
	   	//速度Vp的即时修正
	 void updateVp(double fp[3],double Wiep[3],double Wepp[3],double V[3],double t2)
	   	{
	   		double KV[3];
	   		double Init_V[3];		
	   		double g[3];
	   		double WW[3][3];
	   		double WV[3];
	        double GWV[3];
	   	    double t2KV[3];
	        t2=0.04;
		
	   		WW[0][0]=0;
	   		WW[0][1]=2*Wiep[2];
	   		WW[0][2]=-(2*Wiep[1]+Wepp[1]);
	   		WW[1][0]=-2*Wiep[2];
	   		WW[1][1]=0;	   		
	   		WW[1][2]=2*Wiep[0]+Wepp[0];
	   		WW[2][0]=2*Wiep[1]+Wepp[1];
	   		WW[2][1]=-2*Wiep[0]+Wepp[0];
	   		WW[2][2]=0;
	   		
	   		g[0]=0;
	   		g[1]=0;
	   		g[2]=9.8;
	   		
			Init_V[0]=V[0];
			Init_V[1]=V[1];
			Init_V[2]=V[2];

			//一阶欧拉法

	   		mulit_MAT_VEC2(WV,WW,Init_V);
	   		add_VECTOR1(GWV,g,WV);
	   		sub_VECTOR1(KV,fp,GWV); 
	   		mulit_NUM_VEC2(t2KV,t2,KV);
	   		add_VECTOR1(V,t2KV,Init_V);
	   	
	   	}
 
	   		//位置速率的计算
	   		void calaWEPP(double C[3][3], double V[2],double WEPP[2])
    { 

       	      double conv_wepp[2][2];
    	       conv_wepp[0][0]=-2*e*dRe*C[0][2]*C[1][2];
    	       
    	       conv_wepp[0][1]=dRe*(1-e*C[2][2]*C[2][2]+2*e*C[1][2]*C[1][2]);
    	       conv_wepp[1][0]=dRe*(1-e*C[2][2]*C[2][2]+2*e*C[0][2]*C[0][2]);
    	       conv_wepp[1][1]=2*e*dRe*C[0][2]*C[1][2];
    	       
    	       mulit_MAT_VEC1(WEPP,conv_wepp,V);
    }
    //位置矩阵C的即时修正
    void updateC(double C[3][3],double WEPP[2],double t2)
    {
    	double WEPP0[3][3];
        double KC[3][3];
        double t2KC[3][3];
        double Init_C[3][3];
    	
    	WEPP0[0][0]=0;
	   	WEPP0[0][1]=0;	
	    WEPP0[0][2]=-WEPP[1];
	    WEPP0[1][0]=0;
	    WEPP0[1][1]=0;
	    WEPP0[1][2]=WEPP[0];
	    WEPP0[2][0]=WEPP[1];
	    WEPP0[2][1]=-WEPP[0];
	    WEPP0[2][2]=0;

   	//一阶欧拉法
	     Init_C[0][0]=C[0][0];
         Init_C[0][1]=C[0][1];
         Init_C[0][2]=C[0][2];
		 Init_C[1][0]=C[1][0];
		 Init_C[1][1]=C[1][1];
		 Init_C[1][2]=C[1][2];
		 Init_C[2][0]=C[2][0];
		 Init_C[2][1]=C[2][1];
		 Init_C[2][2]=C[2][2];

		 mulit_MATRIX(KC,WEPP0,Init_C);
	     mulit_NUM_MAT1(t2KC,t2,KC);
	     add_MATRIX(C,t2KC,Init_C);
	   
	    
	  }
	  //地球速率的计算
	  void calaWIEP(double Wiep[3],double C[3][3],double Wie[3])
	  {
	 	  	
	  	Wie[0]=0;
	  	Wie[1]=0;
	  	Wie[2]=WIE;
	  	
	  	mulit_MAT_VEC2(Wiep,C,Wie);
	  }
	  	  
    //位置计算
    
  void calapos(double POS[3],double C[3][3])
    {
    	double fai,lamda,alpha;
     	 	
        fai=asin(C[2][2]);
    	lamda=atan(C[2][1]/C[2][0]);
    	alpha=atan(C[0][2]/C[1][2]);
    	
    	POS[0]=fai;
    	
    	if (C[2][0]>0)
    		POS[1]=lamda;
    		else
    			{if (lamda<0)
    				  POS[1]=lamda+PI;
    			else 
    				  POS[1]=lamda-PI;
    				}
    			
    	if (C[1][2]<0)
    		POS[2]=alpha+PI;
       	else
       		{
       			if (alpha>0)
       			    POS[2]=alpha;
       			else
       			    POS[2]=alpha+2*PI;
       		}
       	}
       	//姿态计算
       void calczitai(double Angl[3],double T[3][3],double alpha)
    {
       
    	double thta,gama,pusai0=0,pusaiG,pusai;
		
        thta=asin(T[2][1]);
    	gama=atan(-T[2][0]/T[2][2]);
    	pusai=atan(-T[0][1]/T[1][1]);
    	
    	Angl[0]=thta;
    	
    	if (T[2][2]>=0)
    		Angl[1]=gama;
    		else
    			{if (gama<0)
    				  Angl[1]=gama+PI;
    			else 
    				  Angl[1]=gama-PI;
    				}
    			
    	if (T[1][1]<0)
    		pusaiG=pusai0+PI;
       	else
       		{
       			if (pusai0>=0)
       			  pusaiG=pusai0;
       			else
       			  pusaiG=pusai0+2*PI;
       		}
       		
       		pusai=pusaiG+alpha;
       		if(pusai<2*PI)
       		   Angl[2]=pusai;
       		else
       		Angl[2]=pusai-2*PI;
       	}

	   void Initial(double C[3][3],double T[3][3],double Q[4],double POS[3],double V[2],double Angl[3],double Wiep[3],double WEPP[2])
{	
   
       //POS[0]=ALPHA0;POS[1]=LAT0;POS[2]=LONG0;
   double h=0;//初始化高度为0；       
   double Wie[3];
   
   T[0][0]=T[1][1]=T[2][2]=1;
   T[0][1]=T[0][2]=T[1][0]=T[1][2]=T[2][0]=T[2][1]=0;//初始化T矩阵；
   V[0]=V[1]=0;//初始化速度为0；
   POS[0]=0;//初始游动方位角为0；
   POS[1]=45*RAD;//初始纬度为45度；
   POS[2]=126*RAD;//初始经度为126度；
   
   //INITTMAT();
   calczitai(Angl,T,0);//初始化姿态角；
   INITQ(T,Q);//初始化四元数；
   INITCMAT(C,POS);//初始化C矩阵；
   calaWIEP(Wiep,C,Wie);//初始化地球速率;
   calaWEPP(C,V,WEPP);//初始化位置速率WEPP;
   calcG(C,h);//初始化重力加速度；
   calcV(V);//初始化地速V;
  }


	   //矩阵与向量乘法
//矩阵a为2行2列,矩阵b为2行1列,相乘得到矩阵mulit为2行1列。
 void mulit_MAT_VEC1(double mulit[2],double a[2][2],double b[2])
{
   int i,j;
   
      for (i=0;i<2;i++)
       {
       	mulit[i]=0;
       	for(j=0;j<2;j++)
       	    {                  
             mulit[i]+=a[i][j]*b[j];
            }
       }
 }
  //矩阵与向量乘法
//矩阵a为3行3列,矩阵b为3行1列,相乘得到矩阵mulit为3行1列。
 void mulit_MAT_VEC2(double mulit[3],double a[3][3],double b[3])
{
   int i,j;
   
      for (i=0;i<3;i++)
       {
       	mulit[i]=0;
       	for(j=0;j<3;j++)
       	    {                  
             mulit[i]+=a[i][j]*b[j];
            }
       }
 }
 //矩阵与向量乘法
//矩阵a为4行4列,矩阵b为4行1列,相乘得到矩阵mulit为4行1列。
 void mulit_MAT_VEC3(double mulit[4],double a[4][4],double b[4])
{
   int i,j;
   
      for (i=0;i<4;i++)
       {
       	mulit[i]=0;
       	for(j=0;j<4;j++)
       	    {                  
             mulit[i]+=a[i][j]*b[j];
            }
       }
 }
 //数与向量相乘
 //4*1向量
 void mulit_NUM_VEC1(double mulit[4],double n,double a[4])
  {
    	int i;
  	
  	for(i=0;i<4;i++)
    	       {
    	       	mulit[i]=n*a[i];
    	        }
    	         }
  //数与向量相乘
 //3*1向量
 void mulit_NUM_VEC2(double mulit[3],double n,double a[3])
  {
  	
  	int i;
  	
  	for(i=0;i<3;i++)
    	       {
    	       	mulit[i]=n*a[i];
    	        }
    	         }
    	         
    	         
 //数与矩阵相乘
 //3*3矩阵
 void mulit_NUM_MAT1(double mulit[3][3],double n,double a[3][3])
  {
  
  	int i,j;
  	
  	for(i=0;i<3;i++)
    	       {
    	       	for(j=0;j<3;j++)
    	       	  {
    	       	    mulit[i][j]=n*a[i][j];
    	           }
    	        }
   }
  //向量加法
  //3*1向量相加
 void add_VECTOR1(double sum[3],double a[3],double b[3])  
 {
 
  int i;
  for (i=0;i<3;i++)
      {
      	sum[i]=a[i]+b[i];
     
      }
      
   }
    //向量加法
  //4*1向量相加
 void add_VECTOR2(double sum[4],double a[4],double b[4])  
 {
 
  int i;
  for (i=0;i<4;i++)
      {
      	sum[i]=a[i]+b[i];
     
      }
      
   }
    //5个向量加法Y1=Y0+t1/6(K1+2K2+2K3+K4)
   void add_mVECTOR(double sum[4],double a[4],double b[4],double c[4],double d[4],double e1[4] )  
 {
	  double A=0.01;
      int i;
  
  for (i=0;i<4;i++)
      {
      	sum[i]=a[i]+A/6*(b[i]+2*c[i]+2*d[i]+e1[i]);
    
      }
      
   }
    
     //向量减法
     //3*1向量
      
   void sub_VECTOR1(double diff[3],double a[3],double b[3])  
 {
 
  int i;
  
  for (i=0;i<3;i++)
      {
      	diff[i]=a[i]-b[i];
  
      }
      
   } 
    
    //矩阵转置
    //3*3矩阵
   
 void trans_MATRIX1(double a[3][3],double a_trans[3][3])
   {
   	
    int i,j;
    
    for (i=0;i<3;i++)
       {
       	for(j=0;j<3;j++)
       	   {
       	   	a_trans[j][i]=a[i][j];
       	   }
       	}
    }
    //矩阵乘法
//矩阵a为3行3列,矩阵b为3行3列,相乘得到矩阵mulit为3行3列。

void mulit_MATRIX(double mulit[3][3],double a[3][3],double b[3][3])
{
      int i,j,k;
   
      for (i=0;i<3;i++)
       {
       	for(k=0;k<3;k++)
       	mulit[i][k]=0;
           for(j=0;j<3;j++)
          { 
          mulit[i][k]=mulit[i][k]+a[i][j]*b[j][k];
          }
       }
 }
 //矩阵加法
 //矩阵a为3行3列,矩阵b为3行3列,相乘得到矩阵sum为3行3列。
 void add_MATRIX(double sum[3][3],double a[3][3],double b[3][3])
{
  
   int i,j;
   
      for (i=0;i<3;i++)
         {
            for(j=0;j<3;j++)
             { 
              sum[i][j]=a[i][j]+b[i][j];
             }
          }
 }