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

《导航原理》作业(惯性导航部分)一、题目要求 A fighter equipped with SINS is initially at the position of ︒35 NL ︒122X G Y G Z G ,and three accelerometers, X A ,Y A ,Z A are installed along the axes b X ,b Y ,b Z of the body frame respectively.Case 1:stationary onboard testThe body frame of the fighter initially coincides with the geographical frame, as shown in the figure, with its pitching axis b X pointing to the east,rolling axis b Y to the north, and azimuth axis b Z upward. Then the body of the fighter is made to rotate step by step relative to the geographical frame.(1) ︒10around b X(2) ︒30around b Y(3) ︒50-around b ZAfter that, the body of the fighter stops rotating. You are required to compute the final output of the three accelerometers on the fighter, using both DCM and quaternion respectively,and ignoring the device errors. It is known that the magnitude of gravity acceleration is 2/8.9g s m =. Case 2:flight navigationInitially, the fighter is stationary on the motionless carrier with its board 25m above the sea level. Its pitching and rolling axes are both in the local horizon, and its rolling axis is ︒45on the north by east, parallel with the runway onboard. Then the fighter accelerate along the runway and take off from the carrier.The output of the gyros and accelerometers are both pulse numbers,Each gyro pulse is an angular increment of sec arc 1.0-,and each accelerometer pulse is g 6e 1-,with 2/8.9g s m =.The gyro output frequency is 10 Hz,andthe accelerometer’s is 1Hz. The output of gyros and accelerometers within 5400s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of35400⨯ and am of 35400⨯ respectively. The format of data as shown in the tables, with 10 rows of each matrix selected. Each row represents the out of the type of sensors at each sample time.The Earth can beseen as an idealsphere, with radius6368.00km andspinning rate rad/s 10292.75-⨯, Theerrors of sensors areignored, so is theeffect of height onthe magnitude ofgravity. The outputof the gyros are to integrated every 0.1s. The rotation of geographical frame is to be updated every 1s, so are the velocities and position of the figure. You are required to:(1)Compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the fighter.(2)Compute the total horizontal distance traveled by the fighter.(3)Draw the latitude-versus-longitude trajectory of the fighter, with horizontal longitude axis.(4)Draw the curve of the height of fighter, with horizontal time axis.二、Case1解答2.1 方向余弦阵法（1） 绕Xb 轴转过ψ︒=10ϕ ⎪⎪⎪⎭⎫ ⎝⎛︒︒-︒︒=⎪⎪⎪⎭⎫ ⎝⎛-=10cos 10sin 010sin 10cos 0001cos sin 0sin cos 0001ϕϕϕϕϕC（2） 绕Yb 轴转过︒=30θ ⎪⎪⎪⎭⎫ ⎝⎛︒︒︒-︒=⎪⎪⎪⎭⎫ ⎝⎛-=30cos 030sin 01030sin 030cos cos 0sin 010sin 0cos θθθθθC（3） 绕Zb 轴转过︒-=50ψ所以变换后的坐标由于初始时刻有 所以计算得( Z Y X )=(3581.86027.2-4054.4-)三个加速度计的输出分别是计算程序见附录一2.2 四元数法（1）绕Xb（2）绕Yb（3）绕Zb 则合成四元数合成四元数的逆由公式计算得 (Z Y X )=(3581.86027.2-4054.4-)三个加速度计的输出分别是由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。

2024.1黑龙江教育·理论与实践一、引言科技发展急需人才，人才培养取决于教育。

当前，随着我国社会改革的不断深化，社会思潮多样并存，各种思想交相融合，多元文化冲突更加频繁。

而学生正处于知识体系、思维方式和价值观念的形成时期，极易受到各种现象、观点、言论的影响。

极端个人主义、拜金主义、享乐主义等不良思潮给学生带来了消极影响，导致部分学生理想信念迷失、道德行为欠缺[1]。

如何培养富有社会责任感的创新人才，是近年来高等教育特别关注和不断探讨的课题。

专业课程教学融入思政元素，正是解决上述问题的一种尝试，也是加强和改进学生素质教育的一种探索[2]。

这种方式符合高等教育与时俱进的发展需要，不仅能够克服人才培养中的种种弊端，也极大地推动了高等教育的改革和创新，具有十分重要的意义[3]。

然而，课程思政在我国高校实施的时间并不长，还有待进一步健全和完善，尚需在理论上深入研究，在实践中总结经验。

文章结合“导航原理”实验教学的特点，将课程思政融入实验教学中，探索与实践该种教学方式对当代学生素质教育的提升效果。

二、“导航原理”实验教学特点“导航原理”作为高等工科院校控制科学与工程学科或航空宇航科学与技术学科的一门专业课程，是学习后续专业课程，如“飞行器控制与制导”“航天器控制”“无人机控制”“最优导航与滤波”等的基础。

“导航原理”实验教学不仅要帮助学生建立起惯性空间的概念，使学生加深对惯性器件结构特点、工作原理和基本特性的了解，实现对理论知识的验证，更重要的是通过实验使学生领悟惯性导航原理的应用规律，提高学生的动手能力、工程实践能力、设计能力和创新能力。

惯性导航系统是导弹和火箭的“眼睛”和“大脑”，提高惯性导航系统的精度是精确打击的关键。

惯性导航器件和系统的设计与制造需要精益求精的工匠精神和创新精神。

“导航原理”实验教学以培育学生科研实践能力和创新精神为目标，融合了国家战略、人才培养内涵式建设、学生个性化发展等多方面的内容[4]，具有深厚的课程思政资源和基础。

