哈工大導航原理、慣性技術大作業(yè)

AssignmentofPrinciplesofNavigation

(INS)MyChinese白赫MyClassNo.1204201MyNo.1120110412Dec28th,2015Contents1.TheFirstTaskstationarytest 31.1Taskdescription 31.2TheProcessofSolution 31.3ProgrammingCodesinMATLAB 41.4AnalysisofResult 42.TheSecondTaskflighttest 5description 52.2TheProcessofSolution 62.3ProgrammingCodesinMATLAB 92.4AnalysisofResult 123.Reflectiononthesimulation 121.TheFirstTaskstationarytest1.1TaskdescriptionAmissileequippedwithSINSisinitiallyatthepositionof46oNLand123oEL,stationaryonalaunchpad.Threegyros,GX,GY,GZ,andthreeaccelerometers,AX,AY,AZ,areinstalledalongtheaxesXb,Yb,Zbofitsbodyframerespectively.Thebodyframeofthemissileinitiallycoincideswiththegeographicalframe,asshowninthefigure,withitspitchingaxisXbpointingtotheeast,rollingaxisYbtothenorth,andazimuthaxisZbupward.Thenthebodyofthemissileismadetorotatein3steps:(1)-22°aroundXb(2)78°aroundYb(3)-16°aroundZb

Up ZbXbEast

NorthYbAfterthat,thebodyofthemissilestopsrotating.Youarerequiredtocomputethefinaloutputsofthethreeaccelerometersonthemissile,usingquaternionandignoringthedeviceerrors.Itisknownthatthemagnitudeofgravityaccelerationisg=9.8m/s2.1.2TheProcessofSolutionFirstrotation:aroundaxisXfor,thequaternionis:Secondrotation:aroundaxisYfor,thequaternionis:Thirdrotation:aroundaxisZfor,thequaternionis:Thecombinationquaternionandis:Thusthefinalcoordinateis：Thefinaloutputoftheaccelerometersonthemissileare:1.3ProgrammingCodesinMATLAB%The1/2angleofthreerotationsAngle_X=(-22/2)/180*pi;Angle_Y=(78/2)/180*pi;Angle_Z=(-16/2)/180*pi;q1=[cos(Angle_X),sin(Angle_X),0,0];%Thequaternionoffirstrotationq2=[cos(Angle_Y),0,sin(Angle_Y),0];%Thequaternionofsecondrotationq3=[cos(Angle_Z),0,0,sin(Angle_Z)];%ThequaternionofthirdrotationA0=[0,0,0,9.8];%ThequaternionofAccintheoldframe%Themultiplicationofquaternion(q=q1q2q3)p=quatmultiply(q1,q2);q=quatmultiply(p,q3);%Theinverseofquaternionqqinv=quatinv(q);%TheoutputofAccinthefighterA1=quatmultiply(qinv,A0);A2=quatmultiply(A1,q)1.4AnalysisofResultWehavelearnedinsection1.1ofUnit10,successiverotationscanbecombinedintoasinglerotation.Thebodyofthismissileismadetorotatein3steps.Thenthequaternionqforthecombinedrotationcanbeobtainedusingq.Now,wehavearotatingframe,andafixedvector,namelyaccelerometersvectorinthiscircumstance.Thecoordinatesofthevectorcanbetransformedfromtheoldframetonewframeusingimages,thatis,hefinaloutputsthreeonthemissile.2.TheSecondTaskflighttestdescriptionInitially,themissileisstationaryonthelaunchpad,400mabovethesealevel.Itsrollingaxisisverticalup,anditspitchingaxisistotheeast.Thenthemissileisfiredup.Thethegyrosandarenumbers.gyroisanincrementof0.01arc-sec,eachiswithg=Thegyrooutputistheacis10Hz.Theandaccelerometersareinadataimu.mat,matricesgm263000×3amTheformatofdataisasshowninthe10rowsofeachmatrixselected.rowoftheofattime.AXAYAZ-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-303759452365312-15643989-352289653180268-15924172-400819853995223-16204356-449350154810179-16484539-497880355625135-16764723-497880355625135-16764723-1872-3611-1945-720-1-2014-10800-2087-14390-2160-17990-2234-21601-2303-25201-2375-28781-2450-32390-2522-35992TheEarthcanbeseenasanidealsphere,withradius6371.00kmspinningrate7.292×10-5rad/s,Theerrorsofthesensorsareignored,soistheeffectofheightonthemagnitudeofgravity.Theoutputsofthegyrosaretobeintegratedevery0.005s.Therotationofthegeographicalframeistobeupdatedevery0.1s,soarethevelocitiesandpositionsofthemissile.areComputefinalnion,height,andverticaloftheComputebytheofmissile,axis.thethethemissile,axis.2.2TheProcessofSolutionThesimplifiednavigationalgorithmforSINSisasshownintheFigure1.Figure1:SimplifiednavigationalgorithmforSINSResults:(1)ThefinalattitudequaternionThefinallongitude,latitudeandheightofthemissileareasfollow:Longitude=,Latitude=Height=mTheeast,northandverticalofthefighteratlastare:(2)Thetotalhorizontaldistancetraveledbythemissileis:.Andthecurveofthehorizontaldistanceofthemissile,withhorizontaltimeaxisisasshownintheFigure2.Figure2:Thecurveofthehorizontaldistanceofthemissile(3)Thelatitude-versus-longitudetrajectoryofthemissileisasshownintheFigure3.Figure3:Thelatitude-versus-longitudetrajectoryofthemissile(4)Thecurveoftheheightofthemissile,withhorizontaltimeaxisisasshownintheFigure4.Figure4:Thecurveoftheheightofthemissile2.3ProgrammingCodesinMATLAB%initializetheparameterofsystemk=20;K=13150;T=0.1;Gyro_pulse=0.01/3600/180*pi;Acc_pulse=9.8/10000000;R=6371000;g=9.8;W_earth=0.00007292;Q=zeros(13151,4);Q(1,:)=[cos((90/2)/180*pi),sin((90/2)/180*pi),0,0];%initialquaternion%defineandinitializethematrixofLongitude,LatitudeandHeightLongitude=zeros(1,13151);Latitude=zeros(1,13151);Height=zeros(1,13151);Longitude(1)=123;Latitude(1)=46;Height(1)=400;%defineandinitializethematrixofvelocitywithlengthof13150Ve=zeros(1,13151);%eastvelocityVn=zeros(1,13151);%northvelocityVu=zeros(1,13151);%upwardvelocityX=zeros(1,13151);%distanceofhorizon%defineandinitializethematrixofspecificforcewithlengthof13150fe=zeros(1,13151);fn=zeros(1,13151);fu=zeros(1,13151);FE=zeros(1,13151);FN=zeros(1,13151);FU=zeros(1,13151);%loadthedataprovidedbyteacherloadimu.mat;forN=1:K%startingiterationforpositionandxelocityq=zeros(21,4);q(1,:)=Q(N,:);forn=1:k%startingiterationforattitudew=Gyro_pulse*gm((N-1)*20+n,1:3);%angularincrementoutputofgyrow_mod=quatmod([w,0]);W=[0,-w(1),-w(2),-w(3);w(1),0,w(3),-w(2);%angularincrementmatrixw(2),-w(3),0,w(1);w(3),w(2),-w(1),0];I=eye(4);%4orderimplementation%TaylorseriesapproximatelyS=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,:)')';end%endoftheinnerloop,namelyattitudeloopQ(N+1,:)=q(n+1,:);%angularratesofgeographicalframeWE=-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,:));%updatingattitudeofmissleusingrotationofgeographicalframe%specificforcemeasuredbyAcc.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);%computingtherelativeaccelarationofvehicleFE(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;%integratedforvelocityVe(N+1)=FE(N)*T+Ve(N);Vn(N+1)=FN(N)*T+Vn(N);Vu(N+1)=FU(N)*T+Vu(N);%updatingthepositionLatitude(N+1)=(Vn(N)+Vn(N+1))/2*T/R/pi*180+Latitude(N);Longitude(N+1)=(Ve(N)+Ve(N+1))/2*T/(R*cos(Latitude(N)/180*pi))/pi*180+Longitude(N);Height(N+1)=(Vu(N)+Vu(N+1))/2*T+Height(N);%computingthedistanceofhorizonXe=(Ve(N)+Ve(N+1))/2*T;Xn=(Vn(N)+Vn(N+1))/2*T;X(N+1)=X(N)+sqrt(Xe^2+Xn^2);end%drawthecurveofLongitudeandLatitudefigure(1);xlabel('經(jīng)度/度');ylabel('緯度/度');gridon;holdon;plot(Longitude,Latitude);%drawthecurveofheightfigure(2);xlabel('時間/秒');ylabel('高度/米');gridon;holdon;plot((0:13150),Height);%drawthecurveofdistanceofhorizonfigure(3);xlabel('時間/秒');ylabel('航程/米');gridon;holdon;plot((0:13150),X)2.4AnalysisofResultTherearetwoloopsfornavigationcomputing.Theinnerloopisiteratedforattitudewhiletheoutisiteratedforpositionandvelocity.InSINS,thedifferentialequationiswhereqisattitudequaternionofmissile.ItsPeano-BakersolutionisUsingtheoutputofgyros,wecangaintheangularincrementmatrix.Andtheorderimplementationcanbeused.Thus,everyiteration,thewillbeupdated.Meanwhilethegeographicalframeisalsochangedbecauseofthemotionofvehicleandthespinningoftheearththoughtherateisreallylow.Toensuretheprecisionofnavigationweshouldupdatethegeographicalframeevery0.1second.Bynowthedigitalplatformisestablishedthroughcomputer.Thenweshouldacquirethevehicle’srelativeaccelerationonearththroughthetransformationofaccelerometers’output.Finally,thefirstintegrationgetthevelocityandintegrationagaintogettheposition,whichmeanst