📄 trace_generate.c
字号:
/*****************************************************************
功能：轨迹发生器，采用北天东坐标系
******************************************************************/
#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])*sin(angle[1])*cos(angle[2]);
cnb[2*3+0]=sin(angle[0])*cos(angle[2])+cos(angle[0])*sin(angle[1])*sin(angle[2]);
cnb[2*3+1]=-cos(angle[1])*sin(angle[2]);
cnb[2*3+2]=cos(angle[0])*cos(angle[2])-sin(angle[0])*sin(angle[1])*sin(angle[2]);
return;
}
/*********************************************************************************************
功能：用于产生加速度和角速度
**********************************************************************************************/
void indata(double *y)
{
double Sita,Vn,Vu,Ve,V[3];
double A,B,Wx,Wy,Wz,Rn,Rm;
double Wnbn[3];
double *pp1,*pp2,*pp3;
int i,j;
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]));
Sita=y[16];
Vn=y[12]; Vu=y[13]; Ve=y[14];
A=Vn/(Rm+y[2]);
B=Ve/((Rn+y[2])*cos(y[0]));
Wx=B*cos(y[0]); Wy=B*sin(y[0]); Wz=-A;
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];
for(i=0;i<3;i++) for(j=0;j<3;j++) cbn[i*3+j]=cnb[j*3+i];
/*********************************************************************************************/
//作大圆直线运动
if(flag_motion==0)
{ Wnbb[0]=0.0;
Wnbb[1]=-Wy;
Wnbb[2]=0.0;
}
/*********************************************************************************************/
//作固定航向直线运动
if(flag_motion==1)
{ Wnbb[0]=0.0;
Wnbb[1]=0.0;
Wnbb[2]=0.0;
}
/*********************************************************************************************/
//在任意位置作以大圆运动为基础的爬升运动
if(flag_motion==2)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= 200.*cos(Sita)/Re+200./5000.;
}
/*********************************************************************************************/
//在任意位置作以固定航向运动为基础的爬升运动
if(flag_motion==3)
{ Wnbb[0]=0.0;
Wnbb[1]=0.0;
//Wnbb[2]=200.*cos(Sita)/Re+20./5000.; //
Wnbb[2]=PI/180;
}
/*********************************************************************************************/
//作以大圆运动为基础的水平圆运动指令，按照侧向加速度求出转弯角速度
if(flag_motion==4)
{ Wnbn[0]=0.0;
Wnbn[1]=-Wy+19.6020/200; //200/Re/5000*sqrt(Re*Re-5000*5000)
Wnbn[2]=0.0;
pp1=(double *)cnb;
pp2=(double *)Wnbn;
pp3=(double *)Wnbb;
matrixmultiply(pp1,pp2,pp3,3,3,1);
}
/*********************************************************************************************/
//作以固定航向运动为基础的水平圆运动指令，按照侧向加速度求出转弯角速度
if(flag_motion==5)
{ Wnbn[0]=0.0;
//Wnbn[1]=2.6020/200.;
Wnbn[1]=-PI/60;
Wnbn[2]=0.0;
pp1=(double *)cnb;
pp2=(double *)Wnbn;
pp3=(double *)Wnbb;
matrixmultiply(pp1,pp2,pp3,3,3,1);
}
/*********************************************************************************************/
/* if(flag_motion==6&flag_pitch==0)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= V0*cos(Sita)/Re+A1/V0;
Delta_Sita=Sita0-Sita;
W1=Wnbb[3]*0.01;
W2=Delta_Sita/0.01;
if(Delta_Sita<=W1)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= W2;
}
}
if(flag_motion==6&flag_pitch==1)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= 0.0;
}
if(flag_motion==6&flag_pitch==2)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= V0*cos(Sita)/Re-A2/V0;
Delta_Sita=-Sita;
W1=Wnbb[3]*0.01;
W2=Delta_Sita/0.01;
if(Delta_Sita>=W1)
{ Wnbb[0]=-Wy*sin(Sita);
Wnbb[1]=-Wy*cos(Sita);
Wnbb[2]= W2;
}
}
if(flag_motion==6&flag_pitch==3)
{ Wnbb[0]= 0.0;
Wnbb[1]=-Wy;
Wnbb[2]= 0.0;
}
if(flag_motion==6&flag_pitch==4)
{ Wnbb[0]=Wnbb[0];
Wnbb[1]=Wnbb[1];
Wnbb[2]=Wnbb[2];
}*/
/*********************************************************************************************/
pp1=(double *)cbn;
pp2=(double *)Wnbb;
pp3=(double *)Wnbn;
matrixmultiply(pp1,pp2,pp3,3,3,1);
V[0]=Vn; V[1]=Vu; V[2]=Ve;
pp1=(double *)Wnbn;
pp2=(double *)V;
pp3=(double *)acc;
chacheng(pp1,pp2,pp3);
//if(time>=20000&&time<50000) acc[0]=acc[0]+10.;
//if(time>=40000&&time<50000) acc[1]=acc[1]+10.;
}
/*********************************************************************************************
功能：用于产生惯性输出
**********************************************************************************************/
void outdata(double *Wnbb,double *acc,double *y)
{
int i;
double Wienx,Wieny,Wienz;
double Wennx,Wenny,Wennz;
double Rn,Rm,g;
double Wien[3],Wieb[3],Wenn[3],Wenb[3];
double *pp1,*pp2,*pp3;
double C[9],V[3],Acc[3],A[3];
Rm=Re*(1-esqu)/pow((1-esqu*sin(y[0])*sin(y[0])),1.5)+y[2];
Rn=Re/sqrt(1-esqu*sin(y[0])*sin(y[0]))+y[2];
Wienx= wie*cos(y[0]);
Wieny= wie*sin(y[0]);
Wienz= 0.;
Wien[0]=Wienx;
Wien[1]=Wieny;
Wien[2]=Wienz;
pp1=(double *)cnb;
pp2=(double *)Wien;
pp3=(double *)Wieb;
matrixmultiply(pp1,pp2,pp3,3,3,1);
Wennx= y[14]/(Rn+y[2]);
Wenny= y[14]/(Rn+y[2])*tan(y[0]);
Wennz=-y[12]/(Rm+y[2]);
Wenn[0]=Wennx;
Wenn[1]=Wenny;
Wenn[2]=Wennz;
pp1=(double *)cnb;
pp2=(double *)Wenn;
pp3=(double *)Wenb;
matrixmultiply(pp1,pp2,pp3,3,3,1);
for(i=0;i<3;i++) Wibb[i]=Wnbb[i]+Wieb[i]+Wenb[i];
g=g0*(1.0+0.00193185138639*sin(y[0])*sin(y[0]))/sqrt(1.0-6.69438e-3*sin(y[0])*sin(y[0]));
C[0]= 0.0; C[1]= 2*Wienz+Wennz; C[2]=-(2*Wieny+Wenny);
C[3]=-(2*Wienz+Wennz); C[4]= 0.0; C[5]= (2*Wienx+Wennx);
C[6]= (2*Wieny+Wenny); C[7]=-(2*Wienx+Wennx); C[8]= 0.0;
V[0]=y[12]; V[1]=y[13]; V[2]=y[14];
pp1=(double *)C;
pp2=(double *)V;
pp3=(double *)Acc;
matrixmultiply(pp1,pp2,pp3,3,3,1);
A[0]=acc[0]-Acc[0];
A[1]=acc[1]-Acc[1]+g;
A[2]=acc[2]-Acc[2];
pp1=(double *)cnb;
pp2=(double *)A;
pp3=(double *)Aibb;
matrixmultiply(pp1,pp2,pp3,3,3,1);
}
/*********************************************************************************************
功能：用于向量叉乘
**********************************************************************************************/
void chacheng(double *p1,double *p2,double *p3)
{ int i,j,k;
int ii=3,kk=3,jj=1;
double AA[9],BB[3];
AA[0]=0.; AA[1]=-p1[2]; AA[2]=p1[1];
AA[3]=p1[2]; AA[4]=0.; AA[5]=-p1[0];
AA[6]=-p1[1]; AA[7]=p1[0]; AA[8]=0.;
BB[0]=p2[0]; BB[1]=p2[1]; BB[2]=p2[2];
for(i=0;i<ii;i++)
for(j=0;j<jj;j++)
p3[i*jj+j]=0.0;
for(i=0;i<ii;i++)
for(j=0;j<jj;j++)
for(k=0;k<kk;k++)
p3[i*jj+j]=p3[i*jj+j]+AA[i*kk+k]*BB[k*jj+j];
return;
}
/*********************************************************************************************
功能：用于矩阵相乘
**********************************************************************************************/
void matrixmultiply(double *p1,double *p2,double *p3,int ii,int kk,int jj)
{
int i,j,k;
for(i=0;i<ii;i++)
for(j=0;j<jj;j++)
p3[i*jj+j]=0.0;
for(i=0;i<ii;i++)
for(j=0;j<jj;j++)
for(k=0;k<kk;k++)
p3[i*jj+j]=p3[i*jj+j]+p1[i*kk+k]*p2[k*jj+j];
return;
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -