《北航导航课程设计捷联惯导》由会员分享，可在线阅读，更多相关《北航导航课程设计捷联惯导（20页珍藏版）》请在金锄头文库上搜索。
1、+十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十 *个*个*十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十+*/该程序为导航专业课程设计程序/该程序内容为：通过捷联惯性导航系统已知的陀螺仪和加速度计的输出通过四元数以及四阶龙格库塔法来计算经纬度以及姿态角（没有高度通道）/该程序版本为：1.4/该版本的更新为：数据改为实时读取，修正位置矩阵微分中错误/注意事项：该程序需要把惯性器件输出文件imu_ENU.txt）放入程序根目录中+十十十十十十十十十十十
2、十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十十 *个*个*个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个个/k* _&咨 7/*以 下是宏定乂部分*/# include# include# include# include# define initial_alignment 0初始对准# define R 6378137.0# define fe (1.0/298.257)# define G 9.7803# define deg (180/pi)# define r
3、ad (pi/180)# define pi 3.14159265358979323846# define wie (7292115e-11)# define t 0.020# define times 60000# define lambda。 (1.1615326e+002)*rad# define L0 (3.9813330e+001)*rad# define h0 7.0000000e+001# define Ve0 0# define Vn0 0# define Vu0 0# define fe0 0# define fn0 0# define fu0 0# define psi0
4、(8.9850004e+001)*rad# define theta0 (1.9113951e+000)*rad# define gamma0 (1.0572407e+000)*rad# define init_times 200条件编译命令，0：使用已有姿态角；1：使用/地球半径/地球椭率/重力加速度/弧度转化为度的系数/弧度转化为度的系数/圆周率/地球自转角速度/采样周期20ms/陀螺仪、加速度计以及外部高度输出数据数目/初始经度/初始纬度/初始高度/初始东向速度/初始北向速度/初始天向速度/初始东向加速度/初始北向加速度/初始天向加速度/初始偏航角/初始俯仰角/初始滚转角/初始对准时用的
5、数据平均的数/*以 下是常用全局变量的定 义*longdouble time;longdouble a3,w3;轴上角速度，y轴上角速度，z轴上角速度longdouble T33;longdouble C33;longdouble q4;longdouble psi,theta,gamma;longdouble lambda,L,h;longdouble wcmd3;longdouble Rm,Rn;longint i=0;longdouble dq4;longdouble W44;longdouble temp3;longdouble wepp3;longdouble wnbb3;longd
6、ouble Cwepp33;longdouble dC33;longdouble dV3;longdouble V3;longdouble wiep3;charflag1=1;charflag2=1;longdouble k14;longdouble k24;/x方向加速度，y方向加速度，z方向加速度，x姿态矩阵Tbn位置矩阵Cen四元数参数/偏航角、俯仰角、滚转角/经度、纬度、高度平台指令角速度子午面半径，卯酉圈半径计算次数/四元数参数的微分用于计算四元数参数微分的矩阵三行三列的矩阵与三行一列的矩阵相乘的结果/x,y,z/x,y,z用于C矩阵的更新/C矩阵的微分/速度的微分，x，y，z，分别
7、对应东，北，天/速度，x，y，z，分别对应东，北，天/地球自转角速度/用于四元数的四阶龙格库塔法积分的标志位/用于速度的二阶龙格库塔法积分的标志位longdouble k34;longdouble k53;/*；以下是程序声明部分*/void renew_T();/姿态矩阵更新void renew_attitude_angle();/姿态角更新void init_T();/姿态矩阵初始化void renew_dV();/加速度初始化void _3x3multiply3x3 longdouble a33,longdouble b33, longdouble temp233);/3x3矩阵与3x3
8、矩阵相乘void quaternion_differential();/计算四元数 Q 的微分longdouble integral_C(longdouble a, longdouble b);位置矩阵的积分(使用一阶)void forth_order_Runge_Kutta_method();/四阶龙格库塔法积分计算姿态角void second_order_Runge_Kutta_method();/二阶龙格库塔法积分计算速度void _3x3multiply3x1 longdouble a33,longdouble b3);/3x3矩阵与3行列向量乘积void outer_product_
9、of_vectorlongdouble a3,longdouble b3,longdouble c3);向量外积longdouble sign(longdouble i);/符号函数void C_differential();/位置矩阵微分void quaternion_normalize();/四元数归一化void renew_wcmd();/平台指令角速度更新void renew_wepp();void renew_wiep();/地球自转角速度更新void renew_R();/地球半径更新void renew1();/更新1void init_alignment();/初始对准（粗对准
10、）void init_T();/姿态矩阵初始化void init_C();/位置矩阵初始化void init_T_using_data();/使用已知数据初始化姿态矩阵void init();总初始化void renew_wnbb();/更新姿态矩阵速率void renew_T();/更新姿态矩阵void renew_dV();/更新加速度void renew_attitude_angle();/更新姿态角void renew_position();/更新位置void renew_C();/更新位置矩阵void renew2();/更新2/*迎 下是常用数学程序部 分*/void _3x3mu
11、ltiply3x3 longdouble a33,longdouble b33, longdouble temp233)/三行三列的矩阵与三行三列的矩阵相乘，结果写入emp233 中char s,d,k;for(s=0;s3;s+)for(d=0;d3;d+)temp2sd=0;for(k=0;k3;k+)temp2sd+=ask*bkd;计算四元数参数的微分void quaternion_differential() W00=0;W01=-wnbb0;W02=-wnbb1;W03=-wnbb2;W10=wnbb0;W11=0;W12=wnbb2;W13=-wnbb1;W20=wnbb1;W2
12、1=-wnbb2;W22=0;W23=wnbb0;W30=wnbb2;W31=wnbb1;W32=-wnbb0;W33=0;char j,k;for(j=0;j4;j+)dqj=0;for(j=0;j4;j+)for(k=0;k4;k+)dqj+=0.5*Wjk*qk;longdouble integral_C(longdouble a, longdouble b)用于位置矩阵更新的积分return (a+b*t/2);/一阶积分void forth_order_Runge_Kutta_method()/四阶龙格库塔法求四元数char p;longdouble tmp4;for(p=0;p4;p+)tmpp=qp;if (flag1=1)quaternion_differential();for(p=0;p4;p+)k1p=dqp;flag1+;elseif(flag1=2)for (p=0;p4;p+)qp=qp+t/2*k1p
