北航导航课程设计捷联惯导Word文件下载.docx
- 文档编号:19453700
- 上传时间:2023-01-06
- 格式:DOCX
- 页数:21
- 大小:20.79KB
北航导航课程设计捷联惯导Word文件下载.docx
《北航导航课程设计捷联惯导Word文件下载.docx》由会员分享,可在线阅读,更多相关《北航导航课程设计捷联惯导Word文件下载.docx(21页珍藏版)》请在冰豆网上搜索。
#definetheta0(1.9113951e+000)*rad//初始俯仰角
#definegamma0(1.0572407e+000)*rad//初始滚转角
#defineinit_times200//初始对准时用的数据平均的数
/*****************以下是常用全局变量的定义******************/
longdoubletime;
longdoublea[3],w[3];
//x方向加速度,y方向加速度,z方向加速度,x轴上角速度,y轴上角速度,z轴上角速度
longdoubleT[3][3];
//姿态矩阵Tbn
longdoubleC[3][3];
//位置矩阵Cen
longdoubleq[4];
//四元数参数
longdoublepsi,theta,gamma;
//偏航角、俯仰角、滚转角
longdoublelambda,L,h;
//经度、纬度、高度
longdoublewcmd[3];
//平台指令角速度
longdoubleRm,Rn;
//子午面半径,卯酉圈半径
longinti=0;
//计算次数
longdoubledq[4];
//四元数参数的微分
longdoubleW[4][4];
//用于计算四元数参数微分的矩阵
longdoubletemp[3];
//三行三列的矩阵与三行一列的矩阵相乘的结果
longdoublewepp[3];
//x,y,z
longdoublewnbb[3];
longdoubleCwepp[3][3];
//用于C矩阵的更新
longdoubledC[3][3];
//C矩阵的微分
longdoubledV[3];
//速度的微分,x,y,z,分别对应东,北,天
longdoubleV[3];
//速度,x,y,z,分别对应东,北,天
longdoublewiep[3];
//地球自转角速度
charflag1=1;
//用于四元数的四阶龙格库塔法积分的标志位
charflag2=1;
//用于速度的二阶龙格库塔法积分的标志位
longdoublek1[4];
longdoublek2[4];
longdoublek3[4];
longdoublek5[3];
/******************以下是程序声明部分***********************/
voidrenew_T();
//姿态矩阵更新
voidrenew_attitude_angle();
//姿态角更新
voidinit_T();
//姿态矩阵初始化
voidrenew_dV();
//加速度初始化
void_3x3multiply3x3(longdoublea[3][3],longdoubleb[3][3],longdoubletemp2[3][3]);
//3x3矩阵与3x3矩阵相乘
voidquaternion_differential();
//计算四元数Q的微分
longdoubleintegral_C(longdoublea,longdoubleb);
//位置矩阵的积分(使用一阶)
voidforth_order_Runge_Kutta_method();
//四阶龙格库塔法积分计算姿态角
voidsecond_order_Runge_Kutta_method();
//二阶龙格库塔法积分计算速度
void_3x3multiply3x1(longdoublea[3][3],longdoubleb[3]);
//3x3矩阵与3行列向量乘积
voidouter_product_of_vector(longdoublea[3],longdoubleb[3],longdoublec[3]);
//向量外积
longdoublesign(longdoublei);
//符号函数
voidC_differential();
//位置矩阵微分
voidquaternion_normalize();
//四元数归一化
voidrenew_wcmd();
//平台指令角速度更新
voidrenew_wepp();
voidrenew_wiep();
//地球自转角速度更新
voidrenew_R();
//地球半径更新
voidrenew1();
//更新1
voidinit_alignment();
//初始对准(粗对准)
voidinit_C();
//位置矩阵初始化
voidinit_T_using_data();
//使用已知数据初始化姿态矩阵
voidinit();
//总初始化
voidrenew_wnbb();
//更新姿态矩阵速率
//更新姿态矩阵
//更新加速度
//更新姿态角
voidrenew_position();
//更新位置
voidrenew_C();
//更新位置矩阵
voidrenew2();
//更新2
/******************以下是常用数学程序部分*******************/
void_3x3multiply3x3(longdoublea[3][3],longdoubleb[3][3],longdoubletemp2[3][3])
//三行三列的矩阵与三行三列的矩阵相乘,结果写入temp2[3][3]中
{
chars,d,k;
for(s=0;
s<
3;
s++)
{
for(d=0;
d<
d++)
{
temp2[s][d]=0;
for(k=0;
k<
k++)
{
temp2[s][d]+=a[s][k]*b[k][d];
}
}
}
}
voidquaternion_differential()//计算四元数参数的微分
W[0][0]=0;
W[0][1]=-wnbb[0];
W[0][2]=-wnbb[1];
W[0][3]=-wnbb[2];
W[1][0]=wnbb[0];
W[1][1]=0;
W[1][2]=wnbb[2];
W[1][3]=-wnbb[1];
W[2][0]=wnbb[1];
W[2][1]=-wnbb[2];
W[2][2]=0;
W[2][3]=wnbb[0];
W[3][0]=wnbb[2];
W[3][1]=wnbb[1];
W[3][2]=-wnbb[0];
W[3][3]=0;
charj,k;
for(j=0;
j<
4;
j++)
dq[j]=0;
for(k=0;
dq[j]+=0.5*W[j][k]*q[k];
longdoubleintegral_C(longdoublea,longdoubleb)
//用于位置矩阵更新的积分
return(a+b*t/2);
//一阶积分
voidforth_order_Runge_Kutta_method()//四阶龙格库塔法求四元数
charp;
longdoubletmp[4];
for(p=0;
p<
p++)
tmp[p]=q[p];
if(flag1==1)
quaternion_differential();
//微分得到dq
for(p=0;
k1[p]=dq[p];
flag1++;
else
if(flag1==2)
for(p=0;
q[p]=q[p]+t/2*k1[p];
quaternion_differential();
k2[p]=dq[p];
q[p]=tmp[p];
q[p]=q[p]+t/2*k2[p];
k3[p]=dq[p];
flag1++;
else
if(flag1==3)
for(p=0;
{
q[p]=q[p]+t*k3[p];
}
quaternion_differential();
q[p]=tmp[p]+t/6*(k1[p]+2*k2[p]+2*k3[p]+dq[p]);
//积分
flag1=2;
k1[p]=dq[p];
quaternion_normalize();
renew_T();
voidsecond_order_Runge_Kutta_method()//用二阶龙格库塔法更新速度
longdoubletmp[3];
tmp[p]=V[p];
if(flag2==1)
renew_dV();
k5[p]=dV[p];
flag2++;
V[p]=V[p]+t/2*dV[p];
V[p]=tmp[p]+t/4*(k5[p]+dV[p]);
//积分
void_3x3multiply3x1(longdoublea[3][3],longdoubleb[3])
//三行三列的矩阵与三行一列的矩阵相乘,结果写入temp[3]中
chari,j;
longdoublec[3];
for(i=0;
i<
i++)
c[i]=b[i];
temp[i]=0;
for(j=0;
temp[i]+=a[i][j]*c[j];
voidouter_product_of_vector(longdoublea[3],longdoubleb[3],longdoublec[3])
//c=axb,向量外积
longdoublek[3];
chari;
k[i]=a[i];
c[0]=k[1]*b[2]-k[2]*b[1];
c[1]=k[2]*b[0]-k[0]*b[2];
c[2]=k[0]*b[1]-k[1]*b[0];
longdoublesign(longdoublei)//符号函数
if(i>
=0)
return1.0;
return-1.0;
voidC_differential()//位置矩阵的微分
Cwepp[0][0]=0;
Cwepp[0][1]=wepp[2];
Cwepp[0][2]=-wepp[1];
Cwepp[1][0]=-wepp[2];
Cwepp[1][1]=0;
Cwepp[1][2]=wepp[0];
Cwepp[2][0]=wepp[1];
Cwepp[2][1]=-wepp[0];
Cwepp[2][2]=0;
_3x3multiply3x3(Cwepp,C,dC);
//结果写入dC[3][3]
voidquaternion_normalize()//四元数归一化
longdoubletemp;
temp=sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
q[0]=q[0]/temp;
q[1]=q[1]/temp;
q[2]=q[2]/temp;
q[3]=q[3]/temp;
/***********************************以下是更新1部分内容***************************/
voidrenew_wcmd()//平台指令角速度更新(wipp)
wcmd[0]=wepp[0]+wiep[0];
wcmd[1]=wiep[1]+wepp[1];
wcmd[2]=wiep[2]+wepp[2];
voidrenew_wepp()
wepp[0]=-V[1]/(h+Rm);
wepp[1]=V[0]/(Rn+h);
wepp[2]=V[0]/(Rn+h)*tan(L);
voidrenew_wiep()//更新地球自转角速度
wiep[0]=0;
wiep[1]=wie*cos(L);
wiep[2]=wie*sin(L);
voidrenew_R()//地球半径更新
Rm=R*(1-2*fe+3*fe*sin(L)*sin(L));
//更新子午面半径
Rn=R*(1+fe*sin(L)*sin(L));
//更新卯酉圈半径
voidrenew1()//更新1(地理信息和平台指令角速度更新)
renew_R();
//更新地球半径
renew_wepp();
renew_wiep();
//更新地球自转角速度
renew_wcmd();
//更新平台指令角速度
/*****************************以下是初始化部分*******************************/
voidinit_alignment()//初始对准(粗对准)
longdoubleVbt[3][3]={0,0,0,0,0,0,0,0,0};
longdoubleVnt_inverse[3][3]={0,0,0,0,0,0,0,0,0};
inti;
longdoublefx=0,fy=0,fz=0,Wx=0,Wy=0,Wz=0,a,b,c,d,e,f,g;
FILE*tmp;
if((tmp=fopen("
imu_ENU.txt"
"
r"
))==NULL)
//打开文件,读入数据模式
printf("
无法打开文件imu_ENU.txt\n"
);
exit(0);
init_times;
i++)//求一段时间的平均值来算初始姿态角
fscanf(tmp,"
%lf%lf%lf%lf%lf%lf%lf"
&
a,&
b,&
c,&
d,&
e,&
f,&
g);
//时间,x轴加速度,y轴加速度,z轴加速度,x轴角速度,y轴角速度,z轴角速度
fx-=b;
//加速度均取负值
fy-=c;
fz-=d;
Wx+=e;
Wy+=f;
Wz+=g;
fclose(tmp);
//关闭文件指针
fx=fx/init_times;
fy=fy/init_times;
fz=fz/init_times;
Wx=Wx/init_times;
Wy=Wy/init_times;
Wz=Wz/init_times;
Vbt[0][0]=fx;
Vbt[0][1]=fy;
Vbt[0][2]=fz;
Vbt[1][0]=Wx;
Vbt[1][1]=Wy;
Vbt[1][2]=Wz;
Vbt[2][0]=fy*Wz-fz*Wy;
Vbt[2][1]=fz*Wx-fx*Wz;
Vbt[2][2]=fx*Wy-fy*Wx;
Vnt_inverse[0][2]=1/(G*wie*cos(L0));
Vnt_inverse[1][0]=tan(L0)/G;
Vnt_inverse[1][1]=1/(wie*cos(L0));
Vnt_inverse[2][0]=-1/G;
_3x3multiply3x3(Vnt_inverse,Vbt,T);
//求出姿态矩阵,此姿态矩阵未经正交化
renew_attitude_angle();
//计算姿态角(正交化)
init_T();
//姿态矩阵初始化,至此,正交化完成
voidinit_T()//姿态矩阵初始化
T[0][0]=cos(gamma)*cos(psi)-sin(gamma)*sin(theta)*sin(psi);
//通过姿态角求姿态矩阵
T[0][1]=-cos(theta)*sin(psi);
T[0][2]=sin(gamma)*cos(psi)+cos(gamma)*sin(theta)*sin(psi);
T[1][0]=cos(gamma)*sin(psi)+sin(gamma)*sin(theta)*cos(psi);
T[1][1]=cos(theta)*cos(psi);
T[1][2]=sin(gamma)*sin(psi)-cos(gamma)*sin(theta)*cos(psi);
T[2][0]=-sin(gamma)*cos(theta);
T[2][1]=sin(theta);
T[2][2]=cos(gamma)*cos(theta);
q[1]=sign(T[2][1]-T[1][2])*0.5*sqrt(1+T[0][0]-T[1][1]-T[2][2]);
//求四元数各项
q[2]=sign(T[0][2]-T[2][0])*0.5*sqrt(1-T[0][0]+T[1][1]-T[2][2]);
q[3]=sign(T[1][0]-T[0][1])*0.5*sqrt(1-T[0][0]-T[1][1]+T[2][2]);
q[0]=sqrt(1-q[1]*q[1]-q[2]*q[2]-q[3]*q[3]);
quaternion_normalize();
//四元数正交化
renew_T();
//重新通过四元数得到姿态矩阵
voidinit_C()//位置矩阵初始化
C[0][0]=-sin(lambda);
C[0][1]=cos(lambda);
C[0][2]=0;
C[1][0]=-sin(L)*cos(lambda);
C[1][1]=-sin(L)*sin(lambda);
C[1][2]=cos(L);
C[2][0]=cos(lambda)*cos(L);
C[2][1]=cos(L)*sin(lambda);
C[2][2]=s
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 北航 导航 课程设计 捷联惯导