Assignment of Inertial Technology 《惯性技术》作业Autumn 2015Assignment 1: 2-DOF response simulation A 2-DOF gyro is subjected to a sinusoidal torque with amplitude of 4 g.cmand frequency of 10 Hz along its outer ring axis. The angular moment of its rotor is 10000 g.cm/s , and the angular inertias in its equatorial plane are both 4 g.cm/s 2. Please simulate the response of the gyro within 1 second, and present whatever you can discover or confirm from the result.In this simulation, we are going to discuss the response of a 2-DOF gyro to sinusoidal torque input. According to the transfer function of the 2-DOF gyro, the outputs can be expressed as: 12222()()()()yx y x y x y J H s M s M s J J s H s J J s H α=-++ 12222()()()()x y x x y x y J H s M s M s J J s H s J J s H β=+++ The original system transfer function is a 2-input, 2-output coupling system. But the given input only exists one input, we can treat the system as 2 separate FIFO systemAs a consequence, we can establish the block diagram of the system in simulink in Fig 1.1. Substitute the parameters into the system and input, then we have the input signals as follow: 0,4sin(20)y ox M M t π==Then the inverse Laplace transform of the output equals the response of the gyro in timedomain as follows:02222000200222200()sin sin ()()()cos cos ()()ox a ox a x a x a ox ox a ox a a a a a M M t t t J J M M M t t t H H H ωαωωωωωωωωωβωωωωωωωω=-+--=+---Fig 1.1 The block diagram of the system in simulinkAnd the simulation results in time domain within 1 second are shown in the follwing pictures. Fig1.2 is the the output of outer ring ()tα. Fig1.3 is the output of inner ring ()tβ. Fig1.4 is the trajectory of 2-DOF gyro’s response to sinusoidal input. As we can see from the Fig1.3,there are obvious sawtooth wave in the output of the inner ring. It’s a unexpected phenomenon in my original theoretical analysis.Fig1.2 The output of inner ring ()tβFig 1.3 The output of outer ring ()tαI believe the sawtooth wave is caused by the nutation. For the frequency of the nutation isobtained as100002500/3974eHrad s HzJω===≈, which is far higher than the frequencyof the applied sinusoidal torque , namelyaωω.Fig 1.4 Trajectory of 2-DOF gyro’s response to sinusoidal input The trajectory of 2-DOF gyro’s response to sinusoidal input are shown in Fig1.4. As we can see, it’s coupling of X and Y channel scope output. The overall shape is an ellipse, which is not perfect for there are so many sawteeth on the top of it.Note that the major axis of ellipse is in the direction of the forced procession, amplitude of which isoxMHω, whereas the minor axis is in the direction of the torsion spring effects, with amplitude oxaMHω.The nutation components are much smaller than that of the forced vibration, which can be eliminated to get the clear static response.2200222()sin sin()cos(1cos)()ox oxa ax aox ox oxa aa a a aM Mt t tJ HM M Mt t tH H Hαωωωωωωβωωωωωωω≈≈-≈-=--To prove it, we eliminate the effects of the nutation namely the quadratic term in the denominator and get Fig 1.5, which is a perfect ellipse. We can conclude that when input to the 2-DOF gyro is sinusoidal torque, the gyro will do an ellipse conical pendulum as a static response, including procession and the torsion spring effects, together with a high-frequency vibration as the dynamic response.Fig 1.5 Trajectory of the gyro’s response without nutationAssignment 2: Single-axis INS simulation2.1 description of the problemA magnetic levitation train is being tested along a track running north-south. It first accelerates and then cruises at a constant speed. Onboard is a single-axis platform INS, working in the way described by the courseware of Unit 5: Basic problems of INS. The motion information and Earth parameters are shown in table 2-1, and the possible error sources are shown in Table2-2.Fig2.1 The sketch map of the single-axis INS problemYou are asked to simulate the operation of the INS within 10,000 seconds, and investigate, first one by one and then altogether, the impact of these error sources on the performance of the INS.The block diagram in the courseware might be of some help. However, there lurks an inconspicuous error, which you have to correct before you can obtain reasonable results.There are one core relevant formula, to get the specific form of its solution, we should substitute the unknown parameters.(1)()c N ay A K y ga=∆++-Firstly, the input signal is accelerometer of the platform, and the velocity of the platform is the integration of the acceleration./py y ydty Rω=+=-⎰The acceleration along Yp may contains two parts:cos gsin gypf y yααα=-≈-When accelerometer errors are concerned, the output of accelerometer will be:(1)N a yp Na K f A=++When gyro errors concerned:'(1)p g pKωωε=++Only αis unknown:[(1)]tg pttptK dtdtααωεϕϕω=+++-∆∆=⎰⎰And the reference block diagram and simulink block diagram are as follows in Fig2.2, Fig2.3. There is a small fault in the reference block, which is that the sign of the marked add operation should be positive instead of negative.The results of the simulation are shown in Fig 2.4 to Fig 2.13.Fig2.2 The reference block diagram in the courseware(unrectified)Fig2.3 The simulink block diagram for Assignment 22.2 results and analysis of the problemFig.2.4 real acceleration,velocity and displacement output without error sourcesKFig2.5 position bias when only accelerometer scale factor error exists0.0001aFig2.6 position bias output when only gyro scale factor error exists 0.0001g K =Fig2.7 position bias when only acceleromete bias error exists 20.0002/N A m s ∆=Fig2.8 position bias output when only initial velocity error exists 200.01/y m s ∆=Fig2.9 position bias output when only initial position error exists 010y m ∆=Fig2.10 position bias output bias when only gyro bias errorexists 0.000000024240.00681/3/h rad s ε=︒=Fig2.11 position bias output when only initial platform misalignment angle exists0.000012120''345radα==Fig2.12 output considering all the error sourcesFig2.13 position bias output considering all error sourcesAs we can see in the above simulation results, if there is no error we can navigate the train ’s motion correctly, which comes from north to the south as shown in Fig2.4, beginning with an constant acceleration within 60 seconds then cruises at a constant speed, approximately 65 m/s. However, the situation will change a lot when different errors put into the simulation. The initial position error 010y m ∆= effects least as Fig.2.8, for this error doesn ’t enter into the closed loop and it won ’t influence the iterative process. The position bias is constant and can be negligible.In the second case, when the accelerometer scale factor error exists, 0.0001a K =, as shown in Fig2.5, the result are stable and almost accurate, the position bias is a sinusoidal output.So it is with the accelerometer bias error situation, 20.0002/N A m s ∆=, in Fig2.7, the initialvelocity error, 200.01/y m s ∆= in Fig2.9, and the initial platform misalignment angle,05''α=, in Fig2.11. However, the influence degrees of the different factors are not in the samemagnitude. The accelerometer scale factor influences the least with magnitude of 5, then the initial velocity larger magnitude of 8, and the accelerometer bias magnitude of 25. The influence of the initial platform misalignment angle is much more significant with a magnitude of 150. All the navigation bias in the second kind case is sinusoidal, which means they ’re limited and negligible as time passes by.In the third case, such as the gyro scale factor error situation, 0.0001g K =, in Fig2.6, and the gyro bias error,0.01/h ε=︒, results in Fig2.10, effects the most significant, the trajectory of the navigation disvergence accumulated as time goes. The position bias is a combination of sinusoidal signal and ramp signal. They also show that the longitudinal and distance errors resulted from gyro drifts are not convergent in time. It means the errors in the gyroscope do most harm to our navigation. And due to the significant influence of the gyro bias errors and the gyroscope scale factor error, results considering all the error sources disverge, and the navigationposition of the motion will be away from the real motion after a enough long time, as shown in Fig2.10. The gyro bias error is the most significant effect factor of all errors. By the time of 10000s, it has reaches 1600m, and it’s nearly the quantity of the position bias considering all error sources. Through contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias.Impression of the Whole simulation experimentThrough contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias, and the the gyro bias or the drift error do most harm to our navigation. So it is a must for us to weaken or eliminate it anyway. In spite of all the disadvantages discussed above, the INS still shows us a relatively accurate results of single-axis navigation.Assignment 3: SINS simulation3.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123 o EL, stationary on a launch 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.Case 1: stationary testThe 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 axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22o around Xb(2) 78o around Yb(3) –16o around ZbFig 3.1 Introduction to assignment 3After that, the body of the missile stops rotating. You are required to compute the final outputs of the three 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.Case 2: flight tes tInitially, the missile is stationary on the launch pad, 400m above the sea level. 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.01arc-sec, and each accelerometer pulse is 1e-7g, with g =9.8m/s2. The gyro output frequency is 200Hz, and the accelerometer’s is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MA TLAB 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 each matrix selected. Each row represents the outputs of the type of sensors at each sampling time. The 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 are 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:(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the missile.(2) compute the total horizontal distance traveled by the missile.(3) draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.(4) draw the curve of the height of the missile, with horizontal time axis.Fig 3.2 simplified navigation algorithm for SINS 3.2Procedure code3.2.1 Sub function code：quaternion multiply code:function [q1]=quml(q1,q2);lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);q1=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2;endquaternion inversion code:function [qni] =qinv(q)q(1)=q(1);q(2)=-q(2);q(3)=-q(3);q(4)=-q(4);qni=q;end3.2.2case1 DCM algorithm：function ans11cz=[cos(-22/180*pi) sin(-22/180*pi) 0 ;-sin(-22/180*pi) cos(-22/180*pi) 0;0 0 1]; %The third rotation DCMcx=[1 0 0 ;0 cos(78/180*pi) sin(78/180*pi);0 -sin(78/180*pi) cos(78/180*pi)]; %The first rotation DCMcy=[cos(-16/180*pi) 0 -sin(-16/180*pi);0 1 0;sin(-16/180*pi) 0 cos(-16/180*pi)]; %The second rotation DCM A=cz*cy*cx*[0;0;-9.8]end3.2.3case1 quaternion algorithm：function ans12g=[0;0;0;-9.8];q1=[cos(-11/180*pi);sin(-11/180*pi);0;0]; %The first rotation quaternionq2=[cos(39/180*pi);0;sin(39/180*pi);0]; %The second rotation quaternionq3=[cos(-8/180*pi);0;0;-sin(-8/180*pi)]; %The third rotation quaternionr=quml(q1,q2); %call the quaternion multiplication subfunction q=quml(r,q3);P1=[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)];P2=[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)];P=P1*P2;gn=P*g;gn=gn(2:4)end3.2.4 case2 SINS quaternion algorithm code：function ans2clc;clear;%parameters initializing：T=0.005;K=13150;R=6371000; %radius of earthwE=7.292*10^(-5); %spinning rate of earthQ=zeros(4, 263001) ;%quaternion matrix initializinglongitude=zeros(1,13151);latitude=zeros(1,13151);H=zeros(1,13151); %altitude matrixQ(:,1)=[cos(45/180*pi);-sin(45/180*pi);0;0]; % initial quaternion longitude(1)=123; %initial longitudelatitude(1)=46; % initial latitudeH(1)=400; %initial altitudelength=0;g=9.8;vE = zeros(1,13151); %eastern velocityvN = zeros(1,13151); %northern velocityvH = zeros(1,13151); %upward velocityvE(1)=0;vN(1)=0;vH(1)=0;load imu.mat %data loading%main calculation sectionfor N=1:Kq1=zeros(4,11);q1(:,1)=Q(:,N);for n=1:20 % Attitude iteration wx=0.01/(3600*180)*pi*gm((N-1)*10+n,1); % Angle incrementwy=0.01/(3600*180)*pi*gm((N-1)*10+n,2);wz=0.01/(3600*180)*pi*gm((N-1)*10+n,3);w=[wx,wy,wz]';normw=norm(w); % Norm calculationW=[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-normw^2/48;C=1-normw^2/8;q1(:,n+1)=(C*I+S*W)*q1(:,n);Q(:,N+1)=q1(:,n+1);endWE=-vN(N)/R; % rotational angular velocity component of a geographic coordinate systemWN=vE(N)/R+wE*cos(latitude(N)/180*pi);WH=vE(N)/R*tan(latitude(N)/180*pi)+wE*sin(latitude(N)/180*pi);attitude=[WE,WN,WH]'*T; %correction of the quaternion by updating the rotation of geographic coordinatenormattitude=norm(attitude);e=attitude/normattitude;QG=[cos(normattitude/2);sin(normattitude/2)*e];Q(:,N+1)=quml(qinv(QG),Q(:,N+1));fx=1e-7*9.8*am(N,1); %specific force measured by accelerometerfy=1e-7*9.8*am(N,2);fz=1e-7*9.8*am(N,3);Fb=[fx fy fz]';F=quml(Q(:,N+1),quml([0;Fb],qinv(Q(:,N+1)))); %The specific force is decomposed into geographic coordinate system.FE(N)=F(2);FN(N)=F(3);FU(N)=F(4);%calculate the velocity of the vehicle:VED(N)=FE(N)+vE(N)*vN(N)/R*tan(latitude(N)/180*pi)-(vE(N)/R+2*wE*cos(latitude(N)/ 180*pi))*vH(N)+2*vN(N)*wE*sin(latitude(N)/180*pi);VND(N)=FN(N)-2*vE(N)*wE*sin(latitude(N)/180*pi)-vE(N)*vE(N)/R*tan(latitude(N)/180 *pi)-vN(N)*vH(N)/R;VUD(N)=FU(N)+2*vE(N)*wE*cos(latitude(N)/180*pi)+(vE(N)^2+vN(N)^2)/R-g;%integration and get the relative velocity of vehicle:vE(N+1)=VED(N)*T+vE(N);vN(N+1)=VND(N)*T+vN(N);vH(N+1)=VUD(N)*T+vH(N);% integration and get the position of vehicle:longitude(N+1)=vE(N)/(R*cos(latitude(N)/180*pi))*T/pi*180+longitude(N);latitude(N+1)=vN(N)/R*T/pi*180+latitude(N);H(N+1)=vH(N)*T+H(N);length=sqrt((vE(N))^2+(vN(N))^2)+length;endfigure(1) %picture the longitude-latitude curve of the motion within 1315 secondstitle(' longitude-latitude ');hold ongrid onplot(longitude,latitude);figure(2) % picture the altitude curve of the motion within 1315 secondstitle('altitude');hold ongrid onplot(0:13150,H);3.3Simulation outputs and results analysisIn case 1, the DCM algorithm and quaternion results are the same as follow:A =7.53165.9788-1.8892And the results suggest the solution of DCM and quaternion is equivalence in this problem.In case 2, the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis) and the altitude curve of the missile are shown as follows in Fig 3.3 and Fig 3.4.Fig3.3 the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis)Fig3.4the altitude curve of the missileImpression of the Assignment 3In this assignment, we simulate the missile navigation situation in a real problem, as the problem we have done before. I did this based on my own original code, but to my surprise, it’s harder than I have expected. For the detailed thoughts for my program I have forgotten. I have to recheck the code line by line, But I am still troubled in correcting the initial parameters for many times. It has taught me a lesson, which is never to be egotistical for your ability to memory and the work you have done or understood.。

