/*****************************************************************
功能：轨迹发生器，采用北天东坐标系
******************************************************************/
#include "math.h"
#include "stdlib.h"
#include "stdio.h"
#define DYNUM 18
#define PI 3.1415926535897932384626433832795
#define wie 7.292115E-5
#define Re 20925000.
#define esqu 0.00669437999013
#define g0 32.174
#define TIME 90000
#define step 0.01
void dery(double *y,double *Wnbb,double *acc,double *dy);
void init_cnb(double *angle,double *cnb);
void indata(double *y);
void outdata(double *Wnbb,double *a,double *y);
void chacheng(double *p1,double *p2,double *p3);
void matrixmultiply(double *p1,double *p2,double *p3,int ii,int kk,int jj);
double Wnbb[3],acc[3],cbn[9],cnb[9],Wibb[3],Aibb[3],aaa;
int flag_motion=1,flag_pitch=0,time;
FILE *tr,*Velo;
main()
{
int i,j;
static int IsInit=0,flag_rk=1;
static double y0[DYNUM],dy[DYNUM];
double y[DYNUM],yy[DYNUM],angle[3];
double iniv[3],v[3];
double stepl;//=0.01;
time=0;
if((tr=fopen("trace.dat","w"))==NULL)
{ puts("CAN'T OPEN trace.dat to save trace.data");
puts("PROGRAM IS TERMINATED !");
exit(1);
}
if((Velo=fopen("velocity.dat","w"))==NULL)
{ puts("CAN'T OPEN velocity.dat to save velocity.data");
puts("PROGRAM IS TERMINATED !");
exit(1);
}
angle[0]=0./180.*PI; //pesi
angle[1]=0./180.*PI; //sita
angle[2]=0./180.*PI; //gama
init_cnb(angle,cnb);
for(i=0;i<3;i++) for(j=0;j<3;j++) cbn[i*3+j]=cnb[j*3+i];
iniv[0]=0.;
iniv[1]=0.0;
iniv[2]=0.0;
v[0]=cbn[0*3+0]*iniv[0]+cbn[0*3+1]*iniv[1]+cbn[0*3+2]*iniv[2];
v[1]=cbn[1*3+0]*iniv[0]+cbn[1*3+1]*iniv[1]+cbn[1*3+2]*iniv[2];
v[2]=cbn[2*3+0]*iniv[0]+cbn[2*3+1]*iniv[1]+cbn[2*3+2]*iniv[2];
y[0]=39./180.*PI; y[1]=116./180.*PI; y[2]=0.0;
y[3]=cnb[0*3+0]; y[4]=cnb[0*3+1]; y[5]=cnb[0*3+2];
y[6]=cnb[1*3+0]; y[7]=cnb[1*3+1]; y[8]=cnb[1*3+2];
y[9]=cnb[2*3+0]; y[10]=cnb[2*3+1]; y[11]=cnb[2*3+2];
y[12]=v[0]; y[13]=v[1]; y[14]=v[2];
y[15]=angle[0]; y[16]=angle[1]; y[17]=angle[2];
while(time<TIME)
{
if(!IsInit)
{
stepl=step;
for(i=0;i<DYNUM;i++)
{ yy[i]=y[i];
y0[i]=y[i];
dy[i]=0.0;
}
indata(y);
outdata(Wnbb,acc,y);
fprintf(tr,"%20.12lf",time*stepl);
fprintf(tr,"%20.12lf",Aibb[0]);
fprintf(tr,"%20.12lf",Aibb[1]);
fprintf(tr,"%20.12lf",Aibb[2]);
fprintf(tr,"%20.12lf",Wibb[0]);
fprintf(tr,"%20.12lf",Wibb[1]);
fprintf(tr,"%20.12lf",Wibb[2]);
dery(yy,Wnbb,acc,dy);
fprintf(tr,"\n");
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
if(j==0)
{ indata(yy);
outdata(Wnbb,acc,yy);
fprintf(tr,"%20.12lf",time*stepl);
fprintf(tr,"%20.12lf",Aibb[0]);
fprintf(tr,"%20.12lf",Aibb[1]);
fprintf(tr,"%20.12lf",Aibb[2]);
fprintf(tr,"%20.12lf",Wibb[0]);
fprintf(tr,"%20.12lf",Wibb[1]);
fprintf(tr,"%20.12lf",Wibb[2]);
fprintf(tr,"\n");
}
dery(yy,Wnbb,acc,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
if(time>30000&&time<36000)
flag_motion=5;
else
flag_motion=1;
// if(time>50000&&time<53000)
// flag_motion=3;
indata(yy);
outdata(Wnbb,acc,yy);
fprintf(tr,"%20.12lf",time*stepl);
fprintf(tr,"%20.12lf",Aibb[0]);
fprintf(tr,"%20.12lf",Aibb[1]);
fprintf(tr,"%20.12lf",Aibb[2]);
fprintf(tr,"%20.12lf",Wibb[0]);
fprintf(tr,"%20.12lf",Wibb[1]);
fprintf(tr,"%20.12lf",Wibb[2]);
fprintf(tr,"\n");
dery(yy,Wnbb,acc,dy);
for(i=0;i<DYNUM;i++) y[i]=y0[i]+stepl*dy[i]/6.0; //y=y0+k4/6
fprintf(Velo,"%20.12lf",time*stepl);
fprintf(Velo,"%20.12lf",y[0]);
fprintf(Velo,"%20.12lf",y[1]);
fprintf(Velo,"%20.12lf",y[2]);
fprintf(Velo,"%20.12lf",y[12]);
fprintf(Velo,"%20.12lf",y[13]);
fprintf(Velo,"%20.12lf",y[14]);
fprintf(Velo,"%20.12lf",y[15]);
fprintf(Velo,"%20.12lf",y[16]);
fprintf(Velo,"%20.12lf",y[17]);
fprintf(Velo,"\n");
//下一时刻开始
for(i=0;i<DYNUM;i++) { yy[i]=y[i]; y0[i]=y[i]; } //y0=y; yy=y
dery(yy,Wnbb,acc,dy);
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;
}
}
/*********************************************************************************************
功能：右端子函数，产生轨迹的微分方程组
**********************************************************************************************/
void dery(double *y,double *Wnbb,double *acc,double *dy)
{ double Vn,Vu,Ve;
double Wnbbx,Wnbby,Wnbbz;
double Rm,Rn,g;
Rm=Re*(1-esqu)/pow((1-esqu*sin(y[0])*sin(y[0])),1.5);
Rn=Re/sqrt(1-esqu*sin(y[0])*sin(y[0]));
g=g0*(1.0+0.00193185138639*sin(y[0])*sin(y[0]))/sqrt(1.0-0.00669437999013*sin(y[0])*sin(y[0]));
cnb[0*3+0]=y[3]; cnb[0*3+1]=y[4]; cnb[0*3+2]=y[5];
cnb[1*3+0]=y[6]; cnb[1*3+1]=y[7]; cnb[1*3+2]=y[8];
cnb[2*3+0]=y[9]; cnb[2*3+1]=y[10]; cnb[2*3+2]=y[11];
Vn=y[12];
Vu=y[13];
Ve=y[14];
//纬度
dy[0]=Vn/(Rm+y[2]);
//经度
dy[1]=Ve/(Rn+y[2])/cos(y[0]);
//高度
dy[2]=Vu;
Wnbbx=Wnbb[0];
Wnbby=Wnbb[1];
Wnbbz=Wnbb[2];
//方向余弦解姿态矩阵
dy[3]=Wnbbz*y[6]-Wnbby*y[9];
dy[4]=Wnbbz*y[7]-Wnbby*y[10];
dy[5]=Wnbbz*y[8]-Wnbby*y[11];
dy[6]=-Wnbbz*y[3]+Wnbbx*y[9];
dy[7]=-Wnbbz*y[4]+Wnbbx*y[10];
dy[8]=-Wnbbz*y[5]+Wnbbx*y[11];
dy[9]=Wnbby*y[3]-Wnbbx*y[6];
dy[10]=Wnbby*y[4]-Wnbbx*y[7];
dy[11]=Wnbby*y[5]-Wnbbx*y[8];
//北天东下的速度
dy[12]=acc[0];
dy[13]=acc[1];
dy[14]=acc[2];
//姿态角
dy[15]=Wnbby*cos(y[17])/cos(y[16])-Wnbbz*sin(y[17])/cos(y[16]);
dy[16]=Wnbby*sin(y[17])+Wnbbz*cos(y[17]);
dy[17]=Wnbbx-Wnbby*cos(y[17])*tan(y[16])+Wnbbz*sin(y[17])*tan(y[16]);
return;
}
void init_cnb(double *angle,double *cnb)
{
/*导弹纵轴为x轴,采用北天东坐标系*/
cnb[0*3+0]=cos(angle[0])*cos(angle[1]);
cnb[0*3+1]=sin(angle[1]);
cnb[0*3+2]=-sin(angle[0])*cos(angle[1]);
cnb[1*3+0]=sin(angle[0])*sin(angle[2])-cos(angle[0])*sin(angle[1])*cos(angle[2]);
cnb[1*3+1]=cos(angle[1])*cos(angle[2]);
cnb[1*3+2]=cos(angle[0])*sin(angle[2])+sin(angle[0
