2015惯性导航大作业.docx

MyChineseName MyClassNo. 1204103MyStudentNo. AssignmentofPrinciplesofNavigation导航原理作业（惯性导航部分2015秋）Thereportistocontain:1.Descriptionofthetaskscontentsofthenextpage.2.Thecodeofyourprograms,andtheirexplanation.3.Theresultsofyourcomputationorsimulation.4.Youranalysisoftheresult,andyourreflectionontheprogrammingorsimulation5.Originalitystatementsorreference/assistanceacknowledgements.Englishisexpectedinwriting,thoughChineseisalsoaccepted.AXAYAZ-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-303759452365312-15643989-352289653180268-15924172-400819853995223-16204356-449350154810179-16484539-497880355625135-16764723-497880355625135-16764723GXGYGZ-1872-3611-1945-720-1-2014-10800-2087-14390-2160-17990-2234-21601-2303-25201-2375-28781-2450-32390-2522-35992TaskdescriptionAmissileequippedwithSINSisinitiallyatthepositionof46oNLand123oEL,stationaryonalaunchpad.Threegyros,GX,GY,GZ,andthreeaccelerometers,AX,AY,AZ,areinstalledalongtheaxesXb,Yb,Zbofitsbodyframerespectively.Case1:stationarytestThebodyframeofthemissileinitiallycoincideswiththegeographicalframe,asshowninthefigure,withitspitch-UpZbingaxisXbpointingtotheeast,rollingaxisYbtothenorth,andazimuthaxisZbupward.Thenthebodyofthemissileismadetorotatein3steps:(1)-22oaroundXbNorthYb(2)78oaroundYbXbEast(3)16aroundZboAfterthat,thebodyofthemissilestopsrotating.Youarerequiredtocomputethefinaloutputsofthethreeaccelerometersonthemissile,usingquaternionandignoringthedeviceerrors.Itisknownthatthemagni-tudeofgravityaccelerationisg=9.8m/s2.Case2:flighttestInitially,themissileisstationaryonthelaunchpad,400mabovethesealevel.Itsrollingaxisisverticalup,anditspitchingaxisistotheeast.Thenthemissileisfiredup.Theoutputsofthegyrosandaccelerometersarebothpulsenumbers.Eachgyropulseisanangularincrementof0.01arc-sec,andeachaccelerometerpulseis1e-7g,withg=9.8m/s2.Thegyrooutputfrequencyis200Hz,andtheac-celerometersis10Hz.Theoutputsofthegyrosandaccel-erometerswithin1315sarestoredinaMATLABdatafilenamedimu.mat,containingmatricesgmof2630003andamof131503respectively.Theformatofthedataisasshowninthetables,with10rowsofeachmatrixselected.Eachrowrepresentstheoutputsofthetypeofsensorsateachsamplingtime.TheEarthcanbeseenasanidealsphere,withradius6371.00kmandspinningrate7.29210-5rad/s,Theerrorsofthesensorsareignored,soistheeffectofheightonthemagnitudeofgravity.Theoutputsofthegyrosaretobeintegratedevery0.005s.Thero-tationofthegeographicalframeistobeupdatedevery0.1s,soaretheve-locitiesandpositionsofthemissile.Youarerequiredto:(1)computethefinalattitudequater-nion,longitude,latitude,height,andeast,north,verticalvelocitiesofthemissile.(2)computethetotalhorizontaldistancetraveledbythemissile.(3)drawthelatitude-versus-longitudetrajectoryofthemissile,withhorizontallongitudeaxis.(4)drawthecurveoftheheightofthemissile,withhorizontaltimeaxis.The answer of case1：usingmethod of quaternion ：step1：-22oaroundXbq=cos2+sin2n=cos2+sin2iIn this equation =-22change into radian q=cos-22360,sin-22360,0,0step2：78oaroundYbq=cos2+sin2n=cos2+sin2jIn this equation =78 q=cos78360,0,sin78360,0step3：16aroundZbq=cos2+sin2n=cos2+sin2kIn this equation =-16 q=cos-16360,0,0,sin-16360combine q q=qqq=+p1i + p2j +p3k q-1=qqq=-p1i- p2j-p3kuse matlab to calculate input q1 q2 q3:CODE:q1=cos(-22/360*pi) sin(-22/360*pi) 0 0;q2=cos(78/360*pi) 0 sin(78/360*pi) 0;q3=cos(-50/360*pi) 0 0 sin(-50/360*pi);a function named quatmultiply() which can calculate the quaternion automatically from /CODE:q12=quatmultiply(q1,q2);q=quatmultiply(q12,q3);then do the quaternion operation R=q-1Rqa function named quatinv() which can calculate the inverse quaternion from /CODE:R=0 0 0 9.8;Q=q(1) q(2) q(3) q(4);Iq=quatinv(Q);M=quatmultiply(Iq,R);A=quatmultiply(M,Q);print the resultCODE:A=Anorm(A)the result is: A = 0.0000 -7.5316 -5.9788 1.8892ans = 9.8000The answer of case2：According to this graph,firstly,analyse and separate each function of every step:initialization function -SET INITIAL VALUE:NavTime=1315; %navigation timecurrent=0;R=6371000; %the radius of earthg0=9.8; %gravity accelerationwie=7.292e-5; %spinning ratedT=0.1; %set update timedt=0.005; K= NavTime /dT; %loop timek=dT/dt; %set NL ELLatitude0=46*pi/180; %46 NLLongitude0=123*pi/180; %123 ELHeight0=400; %initial Height is 400m VE0=0; %east velocity=0VN0=0; %north velocity=0VK0=0; %vertical velocity=0e0=90*pi/180; %90 north by eastQ0=cos(e0/2),cos(e0/2),0,0; %define matrix to store latitude longitude and heightQ=zeros(K,4); %Quaternion matrixLatitude=zeros(K,1); %Latitude matrixLongitude=zeros(K,1); %Longitude matrixHeight=zeros(K,1); %Height matrixVE=zeros(K,1);VN=zeros(K,1);VK=zeros(K,1);Vt=zeros(K,1);azimuth=zeros(K,1); pitch=zeros(K,1); roll=zeros(K,1); %azimuth pitch rollpg=0; %pointer of gyro%initial matrix to store latitude longitude and heightQ(1,:)=Q0; %Initialization of quaternion Latitude(1)=Latitude0; %Initialization of LatitudeLongitude(1)=Longitude0; %Initialization of LongitudeHeight(1)=Height0; %Initialization of Height lmd=Q(1); p1=Q(2); p2=Q(3); p3=Q(4); Ml_qi= lmd p1 p2 p3 % left matrix of Qs inverse -p1 lmd p3 -p2 -p2 -p3 lmd p1 -p3 p2 -p1 lmd; Mr_q= lmd -p1 -p2 -p3 % right matrix of Q p1 lmd p3 -p2 p2 -p3 lmd p1 p3 p2 -p1 lmd; CA=Ml_qi*Mr_q; C=CA(2:4,2:4); % DCM C corresponding to Q % pitching angle -90 90, improvement is required to avoid singularity pitch= asin(C(2,3)*(180/pi); % azimuth angle -180 180, improvement is required to avoid singularity azimuth=atan(-C(2,1)/C(2,2)*(180/pi); if -C(2,1)0 & C(2,2)0, azimuth=azimuth+180; end if -C(2,1)0 & C(2,2)0 & C(3,3)0, roll=roll+180; end if -C(1,3)0 & C(3,3)for N=1:K %set a loop to update the frame if mod(N,10)=0 current=current+1; else current=current;end if N=1 Q(1,:)=Q0; Latitude(1)=Latitude0; Distanceitude(1)=Longitude0; Height(1)=Height0; VE(1)=VE0; VN(1)=VN0; VK(1)=VK0; else Q(N,:)=Q(N-1,:); Latitude(N)=Latitude(N-1); Longitude(N)=Longitude(N-1); Height(N)=Height(N-1); VE(N)=VE(N-1); VN(N)=VN(N-1); VK(N)=VK(N-1);endfor n=1:k pg=pg+1; gmpg=gm(pg,:); gmpg=gmpg*0.01/3600*pi/180; %current output of gyro dgx=gmpg(1); %three axis dgy=gmpg(2); dgz=gmpg(3); dE=. %angular increment 0 -dgx -dgy -dgz dgx 0 dgz -dgy dgy -dgz 0 dgx dgz dgy -dgx 0;dE2=dgx2+dgy2+dgz2; %angular rotation norm kc=1,1-dE2/8,1-dE2/8,1-dE2/8+(dE22)/384; %4th order implementationks=0.5,0.5,0.5-dE2/48,0.5-dE2/48;order=4;dQ=kc(order)*eye(4)+ks(order)*dE;Q(N,:)=(dQ*Q(N,:);endwE=-VN(N)/R; %angular rates of geographical framewN=VE(N)/R+wie*cos(Latitude(N);wK=VE(N)/R*tan(Latitude(N)+wie*sin(Latitude(N);dr=wE wN wK*dT; %updating attitude of vehicle using rotation .dr0=norm(dr); %of geographical framedn=dr/dr0;dQ=cos(dr0/2) sin(dr0/2)*dn(1),sin(dr0/2)*dn(2),sin(dr0/2)*dn(3); dQ=quatinv(dQ); %InverseQ(N,:)=quatmultiply(dQ,Q(N,:); %Get Current GF to BF quaternion j0=0 0 1 0; j0=quatmultiply(Q(N,:),j0);j0=quatmultiply(j0,quatinv(Q(N,:);Get speed- fa=am(N,:)*(1e-7)*g0; fb=0 fa(1),fa(2),fa(3); Qinv=quatinv(Q(N,:); tmp=quatmultiply(Q(N,:),fb); fg=quatmultiply(tmp,Qinv); fE=fg(2); fN=fg(3);fK=fg(4);Get relative accelerate- dVE=fE+VE(N)*VN(N)/R*tan(Latitude(N)-(VE(N)/R+2*wie*cos(Latitude(N)*VK(N)+. 2*VN(N)*wie*sin(Latitude(N); dVN=fN-2*VE(N)*wie*sin(Latitude(N)-VE(N)2/R*tan(Latitude(N)-VN(N)*VK(N)/R; dVK=fK+2*VE(N)*wie*cos(Latitude(N)+(VE(N)2+VN(N)2)/R-g0; v=dVE dVN dVK*dT+VE(N) VN(N) VK(N); %integration for relative velocities VE(N)=v(1); VN(N)=v(2); VK(N)=v(3); %east,north,vertical velocities V=VE(N),VN(N),VK(N); Vt(N)=norm(V); %sqrt(VE(N)2+VN(N)2+VK(N)2) Distance=Distance+sqrt(V(1)2+V(2)2)*dT; %North and east velocity if(N=1) avgVE=VE(1); avgVN=VN(1); avgVK=VK(1); else avgVE=(VE(N)+VE(N-1)*0.5; %avgV is mean velocity between time N and N-1 avgVN=(VN(N)+VN(N-1)*0.5; avgVK=(VK(N)+VK(N-1)*0.5;endGet position - Longitude(N)=dT*avgVE/(R*cos(Latitude(N)+Longitude(N); Latitude(N)=dT*avgVN/R+Latitude(N); Height(N)=dT*avgVK+Height(N); azimuth(N),pitch(N),roll(N)=q2eul(Q(N,:);endDisplay the graphs on Screen-disp(Computing complete!);close all;disp(Final Longitude,Latitude,Height:);Final=Longitude(N)*180/pi,Latitude(N)*180/pi,Height(N) %Final Longitude,Latitude,Heightdisp(Final Quaternion:);QN=Q(N,:)disp(Final Velocity:VE,VN,VK:);FinalVelocity=VE(N),VN(N),VK(N)figure(1);plot(Longitude*180/pi,Latitude*180/pi);title(Longitude-Latitude);xlabel(Longitude);ylabel(Latitude);grid on; figure(2);plot(Height);title(Height-time);xlabel(time);ylabel(Height);grid on; figure(3);plot(azimuth);title(azimuth-time);xlabel(time);ylabel(azimuth);grid on;figure(4);plot(pitch);title(pitch-time);xlabel(time);ylabel(pitch);grid on;figure(5);plot(roll);title(roll-time);xlabel(time);ylabel(roll);grid on;figure(6);pl