4.1说明惯性导航的基本原理及特点惯性导航是一种利用惯性原理的自主式的导航方式。

它完全依靠机载惯性设备自主地完成导航任务，是航天、航空和航海领域中的一种广泛使用的主要导航方法。

惯性导航具有以下优点（1）由于它是不依赖于任何外部信息．也不向外部辐射能量的自主式系统．故隐蔽性好且不受外界电磁干扰的影响；（2）可全天候、全球、全时间地工作于空中、地球表面乃至水下．（3）能提供位置、速度、航向和姿态角数据，所产生的导航信息连续性好而且噪声低．（4）数据更新率高、短期精度和稳定性好。

4.2导航平台的主要性能指标有哪些？说明其意义导航平台的主要性能指标主要包括稳定性及精度问题等，要提高其稳定性可以设置阻尼器等，要提高精度则可在回路中加上校正装置等。

4.3说明指北方位、自由方位、游移方位三种导航系统的异同。

三种导航系统异同：4.4说明纯惯导高度通道为什么是不稳定的？在精确导航中，g 不是常值而是高度H 的函数，当R H >>时)21(0RH g g -=。

由此，纯惯导高度通道的特征方程式为：)2()2(22=-+=-Rg s Rg s Rg s因此该系统虽然是一个闭环系统，但特征式有一个正根，说明系统是不稳定的，计算高度H 误差是扩散的。

4.5说明高度通道阻尼回路的作用。

由于纯惯导高度通道是不稳定的，因此不能直接采用纯惯性的高度通道，而必须引入外部高度信息（气压高度表、无线电高度表、大气数据系统等）对高度通道构成阻尼回路，这种阻尼回路有时也叫互补滤波器，使两种高度住处起互补作用，这样两方面可以取长补短，得到动态品质好而误差不随时间发散的组合高度系统。

4.6结合第一章的知识，进一步说明舒勒摆的概念。

如何使导航平台具有舒勒特性？舒勒摆指单摆长度L 正好等于地球半径R 时，其自然振荡周期等于84.4min 能指示垂线的这样一个装置，此装置不会受支点加速度的干扰，它的重要意义还在于，任何装置只要能使它的振动周期满足等于84.4min ，它就具有摆长等于地球半径R 单摆的抗干扰能力。

卫星定位导航原理实验专业：班级：学号：姓名：日期：实验一实时卫星位置解算及结果分析一、实验原理实时卫星位置解算在整个GPS接收机导航解算过程中占有重要的位置。

卫星位置的解算是接收机导航解算（即解出本地接收机的纬度、经度、高度的三维位置）的基础。

需要同时解算出至少四颗卫星的实时位置，才能最终确定接收机的三维位置。

对某一颗卫星进行实时位置的解算需要已知这颗卫星的星历和GPS时间。

而星历和GPS时间包含在速率为50比特/秒的导航电文中。

导航电文与测距码（C/A码）共同调制L1载频后，由卫星发出。

本地接收机相关接收到卫星发送的数据后，将导航电文解码得到导航数据。

后续导航解算单元根据导航数据中提供的相应参数进行卫星位置解算、各种实时误差的消除、本地接收机位置解算以及定位精度因子（DOP）的计算等工作。

关于各种实时误差的消除、本地接收机位置解算以及定位精度因子（DOP）的计算将在后续实验中陆续接触，这里不再赘述。

卫星的额定轨道周期是半个恒星日，或者说11小时58分钟2.05秒；各轨道接近于圆形，轨道半径（即从地球质心到卫星的额定距离）大约为26560km。

由此可得卫星的平均角速度ω和平均的切向速度v s为：ω=2π/(11*3600+58*60+2.05)≈0.0001458rad/s (1.1)v s=rs*ω≈26560km*0.0001458≈3874m/s (1.2) 因此，卫星是在高速运动中的，根据GPS时间的不同以及卫星星历的不同（每颗卫星的星历两小时更新一次）可以解算出卫星的实时位置。

本实验同时给出了根据当前星历推算出的卫星在11小时58分钟后的预测位置，以此来验证卫星的额定轨道周期。

本实验另一个重要的实验内容是对卫星进行相隔时间为1s的多点测量（本实验给出了三点），根据多个点的测量值，可以估计Doppler频移。

由于卫星与接收机有相对的径向运动，因此会产生Doppler效应，而出现频率偏移。

Assignment ofInertial Technology《惯性技术》作业My Chinese NameMy Student ID 15S004001Autumn 2015Assignment 1: 2-DOF response simulationA 2-DOF gyro is subjected to a sinusoidal torque with amplitude of 4 g.cm and frequency of 10 Hz along its outer ring axis. The angular moment of its rotor is 10000 g.cm/s , and the angular inertias in its equatorial plane are both 4 g.cm/s 2. Please simulate the response of the gyro within 1 second, and present whatever you can discover or confirm from the result.In this simulation, we are going to discuss the response of a 2-DOF gyro to sinusoidal torque input. According to the transfer function of the 2-DOF gyro, the outputs can be expressed as:12222()()()()yx y x y x y J Hs M s M s J J s H s J J s H α=-++12222()()()()x yx x y x y J Hs M s M s J J s H s J J s H β=+++ The original system transfer function is a 2-input, 2-output coupling system. But the given input only exists one input, we can treat the system as 2 separate FIFO systemAs a consequence, we can establish the block diagram of the system in simulink in Fig 1.1. Substitute the parameters into the system and input, then we have the input signals as follow: 0,4sin(20)y ox M M t π==Then the inverse Laplace transform of the output equals the response of the gyro in timedomain as follows:0222200020222200()sin sin ()()()cos cos ()()ox a oxa x a x a ox ox a ox a a a a a M M t t t J J M M M t t t H H H ωαωωωωωωωωωβωωωωωωωω=-+--=+---Fig 1.1 The block diagram of the system in simulinkAnd the simulation results in time domain within 1 second are shown in the follwing pictures. Fig1.2 is the the output of outer ring ()t α. Fig1.3 is the output of inner ring ()t β. Fig1.4 is the trajectory of 2-DOF gyro ’s response to sinusoidal input. As we can see from the Fig1.3,there are obvious sawtooth wave in the output of the inner ring. It ’s a unexpected phenomenon in my original theoretical analysis.Fig1.2 The output of inner ring ()t βFig 1.3 The output of outer ring ()t αI believe the sawtooth wave is caused by the nutation. For the frequency of the nutation is obtained as010*******/3974e H rad s Hz J ω===≈, which is far higher than the frequencyof the applied sinusoidal torque , namely0a ωω .Fig 1.4 Trajectory of 2-DOF gyro ’s response to sinusoidal inputThe trajectory of 2-DOF gyro ’s response to sinusoidal input are shown in Fig1.4. As we can see, it ’s coupling of X and Y channel scope output. The overall shape is an ellipse, which is not perfect for there are so many sawteeth on the top of it.Note that the major axis of ellipse is in the direction of the forced procession, amplitude of which is 0ox M H ω, whereas the minor axis is in the direction of the torsion spring effects,with amplitude ox aM H ω.The nutation components are much smaller than that of the forced vibration, which can be eliminated to get the clear static response.22002220()sin sin ()cos (1cos )()ox oxaa x a ox ox oxa a a a a aM M t t t J H M M M t t t H H H αωωωωωωβωωωωωωω≈≈-≈-=--To prove it, we eliminate the effects of the nutation namely the quadratic term in the denominator and get Fig 1.5, which is a perfect ellipse. We can conclude that when input to the 2-DOF gyro is sinusoidal torque, the gyro will do an ellipse conical pendulum as a static response, including procession and the torsion spring effects, together with a high-frequency vibration as the dynamic response.Fig 1.5 Trajectory of the gyro’s response without nutationAssignment 2: Single-axis INS simulation2.1 description of the problemA magnetic levitation train is being tested along a track running north-south. It first accelerates and then cruises at a constant speed. Onboard is a single-axis platform INS, working in the way described by the courseware of Unit 5: Basic problems of INS. The motion informationand Earth parameters are shown in table 2-1, and the possible error sources are shown in Table2-2.Fig2.1 The sketch map of the single-axis INS problemYou are asked to simulate the operation of the INS within 10,000 seconds, and investigate,first one by one and then altogether, the impact of these error sources on the performance of theINS.The block diagram in the courseware might be of some help. However, there lurks aninconspicuous error, which you have to correct before you can obtain reasonable results.There are one core relevant formula, to get the specific form of its solution, we should substitute the unknown parameters.(1)()c N a y A K yga =∆++-Firstly, the input signal is accelerometer of the platform, and the velocity of the platform is the integration of the acceleration.0/p y y ydt yR ω=+=-⎰The acceleration along Yp may contains two parts:cos gsin g yp f y yααα=-≈- When accelerometer errors are concerned, the output of accelerometer will be:(1)N a yp N a K f A =++When gyro errors concerned:'(1)p g p K ωωε=++Onlyαis unknown:000[(1)]tg p t tp t K dt dtααωεϕϕω=+++-∆∆=⎰⎰And the reference block diagram and simulink block diagram are as follows in Fig2.2, Fig2.3. There is a small fault in the reference block, which is that the sign of the marked add operation should be positive instead of negative.The results of the simulation are shown in Fig 2.4 to Fig 2.13.Fig2.2 The reference block diagram in the courseware(unrectified)Fig2.3 The simulink block diagram for Assignment 22.2 results and analysis of the problemFig.2.4 real acceleration,velocity and displacement output without error sourcesKFig2.5 position bias when only accelerometer scale factor error exists0.0001aFig2.6 position bias output when only gyro scale factor error exists 0.0001g K =Fig2.7 position bias when only acceleromete bias error exists 20.0002/N A m s ∆=Fig2.8 position bias output when only initial velocity error exists 200.01/ym s ∆=Fig2.9 position bias output when only initial position error exists 010y m ∆=Fig2.10 position bias output bias when only gyro bias error exists 0.000000024240.00681/3/h rad s ε=︒=Fig2.11 position bias output when only initial platform misalignment angle exists0.000012120''345radα==Fig2.12 output considering all the error sourcesFig2.13 position bias output considering all error sourcesAs we can see in the above simulation results, if there is no error we can navigate the train ’s motion correctly, which comes from north to the south as shown in Fig2.4, beginning with an constant acceleration within 60 seconds then cruises at a constant speed, approximately 65 m/s. However, the situation will change a lot when different errors put into the simulation. The initial position error 010y m ∆= effects least as Fig.2.8, for this error doesn ’t enter into the closed loop and it won ’t influence the iterative process. The position bias is constant and can be negligible.In the second case, when the accelerometer scale factor error exists, 0.0001a K =, as shown in Fig2.5, the result are stable and almost accurate, the position bias is a sinusoidal output. So it is with the accelerometer bias error situation, 20.0002/N A m s ∆=, in Fig2.7, the initialvelocity error, 200.01/ym s ∆= in Fig2.9, and the initial platform misalignment angle, 05''α=, in Fig2.11. However, the influence degrees of the different factors are not in the samemagnitude. The accelerometer scale factor influences the least with magnitude of 5, then the initial velocity larger magnitude of 8, and the accelerometer bias magnitude of 25. The influence of the initial platform misalignment angle is much more significant with a magnitude of 150. All the navigation bias in the second kind case is sinusoidal, which means they ’re limited and negligible as time passes by.In the third case, such as the gyro scale factor error situation, 0.0001g K =, in Fig2.6, and the gyro bias error,0.01/h ε=︒, results in Fig2.10, effects the most significant, the trajectory of the navigation disvergence accumulated as time goes. The position bias is a combination of sinusoidal signal and ramp signal. They also show that the longitudinal and distance errors resulted from gyro drifts are not convergent in time. It means the errors in the gyroscope do most harm to our navigation. And due to the significant influence of the gyro bias errors and the gyroscope scale factor error, results considering all the error sources disverge, and the navigationposition of the motion will be away from the real motion after a enough long time, as shown in Fig2.10. The gyro bias error is the most significant effect factor of all errors. By the time of 10000s, it has reaches 1600m, and it’s nearly the quantity of the position bias considering all error sources. Through contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias.Impression of the Whole simulation experimentThrough contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias, and the the gyro bias or the drift error do most harm to our navigation. So it is a must for us to weaken or eliminate it anyway. In spite of all the disadvantages discussed above, the INS still shows us a relatively accurate results of single-axis navigation.Assignment 3: SINS simulation3.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123 o EL, stationary on a launch 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.Case 1: stationary testThe 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 axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22o around Xb(2) 78o around Yb(3) –16o around ZbFig 3.1 Introduction to assignment 3After that, the body of the missile stops rotating. You are required to compute the final outputs of the three 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.Case 2: flight tes tInitially, the missile is stationary on the launch pad, 400m above the sea level. 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.01arc-sec, and each accelerometer pulse is 1e-7g, with g =9.8m/s2. The gyro output frequency is 200Hz, and the accelerometer’s is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MA TLAB 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 each matrix selected. Each row represents the outputs of the type of sensors at each sampling time. The 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 are 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:(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the missile.(2) compute the total horizontal distance traveled by the missile.(3) draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.(4) draw the curve of the height of the missile, with horizontal time axis.Fig 3.2 simplified navigation algorithm for SINS 3.2Procedure code3.2.1 Sub function code：quaternion multiply code:function [q1]=quml(q1,q2);lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);q1=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2;endquaternion inversion code:function [qni] =qinv(q)q(1)=q(1);q(2)=-q(2);q(3)=-q(3);q(4)=-q(4);qni=q;end3.2.2case1 DCM algorithm：function ans11cz=[cos(-22/180*pi) sin(-22/180*pi) 0 ;-sin(-22/180*pi) cos(-22/180*pi) 0;0 0 1]; %The third rotation DCMcx=[1 0 0 ;0 cos(78/180*pi) sin(78/180*pi);0 -sin(78/180*pi) cos(78/180*pi)]; %The first rotation DCMcy=[cos(-16/180*pi) 0 -sin(-16/180*pi);0 1 0;sin(-16/180*pi) 0 cos(-16/180*pi)]; %The second rotation DCM A=cz*cy*cx*[0;0;-9.8]end3.2.3case1 quaternion algorithm：function ans12g=[0;0;0;-9.8];q1=[cos(-11/180*pi);sin(-11/180*pi);0;0]; %The first rotation quaternionq2=[cos(39/180*pi);0;sin(39/180*pi);0]; %The second rotation quaternionq3=[cos(-8/180*pi);0;0;-sin(-8/180*pi)]; %The third rotation quaternionr=quml(q1,q2); %call the quaternion multiplication subfunction q=quml(r,q3);P1=[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)];P2=[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)];P=P1*P2;gn=P*g;gn=gn(2:4)end3.2.4 case2 SINS quaternion algorithm code：function ans2clc;clear;%parameters initializing：T=0.005;K=13150;R=6371000; %radius of earthwE=7.292*10^(-5); %spinning rate of earthQ=zeros(4, 263001) ;%quaternion matrix initializinglongitude=zeros(1,13151);latitude=zeros(1,13151);H=zeros(1,13151); %altitude matrixQ(:,1)=[cos(45/180*pi);-sin(45/180*pi);0;0]; % initial quaternion longitude(1)=123; %initial longitudelatitude(1)=46; % initial latitudeH(1)=400; %initial altitudelength=0;g=9.8;vE = zeros(1,13151); %eastern velocityvN = zeros(1,13151); %northern velocityvH = zeros(1,13151); %upward velocityvE(1)=0;vN(1)=0;vH(1)=0;load imu.mat %data loading%main calculation sectionfor N=1:Kq1=zeros(4,11);q1(:,1)=Q(:,N);for n=1:20 % Attitude iteration wx=0.01/(3600*180)*pi*gm((N-1)*10+n,1); % Angle incrementwy=0.01/(3600*180)*pi*gm((N-1)*10+n,2);wz=0.01/(3600*180)*pi*gm((N-1)*10+n,3);w=[wx,wy,wz]';normw=norm(w); % Norm calculationW=[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-normw^2/48;C=1-normw^2/8;q1(:,n+1)=(C*I+S*W)*q1(:,n);Q(:,N+1)=q1(:,n+1);endWE=-vN(N)/R; % rotational angular velocity component of a geographic coordinate systemWN=vE(N)/R+wE*cos(latitude(N)/180*pi);WH=vE(N)/R*tan(latitude(N)/180*pi)+wE*sin(latitude(N)/180*pi);attitude=[WE,WN,WH]'*T; %correction of the quaternion by updating the rotation of geographic coordinatenormattitude=norm(attitude);e=attitude/normattitude;QG=[cos(normattitude/2);sin(normattitude/2)*e];Q(:,N+1)=quml(qinv(QG),Q(:,N+1));fx=1e-7*9.8*am(N,1); %specific force measured by accelerometerfy=1e-7*9.8*am(N,2);fz=1e-7*9.8*am(N,3);Fb=[fx fy fz]';F=quml(Q(:,N+1),quml([0;Fb],qinv(Q(:,N+1)))); %The specific force is decomposed into geographic coordinate system.FE(N)=F(2);FN(N)=F(3);FU(N)=F(4);%calculate the velocity of the vehicle:VED(N)=FE(N)+vE(N)*vN(N)/R*tan(latitude(N)/180*pi)-(vE(N)/R+2*wE*cos(latitude(N)/ 180*pi))*vH(N)+2*vN(N)*wE*sin(latitude(N)/180*pi);VND(N)=FN(N)-2*vE(N)*wE*sin(latitude(N)/180*pi)-vE(N)*vE(N)/R*tan(latitude(N)/180 *pi)-vN(N)*vH(N)/R;VUD(N)=FU(N)+2*vE(N)*wE*cos(latitude(N)/180*pi)+(vE(N)^2+vN(N)^2)/R-g;%integration and get the relative velocity of vehicle:vE(N+1)=VED(N)*T+vE(N);vN(N+1)=VND(N)*T+vN(N);vH(N+1)=VUD(N)*T+vH(N);% integration and get the position of vehicle:longitude(N+1)=vE(N)/(R*cos(latitude(N)/180*pi))*T/pi*180+longitude(N);latitude(N+1)=vN(N)/R*T/pi*180+latitude(N);H(N+1)=vH(N)*T+H(N);length=sqrt((vE(N))^2+(vN(N))^2)+length;endfigure(1) %picture the longitude-latitude curve of the motion within 1315 secondstitle(' longitude-latitude ');hold ongrid onplot(longitude,latitude);figure(2) % picture the altitude curve of the motion within 1315 secondstitle('altitude');hold ongrid onplot(0:13150,H);3.3Simulation outputs and results analysisIn case 1, the DCM algorithm and quaternion results are the same as follow:A =7.53165.9788-1.8892And the results suggest the solution of DCM and quaternion is equivalence in this problem.In case 2, the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis) and the altitude curve of the missile are shown as follows in Fig 3.3 and Fig 3.4.Fig3.3 the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis)Fig3.4the altitude curve of the missileImpression of the Assignment 3In this assignment, we simulate the missile navigation situation in a real problem, as the problem we have done before. I did this based on my own original code, but to my surprise, it’s harder than I have expected. For the detailed thoughts for my program I have forgotten. I have to recheck the code line by line, But I am still troubled in correcting the initial parameters for many times. It has taught me a lesson, which is never to be egotistical for your ability to memory and thework you have done or understood.。

