//捷联惯导
#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);
}
//计算重�
- 1
- 2
- 3
- 4
- 5
- 6
前往页