cal_direct.c

用龙格－库塔法直接解算微分方程

#include "math.h"
#include "stdlib.h"
#include "stdio.h"

#define DYNUM		12
#define PI			3.1415926535897932384626433832795
#define WIE			7.292115E-5
#define Re			6.378137e6     //为了减小模拟的运算时间，把地球半径改小，减小舒拉周期，本来为6.378137e6
#define e			1/298.275      //e也应该改，但是现在懒得去算，因为e的影响很小
#define g0			9.78049
#define JTOH		0.017453293    //the constand of translating degree to radian.


void dery(double *yy,double *yy2,double *dy);
void init_sq(double *angle,double *sq);
void cal_cbp(double *sq,double *cpb);
void unitray(double *y);
void cal_angle(double *cpb,double *angle);
void Cal_cpb(double *angle,double *cpb);

double Aibpx,Aibpy,Aibpz;
double sq[4];

FILE *jl,*opt,*out,*fp,*zhuansu,*erroroutput,*kalmanshuru;



void main()
{
	int i,j,k=0,n=1,time=1,kk;
	static int IsInit=0,flag_rk=1,temp=0;
	static double y0[DYNUM],dy[DYNUM],stepl,s_indat[7];
	double y[DYNUM],yy[DYNUM];

	if((jl=fopen("strapdown_dir.dat","w"))==NULL)
	{	puts("CAN'T OPEN jielianjiesuan.dat to save result data.");
		puts("PROGRAM IS TERMINATED !");
		exit(1);
	}

  while(time<1000000)
  {
	if(!IsInit)
	{  
		y[0]=0.;			     
		y[1]=0.;			      
		y[2]=0.0;			      
		y[3]=0./180.*PI;			      
		y[4]=0./180.*PI;
        y[5]=0./180.*PI;
		y[6]=0.0;
		y[7]=0.+0.02;			     
		y[8]=0.0;			      
		y[9]=0.0;			      
		y[10]=0.;//+4.8481e-7;			      
		y[11]=0.;
 

		stepl=0.1;		         

		for(i=0;i<DYNUM;i++)	{	yy[i]=y[i];	y0[i]=y[i];	}
		dery(yy,s_indat,dy);

		for(i=0;i<DYNUM;i++)	y0[i]=y0[i]+stepl*dy[i]/6.0; 	     //y0=y0+k1/6
		flag_rk=2;
		IsInit=1;			
	}
	else if(flag_rk==2)
	{
	  	for(j=0;j<=1;j++)
		{  	for(i=0;i<DYNUM;i++)	yy[i]=y[i]+stepl*dy[i]/2.0;	     //yy=y+k1/2; yy=y+k2/2
			dery(yy,s_indat,dy);
			for(i=0;i<DYNUM;i++)	y0[i]=y0[i]+stepl*dy[i]/3.0;     //y0=y0+k2/3, y0=y0+k3/3
		}
	  
	  for(i=0;i<DYNUM;i++)	yy[i]=y[i]+stepl*dy[i];		             //yy=y+k3
	  dery(yy,s_indat,dy);
	  
	  for(i=0;i<DYNUM;i++)	y[i]=y0[i]+stepl*dy[i]/6.0;              //y=y0+k4/6
	

	  kk=time%500;
	  if(kk==0)
      {
	  fprintf(jl,"%20.10lf",time*stepl/3600.);
      fprintf(jl,"%20.10lf",y[0]);
	  fprintf(jl,"%20.10lf",y[1]);
	  fprintf(jl,"%20.10lf",y[2]);
	  fprintf(jl,"%20.10lf",y[3]);
	  fprintf(jl,"%20.10lf",y[4]);
	  fprintf(jl,"%20.10lf",y[5]);
	  fprintf(jl,"%20.10lf",y[6]);
	  fprintf(jl,"%20.10lf",y[7]);
	  fprintf(jl,"%20.10lf",y[8]);
	  fprintf(jl,"%20.10lf",y[9]);
	  fprintf(jl,"%20.10lf",y[10]);
	  fprintf(jl,"%20.10lf",y[11]);
      fprintf(jl,"\n");}
	  for(i=0;i<DYNUM;i++)	{	yy[i]=y[i];	y0[i]=y[i];	}            //y0=y; yy=y
	  dery(yy,s_indat,dy);
	  n++;

	  for(i=0;i<DYNUM;i++)	y0[i]=y0[i]+stepl*dy[i]/6.0; 	         //y0=y0+k1/6
	  flag_rk=2;
	}
      time=time+1;
  }

 return;
}

void dery(double *y,double *s_indat,double *dy)
{	double Wie,g,R,L;
    double cnb[9],angle[3];
    Wie=7.292115e-5;
    g=9.8014146705594;
    R=6378137.;
    L=39.5/180*PI;

	angle[0]=y[5];
	angle[1]=y[3];
	angle[2]=y[4];
	Cal_cpb(angle,cnb);
	/* 计算当前时刻各变量的导数 */
	dy[0]=-g*y[4]+y[6]+2*Wie*sin(L)*y[1];                           //  东速
	dy[1]= g*y[3]+y[7]-2*Wie*sin(L)*y[0];                           //  北速
    dy[2]= 1./R*y[1];                                               //  纬度
	dy[3]=-1./R*y[1]+Wie*sin(L)*y[4]-Wie*cos(L)*y[5]+y[8];          //  东向误差角
	dy[4]= 1./R*y[0]-Wie*sin(L)*y[2]-Wie*sin(L)*y[3]+y[9];          //  北向误差角
	dy[5]= 1./R*y[0]*tan(L)+Wie*cos(L)*y[2]+Wie*cos(L)*y[3]+y[10];  //  天向误差角
    dy[6]= 0.0;                                                //  东向加速度计误差
	dy[7]= 0.0;//+0.1/3600/57.32;                                                //  北向加速度计误差
	dy[8]= 0.0;//+0.1/3600/57.32;                                                //  东向陀螺漂移
	dy[9]= 0.0;                                                //  北向陀螺漂移
	dy[10]=0.0;                                               //  天向陀螺漂移
    dy[11]=1./R/cos(L)*y[0];                                  //  经度


	return;
}

void Cal_cpb(double *angle,double *cpb)
{
    cpb[0*3+0]=cos(angle[0])*cos(angle[2])-sin(angle[0])*sin(angle[1])*sin(angle[2]);
	cpb[0*3+1]=sin(angle[0])*cos(angle[2])+cos(angle[0])*sin(angle[1])*sin(angle[2]);
	cpb[0*3+2]=-cos(angle[1])*sin(angle[2]);
	cpb[1*3+0]=-sin(angle[0])*cos(angle[1]);
	cpb[1*3+1]=cos(angle[0])*cos(angle[1]);
	cpb[1*3+2]=sin(angle[1]);
	cpb[2*3+0]=cos(angle[0])*sin(angle[2])+sin(angle[0])*sin(angle[1])*cos(angle[2]);
	cpb[2*3+1]=sin(angle[0])*sin(angle[1])-cos(angle[0])*sin(angle[1])*cos(angle[2]);
	cpb[2*3+2]=cos(angle[1])*cos(angle[2]);
}