Assignments of Inertial Navigation 《惯性导航》作业1Autumn 2018Assignment 1: Aircraft Displaying on the EarthAttached is a group of MA TLAB programs for displaying an aircraft near the surface of the Earth, as shown in figure 1.1 which is drawn by running the program main.m.If you input the longitude, latitude, heading, pitching, and rolling angles (all in degree), and click the updating button, then the aircraft are expected to be displayed at a proper position on the Earth, and with proper attitude. The coordinates of each vertex of the aircraft 3D model occupies a row in the matrix vtx.Before you click the updating button for the first time, the aircraft is hidden at the center of the Earth. Please rewrite the program redraw.m by adding the necessary rotation and shifting operations to the vertex coordinates of the aircraft. The acquired position and attitude information has been stored in the variables lon, lat, head, pitch and roll. With each updating, the coordinates of the aircraft in vtx in the MA TLAB drawing frame (same as the Earth frame) need to be re-calculated, instead of being kept the same as their values in the original 3D model (vtx0). That is, in the program redraw.m, you need to replace the command line vtx=vtx0 with your own codes for re-calculating vtx.And please explain the rationale behind your rewriting.作业报告一．理论基础a. 旋转变换矩阵在坐标系中，可以通过旋转角度知道一个点在新旧坐标系的坐标 在这样的旋转变化下存在如下的坐标变换矩阵：[x ′y ′z ′]=[1000cosαsinα0−sinαcosα][x y z] 同样的，绕y 轴和z 轴旋转的坐标变换矩阵为：[x ′y ′z ′]=[cosβ0−sinβ010sinβ0cosβ][xy z ] [x ′y ′z ′]=[cosγsinγ0−sinγcosγ0001][x y z ] 其中α、β、γ分别代表绕轴旋转的角度。

