北航导航课程设计捷联惯导.docx

《北航导航课程设计捷联惯导.docx》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、/***********************************************************************************************************************************///该程序为导航专业课程设计程序//该程序内容为：通过捷联惯性导航系统已知的陀螺仪和加速度计的输出通过四元数以及四阶龙格库塔法来计算经纬度以及姿态角（没有高度通道）//该程序版本为：1.4//该版本的更新为：数据改为实时读取，修正位置矩阵微分中错误//注意事项：该程序

2、需要把惯性器件输出文件（imu_ENU.txt）放入程序根目录中/***********************************************************************************************************************************//********************************以下是宏定义部分****************************/#include#include

3、>#include#include#defineinitial_alignment0//条件编译命令，0：使用已有姿态角；1：使用初始对准#defineR6378137.0//地球半径#definefe(1.0/298.257)//地球椭率#defineG9.7803//重力加速度#definedeg(180/pi)//弧度转化为度的系数#definerad(pi/180)//弧度转化为度的系数#definepi3.14159265358979323846//圆周率#definewie(72

4、92115e-11)//地球自转角速度#definet0.020//采样周期20ms#definetimes60000//陀螺仪、加速度计以及外部高度输出数据数目#definelambda0(1.1615326e+002)*rad//初始经度#defineL0(3.9813330e+001)*rad//初始纬度#defineh07.0000000e+001//初始高度#defineVe00//初始东向速度#defineVn00//初始北向速度#defineVu00//初始天向速度#definefe00//初始东向加速度#defi

5、nefn00//初始北向加速度#definefu00//初始天向加速度#definepsi0(8.9850004e+001)*rad//初始偏航角#definetheta0(1.9113951e+000)*rad//初始俯仰角#definegamma0(1.0572407e+000)*rad//初始滚转角#defineinit_times200//初始对准时用的数据平均的数/*****************以下是常用全局变量的定义******************/longdoubletime;longdoublea[3],

6、w[3];//x方向加速度，y方向加速度，z方向加速度，x轴上角速度，y轴上角速度，z轴上角速度longdoubleT[3][3];//姿态矩阵TbnlongdoubleC[3][3];//位置矩阵Cenlongdoubleq[4];//四元数参数longdoublepsi,theta,gamma;//偏航角、俯仰角、滚转角longdoublelambda,L,h;//经度、纬度、高度longdoublewcmd[3];//平台指令角速度longdoubleRm,Rn;//子午面半径，卯酉圈半径longinti=0;//计算次数

7、longdoubledq[4];//四元数参数的微分longdoubleW[4][4];//用于计算四元数参数微分的矩阵longdoubletemp[3];//三行三列的矩阵与三行一列的矩阵相乘的结果longdoublewepp[3];//x,y,zlongdoublewnbb[3];//x,y,zlongdoubleCwepp[3][3];//用于C矩阵的更新longdoubledC[3][3];//C矩阵的微分longdoubledV[3];//速度的微分，x，y，z，分别对应东，北，天longdoubleV[3];//速度

8、，x，y，z，分别对应东，北，天longdoublewiep[3];//地球自转角速度charflag1=1;//用于四元数的四阶龙格库塔法积分的标志位charflag2=1;//用于速度的二阶龙格库塔法积分的标志位longdoublek1[4];longdou