哈工大导航原理、惯性技术大作业

1、Assignment of Principles of Navigation (INS)My Chinese Name白赫My Class No.1204201My Student No.1120110412Dec 28th, 2015Contents1. The First Task - stationary test31.1 Task description31.2 The Process of Solution31.3 Programming Codes in MATLAB41.4 Analysis of Result42. The Second Task - flight t

2、est52.1 Task description52.2 The Process of Solution62.3 Programming Codes in MATLAB92.4 Analysis of Result123. Reflection on the simulation121. The First Task - stationary test1.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123o EL, stationary on a launch

3、 pad. Three gyros, GX, GY, GZ, and three accelerometers, AX, AY, AZ, are installed along the axes Xb, Yb, Zb of its body frame respectively.12The body frame of the missile initially coincides with the geographical frame, as shown in the figure, with its pitching axis Xb pointing to the east, rolling

4、 axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22° around Xb(2) 78° around Yb(3) -16° around ZbUpZbXbEastNorthYbAfter that, the body of the missile stops rotating. You are required to compute the final outputs of the t

5、hree accelerometers on the missile, using quaternion and ignoring the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.1.2 The Process of SolutionFirst rotation: around axis X for , the quaternion is: Second rotation: around axis Y for , the quaternion is: Third r

6、otation: around axis Z for , the quaternion is: The combination quaternion and is:Thus the final coordinate is：The final output of the accelerometers on the missile are:1.3 Programming Codes in MATLAB%The 1/2 angle of three rotations Angle_X=(-22/2)/180*pi;Angle_Y=(78/2)/180*pi;Angle_Z=(-16/2)/180*p

7、i;q1=cos(Angle_X),sin(Angle_X),0,0; %The quaternion of first rotationq2=cos(Angle_Y),0,sin(Angle_Y),0; %The quaternion of second rotationq3=cos(Angle_Z),0,0,sin(Angle_Z); %The quaternion of third rotationA0=0,0,0,9.8; %The quaternion of Acc in the old frame%The multiplication of quaternion(q=q1q2q3)

8、p=quatmultiply(q1,q2);q=quatmultiply(p,q3);%The inverse of quaternion qqinv=quatinv(q);%The output of Acc in the fighterA1=quatmultiply(qinv,A0);A2=quatmultiply(A1,q)1.4 Analysis of ResultWe have learned in section 1.1 of Unit 10, successive rotations can be combined into a single rotation. The body

9、 of this missile is made to rotate in 3 steps. Then the quaternion q for the combined rotation can be obtained using According to this method, we can get the combination quaternion q. Now, we have a rotating frame, and a fixed vector, namely accelerometers vector in this circumstance. The coordinate

10、s of the vector can be transformed from the old frame to new frame using images, that is,Finally, we get the final outputs of the three accelerometers on the missile.2. The Second Task - flight test2.1 Task descriptionInitially, the missile is stationary on the launch pad, 400m above the sea level.

11、Its rolling axis is vertical up, and its pitching axis is to the east. Then the missile is fired up.The outputs of the gyros and accelerometers are both pulse numbers. Each gyro pulse is an angular increment of 0.01 arc-sec, and each accelerometer pulse is 1e-7g, with g = 9.8m/s2. The gyro output fr

12、equency is 200Hz, and the accelerometers is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MATLAB data file named imu.mat, containing matrices gm of 263000×3 and am of 13150×3 respectively. The format of the data is as shown in the tables, with 10 rows of ea

13、ch matrix selected. Each row represents the outputs of the type of sensors at each sampling time.AXAYAZ-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-255229251550357-15363805-303759452365312-15643989-352289653180268-15924172-400819853995223-16204356-449350154810179-16484

14、539-497880355625135-16764723-497880355625135-16764723GXGYGZ-1872-3611-1945-720-1-2014-10800-2087-14390-2160-17990-2234-21601-2303-25201-2375-28781-2450-32390-2522-35992The Earth can be seen as an ideal sphere, with radius 6371.00km and spinning rate 7.292×10-5 rad/s, The errors of the sensors a

15、re ignored, so is the effect of height on the magnitude of gravity. The outputs of the gyros are to be integrated every 0.005s. The rotation of the geographical frame is to be updated every 0.1s, so are the velocities and positions of the missile.You are required to:Compute the final attitude quater

16、nion, longitude, latitude, height, and east, north, vertical velocities of the missile.Compute the total horizontal distance traveled by the missile.Draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.Draw the curve of the height of the missile, with horizont

