捷联惯性导航C语言实现

//捷联惯导 #include <math.h> #include <iostream.h> #include <stdio.h> #define PI 3.1415926 #define WIE 7.2921158e-5//弧度每秒 #define RAD PI/180 #define G 9.8 #define Re 6.378393e6 #define dRe 0.15678e-6 #define e 0.3367e-2 //void Initial(); void Initial(double C[3][3],double T[3][3],double Q[4],double POS[3],double V[2],double Angl[3],double Wiep[3],double WEPP[2]); void INITCMAT(double C[3][3],double POS[3]); void INITQ(double T[3][3],double Q[4]); void INITWIEP(double C[3][3]); void INITWEPP(double C[3][3], double V[2]); void calcG(double C[3][3],double h); void calcV(double V[2]); void calcWPBB(double Wpbb[3],double Wibb[3], double T[3][3],double Wepp[3], double Wiep[3],double WEPP[2]); //void updConvQ(double Wpbb[3]); void updConvQ(double Wpbb[3],double Conv_Q[4][4]); void updateQ(double Wpbb1[3],double Wpbb2[3],double Wpbb3[3],double Q[4],double t1); void unitQ(double Q[4]); void calaT(double Q[4],double T[3][3]); void convacce(double T[3][3],double fp[3],double fb[3]); void updateVp(double fp[3],double Wiep[3],double Wepp[3],double V[3],double t2); //void updateVp(double fp[3],double Wiep[3],double Wepp[3],double Init_V[3],double t2); void calaWEPP(double C[3][3], double V[2],double WEPP[2]); void updateC(double C[3][3],double WEPP[2],double t2); //void updateC(double Init_C[3][3],double C[3][3],double Wepp[2],double t2); void calaWIEP(double Wiep[3],double C[3][3],double Wie[3]); void calapos(double POS[3],double C[3][3]); void calczitai(double Angl[3],double T[3][3],double alpha); void mulit_MAT_VEC1(double mulit[2],double a[2][2],double b[2]); void mulit_MAT_VEC2(double mulit[3],double a[3][3],double b[3]); void mulit_MAT_VEC3(double mulit[4],double a[4][4],double b[4]); void mulit_NUM_VEC1(double mulit[4],double n,double a[4]); void mulit_NUM_VEC2(double mulit[3],double n,double a[3]); void mulit_NUM_MAT1(double mulit[3][3],double n,double a[3][3]); void add_VECTOR1(double sum[3],double a[3],double b[3]); void add_VECTOR2(double sum[4],double a[4],double b[4]); void add_mVECTOR(double sum[4],double a[4],double b[4],double c[4],double d[4],double e1[4]); void sub_VECTOR1(double diff[3],double a[3],double b[3]); void trans_MATRIX1(double a[3][3],double a_trans[3][3]); void mulit_MATRIX(double mulit[3][3],double a[3][3],double b[3][3]); void add_MATRIX(double sum[3][3],double a[3][3],double b[3][3]); void main() { double LAT=45*RAD; double t1=0.01; double t2=0.04; int I=0; int m,n; double T[3][3]; double Wepp[3]; double WEPP[2]; double Wiep[3]; double Q[4]; double fp[3]; double C[3][3]; double V[2]; double Wie[3]; double h=0; double Angl[3]; double alpha=0; double POS[3]; double x=0; double Wf[18]; double Wibb1[3],Wibb2[3],Wibb3[3],fb[3]; double Wpbb1[3],Wpbb2[3],Wpbb3[3]; //Initial(); Initial(C,T,Q,POS,V,Angl,Wiep,WEPP); cout<<C[0][0]<<" "<<C[0][1]<<" "<<C[0][2]<<"\n"; cout<<C[1][0]<<" "<<C[1][1]<<" "<<C[1][2]<<"\n"; cout<<C[2][0]<<" "<<C[2][1]<<" "<<C[2][2]<<"\n"; while(x<10) { I=I+1; cout<<"I="<<I<<"\n"; for(m=0;m<18;m++) { Wf[m]=10*cos(x); x+=0.001; } //cout<<"x="<<x<<"Wf="<<Wf[i]<<"\n"; for(n=0;n<3;n++) { Wibb1[0+n]=Wf[3+n]; Wibb2[0+n]=Wf[9+n]; Wibb3[0+n]=Wf[15+n]; fb[n]=Wf[n]; } calcWPBB(Wpbb1,Wibb1,T,Wepp,Wiep,WEPP);//姿态速率的计算； calcWPBB(Wpbb2,Wibb2,T,Wepp,Wiep,WEPP); calcWPBB(Wpbb3,Wibb3,T,Wepp,Wiep,WEPP); updateQ(Wpbb1,Wpbb2,Wpbb3,Q,t1);//四元数Q的即时修正 if (I%8==0) {unitQ(Q); }//四元数Q的归一化 else { calaT(Q,T); //矩阵T的计算 convacce(T,fp,fb); }//加速度的坐标转换 if (I%4==0) { updateVp(fp,Wiep,Wepp,V,t2); calaWEPP(C,V,WEPP); cout<<"WEPP="<<WEPP[0]<<" "<<WEPP[1]<<"\n"; updateC(C,Wepp,t2); cout<<C[0][0]<<" "<<C[0][1]<<" "<<C[0][2]<<"\n"; cout<<C[1][0]<<" "<<C[1][1]<<" "<<C[1][2]<<"\n"; cout<<C[2][0]<<" "<<C[2][1]<<" "<<C[2][2]<<"\n"; calaWIEP(Wiep,C,Wie); //地球速率的计算 calcG(C,h); //计算重力加速度g的初始值 if (I%8==0) { calczitai(Angl,T,alpha); //姿态计算 cout<<"Angl="<<Angl[0]<<" "<<Angl[1]<<" "<<Angl[2]<<"\n"; } else { calapos(POS,C);//位置计算 calcV(V);//计算地速 cout<<"POS="<<POS[0]<<" "<<POS[1]<<" "<<POS[2]<<"\n"; cout<<"V="<<V[0]<<" "<<V[1]<<"\n"; } } } } //计算矩阵T的初始值 //void INITTMAT() //{ //double T[3][3]={{1,0,0},{0,1,0},{0,0,1}}; //T[0][0]=1/(G*WIE*cos(LAT))*(Wibb[2]*fb[1]-Wibb[1]*fb[2]); //T[0][1]=1/(G*WIE*cos(LAT))*(Wibb[0]*fb[2]-Wibb[2]*fb[0]); //T[0][2]=1/(G*WIE*cos(LAT))*(Wibb[1]*fb[0]-Wibb[0]*fb[1]); //T[1][0]=fb[0]/G*tan(LAT)+Wibb[0]/WIE*1/cos(LAT); //T[1][1]=fb[1]/G*tan(LAT)+Wibb[1]/WIE*1/cos(LAT); //T[1][2]=fb[2]/G*tan(LAT)+Wibb[2]/WIE*1/cos(LAT); //T[2][0]=-fb[0]/G; //T[2][1]=-fb[1]/G; //T[2][2]=-fb[2]/G; // } //计算矩阵C的初始值 //POS[0]=ALPHA0;POS[1]=LAT0;POS[2]=LONG0; void INITCMAT(double C[3][3],double POS[3]) { C[0][0]=-sin(POS[0])*sin(POS[1])*cos(POS[2])-cos(POS[0])*sin(POS[2]); C[0][1]=-sin(POS[0])*sin(POS[1])*sin(POS[2])+cos(POS[0])*sin(POS[2]); C[0][2]=sin(POS[0])*cos(POS[1]); C[1][0]=-cos(POS[0])*sin(POS[1])*cos(POS[2])+sin(POS[0])*sin(POS[2]); C[1][1]=cos(POS[0])*sin(POS[1])*sin(POS[2])-sin(POS[0])*cos(POS[2]); C[1][2]=cos(POS[0])*cos(POS[1]); C[2][0]=cos(POS[1])*cos(POS[2]); C[2][1]=cos(POS[1])*sin(POS[2]); C[2][2]=sin(POS[1]); } //计算初始四元数Q void INITQ(double T[3][3],double Q[4]) { double qn[4]; double abs_q[4]; qn[1]=1+T[0][0]-T[1][1]-T[2][2]; //计算绝对值方程； qn[2]=1-T[0][0]+T[1][1]-T[2][2]; qn[3]=1-T[0][0]-T[1][1]+T[2][2]; qn[0]=1-qn[1]*qn[1]-qn[2]*qn[2]-qn[3]*qn[3]; abs_q[0]=sqrt(qn[0]); abs_q[1]=1/2*sqrt(qn[1]); abs_q[2]=1/2*sqrt(qn[2]); abs_q[3]=1/2*sqrt(qn[3]); Q[0]=abs_q[0]; //确定q0,q1,q2,q3符号； if (T[2][1]-T[1][2]>=0) Q[1]=abs_q[1]; else Q[1]=-abs_q[1]; if (T[0][2]-T[2][0]>=0) Q[2]=abs_q[2]; else Q[2]=-abs_q[2]; if (T[1][0]-T[0][1]>=0) Q[3]=abs_q[3]; else Q[3]=-abs_q[3]; } //计算矩阵C的初始值 //计算平台系地球速率WIEP的初始值 void INITWIEP(double C[3][3]) { double WIEP[3]; int i; for(i=0;i<3;i++) { WIEP[i]=WIE*C[i][2]; } } //计算平台系位置速率WEPP的初始值 void INITWEPP(double C[3][3], double V[2]) { double WEPP[2]; double conv_wepp[2][2]; conv_wepp[0][0]=-2*e*dRe*C[0][2]*C[1][2]; conv_wepp[0][1]=dRe*(1-e*C[2][2]*C[2][2]+2*e*C[1][2]*C[1][2]); conv_wepp[1][0]=dRe*(1-e*C[2][2]*C[2][2]+2*e*C[0][2]*C[0][2]); conv_wepp[1][1]=2*e*dRe*C[0][2]*C[1][2]; mulit_MAT_VEC1(WEPP,conv_wepp,V); } //计算重