导航原理_捷联惯导系统.docVIP

导航原理作业（惯性导航部分） 姓名 班号 学号 成绩 批阅人 仿真题目 一枚导弹采用捷联惯性导航系统，三个速率陀螺仪Gx, Gy, Gz 和三个加速度计 Ax, Ay, Az 的敏感轴分别沿着着弹体坐标系的Xb, Yb, Zb轴。初始时刻该导弹处在北纬45.75度，东经126.63度。 第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。 初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。此后弹体坐标系 Xb-Yb-Zb 相对地理坐标系的转动如下： 首先，弹体绕 Zb（方位轴）转过 -10 度； 接着，弹体绕 Xb（俯仰轴）转过 15 度； 然后，弹体绕 Yb（滚动轴）转过 20 度； 最后弹体相对地面停止旋转。 请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计Ax, Ay, Az的输出。取重力加速度的大小g = 9.8m/s2。 第二种情形：导弹正在飞行中。 初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m，初始北向速度 1800 m/s，初始东向速度和垂直速度都为零。 陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表 0.00001弧度的角增量。加速度计输出的每个脉冲代表1μg，1g = 9.8m/s2。陀螺仪和加速度计输出的采样频率都为10Hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat中，内含一矩阵变量ga，有2000行，6列。每一行中的数据代表每个采样时刻三个陀螺Gx, Gy, Gz和三个加速度计Ax, Ay, Az的输出的脉冲数。格式如下表（前10行） Gx Gy Gz Ax Ay Az -3 0 -30 556000 77000 940977 -2 0 -30 556000 77000 940977 -2 0 -30 555999 77000 940977 -1 0 -30 556001 77000 940977 -2 -1 -30 556001 77001 940978 -3 0 -30 556000 77000 940977 -2 0 -30 556000 77001 940978 -2 0 -31 556000 77000 940976 -1 0 -30 556000 76999 940976 -2 0 -31 556000 77000 940977 将地球视为理想的球体，半径 6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。 请计算200秒后弹体到达的经纬度和高度，东向和北向速度； 请计算200秒后弹体相对当地地理坐标系的姿态四元数； 请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）； 请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。 程序设计说明及代码 第一种情形 方向余弦矩阵法 程序代码 clear;clc; thetax=15*pi/180;thetay=20*pi/180;thetaz=(-10)*pi/180; A0=[0;0;-9.8]; Theta=[0,-thetaz,thetay;thetaz,0,-thetax;-thetay,thetax,0]; theta0=sqrt(thetax^2+thetay^2+thetaz^2); S=(sin(theta0))/theta0;C=(1-cos(theta0))/theta0^2; CT=eye(3)+S*Theta+C*(Theta^2); CTN=inv(CT); A1=CTN*A0 输出结果 四元数法 程序代码 thetax=15*pi/180;thetay=20*pi/180;thetaz=-10*pi/180; theta0=sqrt(thetax^2+thetay^2+thetaz^2); Theta=[0,-thetax,-thetay,-thetaz; thetax,0,thetaz,-thetay; thetay,-thetaz,0,thetax; thetaz,thetay,-thetax,0]; Q=[1;0;0;0];A0=[0;0;0;-9.8]; QY=(cos(theta0/2)*eye(4)+1/theta0*sin(theta0/2)*Theta)*Q; QN=qinv(QY); A1=qmul(qmul(QN,A0),QY) 输出结果 第二种情形 程序代码 clear;clc; %------------------定义初始四元数----------