导航原理_捷联惯导系统

导航原理作业（惯性导航部分）姓名班号学号成绩批阅人一、仿真题目一枚导弹采用捷联惯性导航系统，三个速率陀螺仪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，初始北向速度1800m/s，初始东向速度和垂直速度都为零。陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表0.00001弧度的角增量。加速度计输出的每个脉冲代表1μg，1g=9.8m/s2。陀螺仪和加速度计输出的采样频率都为10Hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat中，内含一矩阵变量ga，有2000行，6列。每一行中的数据代表每个采样时刻三个陀螺Gx,Gy,Gz和三个加速度计Ax,Ay,Az的输出的脉冲数。格式如下表（前10行）GxGyGzAxAyAz-30-3055600077000940977-20-3055600077000940977-20-3055599977000940977-10-3055600177000940977-2-1-3055600177001940978-30-3055600077000940977-20-3055600077001940978-20-3155600077000940976-10-3055600076999940976-20-3155600077000940977ZbYbXb北东天将地球视为理想的球体，半径6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。（1）请计算200秒后弹体到达的经纬度和高度，东向和北向速度；（2）请计算200秒后弹体相对当地地理坐标系的姿态四元数；（3）请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）；（4）请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。二、程序设计说明及代码1.第一种情形（1）方向余弦矩阵法1)程序代码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*A02)输出结果（2）四元数法1)程序代码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)2)输出结果2.第二种情形（1）程序代码clear;clc;%------------------定义初始四元数------------------%Q=[1;0;0;0];%-----------------定义初始地球参数-----------------%R=6371000;g=9.8;omega=2*pi/(24*60*60);%-----------------定义初始位置参数-----------------%lambda=zeros(1,201);phi=zeros(1,201);h=zeros(1,201);lambda(1)=126.63;phi(1)=45.75;h(1,1)=200;%-----------------定义初始速度参数-----------------%VE=zeros(1,201);VN=zeros(1,201);VK=zeros(1,201);VE(1)=0;VN(1)=1800;VK(1)=0;%------------定义迭代和积分环节控制参数-------------%Dt=0.1;k=10;DT=k*Dt;K=200;%-------------------定义比力数组-------------------%fe=zeros(1,201);fn=zeros(1,201);fk=zeros(1,201);%--------------载入陀螺与加速度计输出---------------%load('E:\gaout.mat');%--------------------------------------------------%forN=1:K%位、速迭代开始forn=1:k%姿态子迭代开始thetax=0.00001*ga((N-1)*10+n,1);thetay=0.00001*ga((N-1)*10+n,2);thetaz=0.00001*ga((N-1)*10+n,3);%读取陀螺输出的角增量Theta=[0,-thetax,-thetay,-thetaz;thetax,0,thetaz,-thetay;thetay,-thetaz,0,thetax;thetaz,thetay,-thetax,0];theta=[thetax;thetay;thetaz];theta0=sqrt(thetax^2+thetay^2+thetaz^2);Q=(cos(theta0/2)*eye(4)+((1/theta0)*sin(theta0/2))*Theta)*Q;end%姿态子迭代结束omegae=-(VN(N)/R);%定义东向角速度分量omegan=(VE(N)/R)+omega*cos(phi(N)*pi/180);%定义北向角速度分量omegak=(VE(N)/R)*tan(phi(N)*pi/180)+omega*sin(phi(N)*pi/180);%定义地向角速度分量%--------------用地理坐标系的转动四元数修正载体姿态四元数--------------%gamma=[omegae;omegan;omegak]*DT;gamma0=sqrt(omegae^2+omegan^2+omegak^2);sn=gamma/gamma0;qy=[cos(gamma0/2);(sin(gamma0/2))*sn(1);(sin(gamma0/2))*sn(2);(sin(gamma0/2))*sn(3)];qn=qinv(qy);Q=qmul(qn,Q);QN=qinv(Q);%-------------------------------------------------------------------%fx=(1e-6)*g*mean(ga(((N-1)*10+1):(N*10),4));fy=(1e-6)*g*mean(ga(((N-1)*10+1):(N*10),5));fz=(1e-6)*g*mean(ga(((N-1)*10+1):(N*10),6));%读取加速度计测得的比力fb=[0;fx;fy;fz];%定义加速度计测得的比力向量fg=qmul(qmul(Q,fb),QN);fe(N)=fg(2,:);fn(N)=fg(3,:);fk(N)=fg(4,:);AE=zeros(1,201);AN=zeros(1,201);AK=zeros(1,201);%定义加速度向量%--------------------------计算载体相对加速度--------------------------%AE(N)=fe(N)+((VE(N)*VN(N))/R)*tan(phi(N)*pi/180)-(VE(N)/R+2*omega*cos(phi(N)*pi/180))*VK(N)+2*VN(N)*omega*sin(phi(N)*pi/180);AN(N)=fn(N)-2*VE(N)*omega*sin(phi(N)*pi/180)-(VE(N)^2/R)*tan(phi(N)*pi/180)-VN(N)*VK(N)/R;AK(N)=fk(N)+2*VE(N)*omega*cos(phi(N)*pi/180)+((VE(N)^2+VN(N)^2)/R)-g;%---------------------------------------------------------------------%%------------------------积分计算载体相对速度--------------------------%VE(N+1)=AE(N)*DT+VE(N);VN(N+1)=AN(N)*DT+VN(N);VK(N+1)=AK(N)*DT+VK(N);%---------------------------------------------------------------------%%---------------------------积分计算载体位置---------------------------%lambda(N+1)=(VE(N)/(R*cos(phi(N)*pi/180)))*DT/pi*180+lambda(N);phi(N+1)=(VN(N)/R)*DT/pi*180+phi(N);h(N+1)=VK(N)*DT+h(N);%---------------------------------------------------------------------%end%位、速迭代结束%-------------------------------输出所求结果-------------------------------%fprintf('200秒后弹体到达的经度为%7.4f\n',lambda(1,201));fprintf('200秒后弹体到达的纬度为%6.4f\n',phi(1,201));fprintf('200秒后弹体到达的高度为%6.4f\n',h(1,201));fprintf('200秒后弹体到达的东向速度为%8.4f\n',VE(1,201));fprintf('200秒后弹体到达的北向速度为%8.4f\n',VN(1,201));fprintf('200秒后弹体相对当地地理坐标系的姿态四元数为\t[%5.4f\t%5.4f\t%5.4f\t%5.4f]\n',Q);%-------------------------------------------------------------------------%%---------------------------------绘制图像--