17、al time axis.2.2 The Process of SolutionThe simplified navigation algorithm for SINS is as shown in the Figure 1.Figure 1:Simplified navigation algorithm for SINSResults:(1)The final attitude quaternion The final longitude, latitude and height of the missile are as follow:Longitude=,Latitude=Height=

18、mThe east, north and vertical of the fighter at last are: (2)The total horizontal distance traveled by the missile is :.And the curve of the horizontal distance of the missile, with horizontal time axis is as shown in the Figure 2.Figure 2: The curve of the horizontal distance of the missile(3)The l

19、atitude-versus-longitude trajectory of the missile is as shown in the Figure 3.Figure 3: The latitude-versus-longitude trajectory of the missile(4)The curve of the height of the missile, with horizontal time axis is as shown in the Figure 4. Figure 4: The curve of the height of the missile2.3 Progra

20、mming Codes in MATLAB%initialize the parameter of systemk=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;%initial quaternion %define and initialize the matrix of Longitude,Latitude

21、 and HeightLongitude=zeros(1,13151);Latitude=zeros(1,13151);Height=zeros(1,13151);Longitude(1)=123;Latitude(1)=46;Height(1)=400;%define and initialize the matrix of velocity with length of 13150Ve=zeros(1,13151); %east velocityVn=zeros(1,13151); %north velocityVu=zeros(1,13151); %upward velocityX=ze

22、ros(1,13151); %distance of horizon%define and initialize the matrix of specific force with length of 13150fe=zeros(1,13151);fn=zeros(1,13151);fu=zeros(1,13151);FE=zeros(1,13151);FN=zeros(1,13151);FU=zeros(1,13151);%load the data provided by teacherload imu.mat;for N=1:K %starting iteration for posit

23、ion and xelocity q=zeros(21,4); q(1,:)=Q(N,:); for n=1:k %starting iteration for attitude w=Gyro_pulse*gm(N-1)*20+n,1:3); %angular increment output of gyro w_mod=quatmod(w,0); W= 0,-w(1),-w(2),-w(3); w(1),0,w(3),-w(2); %angular increment matrix w(2),-w(3),0,w(1); w(3),w(2),-w(1),0 ; I=eye(4); % 4 or

24、der implementation % Taylor series approximately S=1/2-w_mod2/48; C=1-w_mod2/8+w_mod4/384; q(n+1,:)=(C*I+S*W)*q(n,:)')' end %end of the inner loop, namely attitude loop Q(N+1,:)=q(n+1,:); % angular rates of geographical frame WE=-Vn(N)/R; WN=Ve(N)/R+W_earth*cos(Latitude(N)/180*pi); WU=Ve(N)/

25、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,:);%updating attitude of missle using rotation of geographical frame %specific force measured by A

26、cc. 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); %computing the relative accelaration of vehicle 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*s

27、in(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; %integrated for velocity Ve(N+1)=FE(N)*T+Ve(N); Vn(N+1)=FN(N)*T+Vn(N); Vu(N+1)=FU(N)*T+Vu(N); %updating

28、 the position Latitude(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); %computing the distance of horizon Xe=(Ve(N)+Ve(N+1)/2*T; Xn=(Vn(N)+Vn(N+1)/2*T; X(N+1)=X(N)+sqrt(Xe2+Xn2)

29、;end%draw the curve of Longitude and Latitudefigure(1);xlabel('经度/度');ylabel('纬度/度');grid on;hold on;plot(Longitude,Latitude);%draw the curve of heightfigure(2);xlabel('时间/秒');ylabel('高度/米');grid on;hold on;plot(0:13150),Height);%draw the curve of distance of horizonf

30、igure(3);xlabel('时间/秒');ylabel('航程/米');grid on;hold on;plot(0:13150),X)2.4 Analysis of ResultThere are two loops for navigation computing. The inner loop is iterated for attitude while the out is iterated for position and velocity. In SINS, the differential equation iswhere q is atti

31、tude quaternion of missile. Its Peano-Baker solution isUsing the output of gyros, we can gain the angular increment matrix.And the order implementation can be used. Thus, every iteration, the will be updated. Meanwhile the geographical frame is also changed because of the motion of vehicle and the s

32、pinning of the earth though the rate is really low. To ensure the precision of navigation we should update the geographical frame every 0.1 second. By now the digital platform is established through computer. Then we should acquire the vehicles relative acceleration on earth through the transformation of accelerometers output. Finally, the first integration get the velocity and integration again to get the position, which means that this simulation of miss