惯性导航系统基本工作原理惯性导航系统是十分复杂的高精度机电综合系统，只有当科学技术发展到一定高度时工程上才能实现这种系统，但其基本工作原理却以经典的牛顿力学为基础。

设质量 m 受弹簧的约束，悬挂弹簧的壳体固定在载体上，载体以加速度 a 作水平运动，则 m处于平衡后，所受到的水平约束力 F 与 a 的关系满足牛顿第二定律： a F。

测量水平约束力F，求的a，对a积分一次，即得水平速度，再m积分一次即得水平位移。

以上所述是简单化了的理性情况。

由于运载体不可能只作水平运动，当有姿态变化时，必须测得沿固定坐标系的加速度，所以加速度计必须安装在惯性平台上，平台靠陀螺维持要求的空间角位置，导航计算和对平台的控制由计算机完成。

陀螺仪组件测取沿运载体坐标系 3 个轴的角速度信号，并被送入导航计算机，经误差补偿计算后进行姿态矩阵计算。

加速度计组件测取沿运载体坐标系 3 个轴的加速度信号，并被送入导航计算机，经误差补偿计算后，进行由运载体坐标系至“平台坐标系”的坐标变换计算。

他们沿机体坐标系三轴安装，并且与机体固连，它们所测得的都是机体坐标系下的物理量。

