哈工大导航原理大作业

./《导航原理》作业<惯性导航部分>班级1104103学号11104103一、题目要求AfighterequippedwithSINSisinitiallyatthepositionofNLandEL,stationaryonamotionlesscarrier.Threegyros,,,andthreeaccelerometers,,,areinstalledalongtheaxes,,ofthebodyframerespectively.Case1:stationaryonboardtestThebodyframeofthefighterinitiallycoincideswiththegeographicalframe,asshowninthefigure,withitspitchingaxispointingtotheeast,rollingaxistothenorth,andazimuthaxisupward.Thenthebodyofthefighterismadetorotatestepbysteprelativetothegeographicalframe.aroundaroundaroundAfterthat,thebodyofthefighterstopsrotating.Youarerequiredtocomputethefinaloutputofthethreeaccelerometersonthefighter,usingbothDCMandquaternionrespectively,andignoringthedeviceerrors.Itisknownthatthemagnitudeofgravityaccelerationis.Case2:flightnavigationInitially,thefighterisstationaryonthemotionlesscarrierwithitsboard25mabovethesealevel.Itspitchingandrollingaxesarebothinthelocalhorizon,anditsrollingaxisisonthenorthbyeast,parallelwiththerunwayonboard.Thenthefighteracceleratealongtherunwayandtakeofffromthecarrier.Theoutputofthegyrosandaccelerometersarebothpulsenumbers,Eachgyropulseisanangularincrementof,andeachaccelerometerpulseis,with.Thegyrooutputfrequencyis10Hz,andtheaccelerometer’sis1Hz.Theoutputofgyrosandaccelerometerswithin5400sarestoredinMATLABdatafilesnamedgout.matandaout.mat,containingmatricesgmofandamofrespectively.Theformatofdataasshowninthetables,with10rowsofeachmatrixselected.Eachrowrepresentstheoutofthetypeofsensorsateachsampletime.TheEarthcanbeseenasanidealsphere,withradius6368.00kmandspinningrate,Theerrorsofsensorsareignored,soistheeffectofheightonthemagnitudeofgravity.Theoutputofthegyrosaretointegratedevery0.1s.Therotationofgeographicalframeistobeupdatedevery1s,soarethevelocitiesandpositionofthefigure.Youarerequiredto:Computethefinalattitudequaternion,longitude,latitude,height,andeast,north,verticalvelocitiesofthefighter.Computethetotalhorizontaldistancetraveledbythefighter.Drawthelatitude-versus-longitudetrajectoryofthefighter,withhorizontallongitudeaxis.Drawthecurveoftheheightoffighter,withhorizontaltimeaxis.二、Case1解答2.1方向余弦阵法绕Xb轴转过ψ绕Yb轴转过绕Zb轴转过所以变换后的坐标由于初始时刻有所以计算得三个加速度计的输出分别是2计算程序见附录一2.2四元数法〔1绕Xb轴转过ψ〔2绕Yb轴转过〔3绕Zb轴转过则合成四元数合成四元数的逆由公式计算得三个加速度计的输出分别是2由两种计算方法的计算结果可以看出,方向余弦阵法和四元数法的计算结果是一致的。计算程序见附录二三、Case2解答3.1程序流程图3.2源程序详见附录三3.3仿真及结果3.3.1运行程序可得：〔1战斗机相对当地地理坐标系的姿态四元数为：〔2经度=纬度=高度=〔3战斗机的东部,北部和垂直速度〔4战斗机总水平距离为：运行程序,绘制纬度-经度战斗机轨迹〔以经度为横轴,纬度为纵轴；如图：3.3.3运行程序,绘制战斗机的高度的曲线〔以时间为横轴,高度为纵轴,如图：四、心得体会通过本次的大作业,我对比力、四元数等惯性导航技术中涉及的基本概念有了更深的理解,同时再次复习方向余弦矩阵法和四元数法求解捷联惯导系统,更加熟练地掌握其算法。除此之外,本次大作业中所有程序均要涉及Matlab编程,在编写程序时,四元数的初始化非常重要,如果四元数的初始化错误,会导致所有的运算结果错误。在编程过程中,每当遇到问题,我就会上网查一些资料,并与周边同学进行深入讨论,学习并解决问题。这进一步提高了我的自学能力和对知识综合应用的能力。五、参考/协助确认本文是根据本人研究成果,并参考相关文献资料整合而成,非本人完全独立研究完成,编程部分很大程度上参考了百度百科及周边同学的研究成果。另外,报告形式上借鉴了上届学长学姐的形式。本人对作业中的某些结论不负完全责任,特此声明。附录一C1=[1,0,00,cos<10/180*pi>,sin<10/180*pi>0,-sin<10/180*pi>,cos<10/180*pi>];%第一次转动方向余弦阵C2=[cos<30/180*pi>,0,-sin<30/180*pi>0,1,0sin<30/180*pi>,0,cos<30/180*pi>];%第二次转动方向余弦阵C3=[cos<-50/180*pi>,sin<-50/180*pi>,0-sin<-50/180*pi>,cos<-50/180*pi>,00,0,1];%第三次转动方向余弦阵A=C3*C2*C1*[0;0;9.8]%计算转动后加速度的输出norm<A>%检验输出是否正确附录二q1=[cos<10/360*pi>;0;0;sin<10/360*pi>];%第一次转动四元数q2=[cos<30/360*pi>;sin<30/360*pi>;0;0];%第二次转动四元数q3=[cos<-50/360*pi>;0;sin<-50/360*pi>;0];%第三次转动四元数r=quml<q1,q2>;%计算四元数乘积q=quml<r,q3>;A1=[q<1>q<2>q<3>q<4>-q<2>q<1>q<4>-q<3>-q<3>-q<4>q<1>q<2>-q<4>q<3>-q<2>q<1>]A2=[q<1>-q<2>-q<3>-q<4>q<2>q<1>q<4>-q<3>q<3>-q<4>q<1>q<2>q<4>q<3>-q<2>q<1>]A=A1*A2;%G=A*[0;0;0;9.8]%计算转动后的弹体加速度计的输出附录三%已知参数k=10;K=5400;T=1;Gyro_pulse=0.1/3600/180*pi;Acc_pulse=9.8/1000000;R=6368000;g=9.8;W_earth=0.00007292;%初始导航参数和地球参数Q=zeros<5401,4>;%定义变换四元数矩阵Q<1,:>=[cos<<-45/2>/180*pi>,0,0,sin<<-45/2>/180*pi>];Longitude=zeros<1,5401>;%定义长度为5400的经度数组Latitude=zeros<1,5401>;%定义长度为5400的纬度数组Height=zeros<1,5401>;%定义长度为5400的高度数组Longitude<1>=122;%初始化经度,纬度,高度Latitude<1>=35;Height<1>=25;%定义速度矩阵,供画图用Ve=zeros<1,5401>;%东向速度Vn=zeros<1,5401>;%北向速度Vu=zeros<1,5401>;%天向速度X=zeros<1,5401>;%定义加速度矩阵fe=zeros<1,5400>;fn=zeros<1,5400>;fu=zeros<1,5400>;FE=zeros<1,5400>;FN=zeros<1,5400>;FU=zeros<1,5400>;loadgout.mat;loadaout.mat;forN=1:K%位置、速度迭代q=zeros<11,4>;q<1,:>=Q<N,:>;forn=1:k%姿态迭代w=Gyro_pulse*gm<<N-1>*10+n,1:3>;w_mod=quatmod<[w,0]>;W=[0,-w<1>,-w<2>,-w<3>;w<1>,0,w<3>,-w<2>;w<2>,-w<3>,0,w<1>;w<3>,w<2>,-w<1>,0];I=eye<4>;S=1/2-w_mod^2/48;C=1-w_mod^2/8+w_mod^4/384;q<n+1,:>=<<C*I+S*W>*q<n,:>'>';endQ<N+1,:>=q<n+1,:>;%地理坐标系的转动角速度分量WE=-Vn<N>/R;WN=Ve<N>/R+W_earth*cos<Latitude<N>/180*pi>;WU=Ve<N>/R*tan<Latitude<N>/180*pi>+W_earth*sin<Latitude<N>/180*pi>;Gama=[WE,WN,WU]*T;%用地理坐标系的转动四元数Gama_mod=quatmod<[WE,WN,WU,0]>;n=Gama/Gama_mod;Qg=[cos<Gama_mod/2>,sin<Gama_mod/2>*n];Q<N+1,:>=quatmultiply<quatinv<Qg>,Q<N+1,:>>;%加速度计测得的比力fb=Acc_pulse*am<N,1:3>;f1=quatmultiply<Q<N+1,:>,[0,fb]>;fg=quatmultiply<f1,quatinv<Q<N+1,:>>>;fe<N>=fg<2>;fn<N>=fg<3>;fu<N>=fg<4>;%计算载体的相对加速度FE<N>=fe<N>+Ve<N>*Vn<N>/R*tan<Latitude<N>/180*pi>-<Ve<N>/R+2*W_earth*cos<Latitude<N>/180*pi>>*Vu<N>+2*Vn<N>*W_earth*sin<Latitude<N>/180*pi>;FN<N>=fn<N>-2*Ve<N>*W_earth*sin<Latitude<N>/180*pi>-Ve<N>^2/R*tan<Latitude<N>/180*pi>-Vn<N>*Vu<N>/R;FU<N>=fu<N>+2*Ve<N>*W_earth*cos<Latitude<N>/180*pi>+<Ve<N>^2+Vn<N>^2>/R-g;%积分计算载体的相对速度Ve<N+1>=FE<N>*T+Ve<N>;