惯性导航轨迹发生器程序

/*****************************************************************
功能：轨迹发生器，采用北天东坐标系
******************************************************************/
#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 *ydouble *Wnbbdouble *accdouble *dy）;
void init_cnb（double *angledouble *cnb）;
void indata（double *y）;
void outdata（double *Wnbbdouble *adouble *y）;
void  chacheng（double *p1double *p2double *p3）;
void matrixmultiply（double *p1double *p2double *p3int iiint kkint jj）;
double Wnbb[3]acc[3]cbn[9]cnb[9]Wibb[3]Aibb[3]aaa;
int flag_motion=1flag_pitch=0time;


FILE *tr*Velo;
main（）
{

	int ij;
	static int IsInit=0flag_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（anglecnb）;
    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  {
	if（!IsInit）
	{  
		stepl=step;	
		for（i=0;i		{  yy[i]=y[i];	
		   y0[i]=y[i];
           dy[i]=0.0;
		}
		indata（y）;
        outdata（Wnbbaccy）;
        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（yyWnbbaccdy）;
	    fprintf（tr“\n“）;
		for（i=0;i		flag_rk=2;
		IsInit=1;	
	}
    else if（flag_rk==2）
	{
	  for（j=0;j<=1;j++）
		{  	for（i=0;i