参与控制和测量的陀螺和加速度计称为惯性器件，这是因为陀螺和加速度计都是相对惯性空间测量的，也就是说加速度计输出的是运载体的绝对加速度，陀螺输出的是运载体相对惯性空间的角速度或角增量。

而加速度和角速度或角增量包含了运载体全部的信息，所以惯导系统仅靠系统本身的惯性器件就能获得导航用的全部信息，它既不向外辐射任何信息，也不需要任何其他系统提供外来信息，就能在全天候条件下，在全球范围内和所有介质环境里自主、隐蔽的进行三维导航，也可用于外层空间的三维导航。

惯导系统的比力方程惯导系统根据与系统类型相应的数学方程（称之为力学编排）对惯性器件的输出作处理，从而获得导航数据。

尽管各种类型的系统相应的力学编排各不相同，但他们都源自同一个方程：比力方程。

比力方程描述了加速度计输出量与运载体速度之间的解析关系：式中： v eT为运载体的地速向量； f 为比力向量，是作用在加速度计质量块单位质量上的非引力外力，由加速度计测量；g 为重力加速度；ie 为地球自转角速度；eT 为惯性平台所模拟的平台坐标系T 相对地球的旋转角速度；dv eTdt表示在平台坐标系 T 内观察到的地速向量的时间变化率。