📄 strapdown.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 e 1/298.275
#define esqu 0.00669437999013
#define g0 32.174
#define JTOH 0.017453293
void dery(double *yy,double *yy2,double *dy);
double Aibpx,Aibpy,Aibpz;
int time=1;
FILE *jl,*fp,*vel,*noise1,*noise2,*noise3;
void main()
{
int i,j,k=0,n=1,kk;
double s_init[10];
static int IsInit=0,flag_rk=1,temp=0;
static double y0[DYNUM],dy[DYNUM],stepl,s_indat[7],s_indatbuff[7],s_velocity[10];
double y[DYNUM],yy[DYNUM],angle[3],cpb[9],cbp[9],random[3];
if((jl=fopen("jielianjiesuan.dat","w"))==NULL)
{ puts("CAN'T OPEN jielianjiesuan.dat to save result data.");
puts("PROGRAM IS TERMINATED !");
exit(1);
}
fp=fopen("trace.dat","rb");
if(fp==NULL)
{
puts("Can't open file trace.dat");
exit(0);
}
vel=fopen("velocity.dat","rb");
if(fp==NULL)
{
puts("Can't open file velocity.dat");
exit(0);
}
/* noise1=fopen("randomdata1.dat","rb");
if(fp==NULL)
{
puts("Can't open file velocity.dat");
exit(0);
}
noise2=fopen("randomdata2.dat","rb");
if(fp==NULL)
{
puts("Can't open file velocity.dat");
exit(0);
}
noise3=fopen("randomdata3.dat","rb");
if(fp==NULL)
{
puts("Can't open file velocity.dat");
exit(0);
}
*/
s_init[0]= 0;
s_init[1]= 116.*PI/180.; //jingdu,weidu,gaodu
s_init[2]= 39.*PI/180.;
s_init[3]= 0.0;
s_init[4]= 0.; //v
s_init[5]= 0.;
s_init[6]= 0.;
s_init[7]=-0.1*PI/180.; //sita
s_init[8]=-0.1*PI/180.; //pesi
s_init[9]=-0.2*PI/180.; //gama
while(time<90000)
{
if(!IsInit)
{ angle[0]=s_init[9];
angle[1]=s_init[7];
angle[2]=s_init[8];
// 按照英美坐标系的计算方法，三个角度的排列顺序为 [滚转角 俯仰角 偏航角]
cpb[0*3+0]=cos(angle[1])*cos(angle[2]);
cpb[0*3+1]=cos(angle[1])*sin(angle[2]);
cpb[0*3+2]=-sin(angle[1]);
cpb[1*3+0]=sin(angle[0])*sin(angle[1])*cos(angle[2])-cos(angle[0])*sin(angle[2]);
cpb[1*3+1]=sin(angle[0])*sin(angle[1])*sin(angle[2])+cos(angle[0])*cos(angle[2]);
cpb[1*3+2]=sin(angle[0])*cos(angle[1]);
cpb[2*3+0]=cos(angle[0])*sin(angle[1])*cos(angle[2])+sin(angle[0])*sin(angle[2]);
cpb[2*3+1]=cos(angle[0])*sin(angle[1])*sin(angle[2])-sin(angle[0])*cos(angle[2]);
cpb[2*3+2]=cos(angle[0])*cos(angle[1]);
y[0]=s_init[1]; //jingdu
y[1]=s_init[2]; //weidu
y[2]=s_init[3]; //high
y[3]=s_init[4]; //Veppx Veppy Veppz
y[4]=s_init[5];
y[5]=s_init[6];
y[6]=cpb[0*3+0];
y[7]=cpb[0*3+1];
y[8]=cpb[0*3+2];
y[9]=cpb[1*3+0];
y[10]=cpb[1*3+1];
y[11]=cpb[1*3+2];
y[12]=cpb[2*3+0];
y[13]=cpb[2*3+1];
y[14]=cpb[2*3+2];
y[15]=0.;
y[16]=0.;
y[17]=0.;
stepl=0.01; //integrate step lengh 2*s_init[12]
for(i=0;i<7;i++) fscanf(fp,"%lf",&s_indatbuff[i]);
/* fscanf(noise1,"%lf",&random[0]);
fscanf(noise2,"%lf",&random[1]);
fscanf(noise3,"%lf",&random[2]);*/
s_indat[0]=s_indatbuff[0];
s_indat[1]=s_indatbuff[1]+0.000;//;
s_indat[2]=s_indatbuff[3]+0.000;
s_indat[3]=-s_indatbuff[2]+0.000;//+4.6875e-005;
/* s_indat[4]=s_indatbuff[4]+9.6962e-6+random[0];
s_indat[5]=s_indatbuff[6]+9.6962e-6+random[2];
s_indat[6]=-s_indatbuff[5]-9.6962e-6+random[1];*/
s_indat[4]=s_indatbuff[4]+9.6962e-6;
s_indat[5]=s_indatbuff[6]+9.6962e-6;
s_indat[6]=-s_indatbuff[5]-9.6962e-6;
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(i=0;i<7;i++) fscanf(fp,"%lf",&s_indatbuff[i]);
s_indat[0]=s_indatbuff[0];
s_indat[1]=s_indatbuff[1]+0.000;//;
s_indat[2]=s_indatbuff[3]+0.000;
s_indat[3]=-s_indatbuff[2]+0.000;//+4.6875e-005;
/* s_indat[4]=s_indatbuff[4]+9.6962e-6+random[0];
s_indat[5]=s_indatbuff[6]+9.6962e-6+random[2];
s_indat[6]=-s_indatbuff[5]-9.6962e-6+random[1];*/
s_indat[4]=s_indatbuff[4]+9.6962e-6;
s_indat[5]=s_indatbuff[6]+9.6962e-6;
s_indat[6]=-s_indatbuff[5]-9.6962e-6;
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<7;i++) fscanf(fp,"%lf",&s_indatbuff[i]);
for(i=0;i<10;i++) fscanf(vel,"%lf",&s_velocity[i]);
s_indat[0]=s_indatbuff[0];
s_indat[1]=s_indatbuff[1]+0.000;//;
s_indat[2]=s_indatbuff[3]+0.000;
s_indat[3]=-s_indatbuff[2]+0.000;//+4.6875e-005;
/* s_indat[4]=s_indatbuff[4]+9.6962e-6+random[0];
s_indat[5]=s_indatbuff[6]+9.6962e-6+random[2];
s_indat[6]=-s_indatbuff[5]-9.6962e-6+random[1];*/
s_indat[4]=s_indatbuff[4]+9.6962e-6;
s_indat[5]=s_indatbuff[6]+9.6962e-6;
s_indat[6]=-s_indatbuff[5]-9.6962e-6;
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
cpb[0*3+0]=y[6];
cpb[0*3+1]=y[7];
cpb[0*3+2]=y[8];
cpb[1*3+0]=y[9];
cpb[1*3+1]=y[10];
cpb[1*3+2]=y[11];
cpb[2*3+0]=y[12];
cpb[2*3+1]=y[13];
cpb[2*3+2]=y[14];
for(i=0;i<3;i++) {for(j=0;j<3;j++) cbp[i*3+j]=cpb[j*3+i];}
kk=time%25;
if(kk==0)
{//fprintf(jl,"%20.10lf",s_indatbuff[0]);
fprintf(jl,"%20.12lf ",y[1]);
fprintf(jl,"%20.12lf ",y[0]);
fprintf(jl,"%20.12lf ",y[2]);
fprintf(jl,"%20.12lf ",y[3]);
fprintf(jl,"%20.12lf ",y[4]);
fprintf(jl,"%20.12lf ",y[5]);
/* fprintf(jl,"%20.10lf",cpb[0]);
fprintf(jl,"%20.10lf",cpb[1]);
fprintf(jl,"%20.10lf",cpb[2]);
fprintf(jl,"%20.10lf",cpb[3]);
fprintf(jl,"%20.10lf",cpb[4]);
fprintf(jl,"%20.10lf",cpb[5]);
fprintf(jl,"%20.10lf",cpb[6]);
fprintf(jl,"%20.10lf",cpb[7]);
fprintf(jl,"%20.10lf",cpb[8]);*/
fprintf(jl,"%20.10lf",cbp[0]);
fprintf(jl,"%20.10lf",cbp[1]);
fprintf(jl,"%20.10lf",cbp[2]);
fprintf(jl,"%20.10lf",cbp[3]);
fprintf(jl,"%20.10lf",cbp[4]);
fprintf(jl,"%20.10lf",cbp[5]);
fprintf(jl,"%20.10lf",cbp[6]);
fprintf(jl,"%20.10lf",cbp[7]);
fprintf(jl,"%20.10lf",cbp[8]);
fprintf(jl,"%20.12lf",s_velocity[4]);
fprintf(jl,"%20.12lf",s_velocity[6]);
fprintf(jl,"%20.12lf",-s_velocity[5]);
fprintf(jl,"%20.10lf",s_indatbuff[1]);
fprintf(jl,"%20.10lf",s_indatbuff[3]);
fprintf(jl,"%20.10lf",-s_indatbuff[2]);
// fprintf(jl,"%20.10lf",-y[15]);
// fprintf(jl,"%20.10lf",y[16]);
// fprintf(jl,"%20.10lf",y[17]);
/* fprintf(jl,"%20.10lf",-y[15]-s_velocity[7]);*/
/* fprintf(jl,"%20.10lf",y[16]-s_velocity[8]);
fprintf(jl,"%20.10lf",y[17]-s_velocity[9]);
buffer=atan(cbp[1]/cbp[0]);
if(buffer<0)
buffer=buffer+PI/2;
fprintf(jl,"%20.10lf",buffer);
fprintf(jl,"%20.10lf",asin(-cpb[2]));
fprintf(jl,"%20.10lf",atan(cpb[5]/cpb[8]));*/
fprintf(jl,"%20.10lf",cbp[5]);
fprintf(jl,"%20.10lf",-cbp[0]*sin(s_velocity[7])+cbp[1]*cos(s_velocity[7]));
fprintf(jl,"%20.10lf",cbp[6]*cos(s_velocity[7])+cbp[7]*sin(s_velocity[7]));
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 Wiepx,Wiepy,Wiepz;
double Weppx,Weppy,Weppz;
double Wippx,Wippy,Wippz;
double Wipbx,Wipby,Wipbz;
double Wpbbx,Wpbby,Wpbbz;
double Rm,Rn;
double g;
double cpb[9],cbp[9];
int i,j,kkk;
/*calculate Rm ,Rn and g;*/
/*Rm=Re*(1-2*e+3*e*sin(y[1])*sin(y[1]))+y[2];
Rn=Re*(1+e*sin(y[1])*sin(y[1]))+y[2];*/
Rm=Re*(1-esqu)/pow((1-esqu*sin(y[1])*sin(y[1])),1.5);
Rn=Re/sqrt(1-esqu*sin(y[1])*sin(y[1]));
g=g0*(1.0+0.00193185*sin(y[1])*sin(y[1]))/sqrt(1.0-0.00669438*sin(y[1])*sin(y[1]));
/*calculate cpb and cbp*/
cpb[0*3+0]=y[6];
cpb[0*3+1]=y[7];
cpb[0*3+2]=y[8];
cpb[1*3+0]=y[9];
cpb[1*3+1]=y[10];
cpb[1*3+2]=y[11];
cpb[2*3+0]=y[12];
cpb[2*3+1]=y[13];
cpb[2*3+2]=y[14];
for(i=0;i<3;i++) for(j=0;j<3;j++) cbp[i*3+j]=cpb[j*3+i];
/* 将加速度计测得的相对于弹体系的比力 Aibb 转换到平台系 Aibp */
Aibpx=s_indat[1]*cbp[0*3+0]+s_indat[2]*cbp[0*3+1]+s_indat[3]*cbp[0*3+2];
Aibpy=s_indat[1]*cbp[1*3+0]+s_indat[2]*cbp[1*3+1]+s_indat[3]*cbp[1*3+2];
Aibpz=s_indat[1]*cbp[2*3+0]+s_indat[2]*cbp[2*3+1]+s_indat[3]*cbp[2*3+2];
/* 计算地球相对于惯性系的角速度(Wie)在平台系的投影 Wiep */
Wiepx= WIE*cos(y[1]);
Wiepy= 0.0;
Wiepz=-WIE*sin(y[1]);
/* 计算平台相对于地球系的角速度(Wep)在平台系的投影 Wepp */
Weppx= y[4]/(Rn+y[2]);
Weppy=-y[3]/(Rm+y[2]);
Weppz=-y[4]*tan(y[1])/(Rn+y[2]);
/* 计算平台相对于惯性系的角速度(Wip)在平台系的投影 Wipp */
Wippx=Wiepx+Weppx;
Wippy=Wiepy+Weppy;
Wippz=Wiepz+Weppz;
/* 将平台相对于惯性系的角速度(Wip)转换到弹体系上(Wipb) */
Wipbx=Wippx*cpb[0*3+0]+Wippy*cpb[0*3+1]+Wippz*cpb[0*3+2];
Wipby=Wippx*cpb[1*3+0]+Wippy*cpb[1*3+1]+Wippz*cpb[1*3+2];
Wipbz=Wippx*cpb[2*3+0]+Wippy*cpb[2*3+1]+Wippz*cpb[2*3+2];
/* 计算弹体相对于平台的角速度(Wpb)在弹体系的投影(Wpbb) */
Wpbbx=s_indat[4]-Wipbx;
Wpbby=s_indat[5]-Wipby;
Wpbbz=s_indat[6]-Wipbz;
/* 计算当前时刻各变量的导数 */
dy[0]= y[4]/((Rn+y[2])*cos(y[1])); /*djingdu dweidu dhigh*/
dy[1]= y[3]/(Rm+y[2]);
dy[2]=-y[5]; //(s_init[7]-y[2]); -k1*(0-y[2]) //gaodu wei 0 ?????????????????????K1
dy[3]=Aibpx+(2*Wiepz+Weppz)*y[4]-(2*Wiepy+Weppy)*y[5]; //dVeppx dVAeppy dVAeppz
dy[4]=Aibpy-(2*Wiepz+Weppz)*y[3]+(2*Wiepx+Weppx)*y[5];
dy[5]=Aibpz+(2*Wiepy+Weppy)*y[3]-(2*Wiepx+Weppx)*y[4]+g; // dVAeppz
//fprintf(jl,"%20.15lf",Wpbbx);
//fprintf(jl,"\n");
dy[6] =Wpbbz*y[9]-Wpbby*y[12];
dy[7] =Wpbbz*y[10]-Wpbby*y[13];
dy[8] =Wpbbz*y[11]-Wpbby*y[14];
dy[9] =-Wpbbz*y[6]+Wpbbx*y[12];
dy[10]=-Wpbbz*y[7]+Wpbbx*y[13];
dy[11]=-Wpbbz*y[8]+Wpbbx*y[14];
dy[12]=Wpbby*y[6]-Wpbbx*y[9];
dy[13]=Wpbby*y[7]-Wpbbx*y[10];
dy[14]=Wpbby*y[8]-Wpbbx*y[11];
if(time>30000)
kkk=0;
dy[15]=Wpbby*sin(y[17])/cos(y[16])+Wpbbz*cos(y[17])/cos(y[16]); //psi
dy[16]=Wpbby*cos(y[17])-Wpbbz*sin(y[17]); //sita
dy[17]=Wpbbx+Wpbby*sin(y[17])*tan(y[16])+Wpbbz*cos(y[17])*tan(y[16]); //gama
return;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -