惯性导航作业
导航系统大作业
导航系统1．简述捷联惯性系统中地理系到机体系的姿态阵bg C 其含义及其功能。
答:含义:导航坐标系g g g O x y z -到机体坐标系b b b O x y z -的一组欧拉角为,,θγψ，导航坐标系经过3次转动到机体坐标系。
g g g x y z 依次沿g O z -、'b O x -、''b O y -旋转角度-ψ、θ、γ后到b b b x y z 。
姿态矩阵中包含了机体的姿态角方位角ψ、俯仰角θ和横滚角γ。
功能：机体陀螺仪输出的角速度信息经过补偿后,积分得到机体坐标系与导航坐标系的姿态信息和姿态转移矩阵。
捷联惯导系统中,加速度计与载体固连,利用姿态阵完成加速度计输出信息从机体坐标到导航坐标的转换。
转换后的加速度计信息经过积分可得到机体在导航坐标系下的速度和位置。
2．画出并用式表达速度三角形（地速、控速、风速)及航迹角、航向角与偏流角之间的关系.答：风速：空气相对于地面的运动速度;空速:飞机相对于空气运动的速度；地速：飞机相对于地面的运动速度。
=+v v v 风地空航向角：机头在水平面投影与真北方向的夹角ϕ;偏流角:空速矢量和地速矢量之间的夹角,用δ表示；航迹角:飞机速度矢量在水平面投影与真北方向的夹角。
航向角ϕ加上偏流角δ等于地速v 地的方位角α。
v 地v 空v 风3．简述惯性导航系统、卫星导航系统、多普勒导航、塔康、VOR/DME 、天文导航其各自的基本工作原理、特点及误差特性。
答:一、惯性导航系统（1）工作原理以牛顿力学定律为基础，以陀螺仪和加速度计为敏感器件进行导航参数解算。
系统根据陀螺仪的输出建立导航坐标系，根据加速度计输出解算出运载体的速度和位置，从而实现姿态和航向解算. （2）特点惯性导航系统不需要任何外来信息，也不会向外辐射任何信息，仅依靠惯性器件就能全天候，全球性的自主三维定位和三维定向,同时具备自主性、隐蔽性和信息的完备性。
（3）误差特性误差随时间积累，短时间导航精度较高。
哈工大惯性技术(导航原理)大作业讲解
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.。
实验三惯性导航实验
实验三惯性导航实验小组成员：杨曦陈魁吴航杨少帅一、 实验目的1、了解惯性导航设备;2、掌握惯性导航设备的物理连接；3、掌握惯性导航信息的处理方法；4、掌握惯性导航方法并学会用编程实现惯性导航算法。
二、 实验器材YH-5000AHRS ；工业控制计算机；数据采集软件； 稳压电源；串口连接线；三、 实验原理（1） 姿态解算基于四元数法解算姿态矩阵。
p j p i p l Q +++=21 （1）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡--++----+++---+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡b b b p b b b b p p p z y x C z y x p p p l lp p p lp p p lp p p p p p l lp p p lp p p lp p p p p p l z y x 222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2 （2） b pbQw Q 21= （3） 上述微分方程表示成矩阵形式：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡321321000021p p p l w w w w w w w w w w w w p p p l b pbxb pbyb pbz b pbx b pbz b pbyb pby b pbz b pbxb pbz b pbyb pbx（4） 初始四元数的确定计算如下：⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡--++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡2c o s 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2s i n 2c o s 2s i n 2c o s 2s i n 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2co s )0(3)0(2)0()0(0000000000000000000000001γθϕγθϕγθϕγθϕγθϕγθϕγθϕγθϕG G G G G G G G p p p l （5） 用四阶龙格库塔法解（4）的微分方程；⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=333231232221131211T T T T T T T T T C p b 由p b C 中提取γϕλ，，G231sin T -=主λ 22211tan T T G -=主ϕ)(tan 33131T T --=主γ 从而可得到：主λλ=⎪⎩⎪⎨⎧<>+>><+=0,020,002122212222T T T T T GG G G πϕϕπϕϕ主主主⎩⎨⎧<>=0,)(-0331333T T sign T πγγγ （2） 速率位置解算将加速度测量的沿坐标系轴向的比力bib a 转换成沿着导航坐标系轴向的比力p ib a ,则速度方程为:p p epp ep p ib p ep g V w a V +⨯+Ω-=)2( 展开得到：⎪⎩⎪⎨⎧-+Ω-+Ω+=+Ω+Ω-=+Ω-Ω+=gV w V w a V V w V a V V w V a V p epy p epx p x p epx p epy p y p ibz pepz p epzp epx p x p epx p z p iby p epy p epzp epy p y p epy p z p ibx p epx )2()2()2(2)2(2 由于Ω,pep w 都很小，故而速度方程简化为：⎪⎩⎪⎨⎧-===ga V a V a V p ibz pepz piby p epy p ibxp epx用一阶欧拉法解，则：⎪⎩⎪⎨⎧+-=++=++=+)(*)()()(*)()(*)(t V T g a T t V t V T a T t V t V T a T t V p epz p ibz p epzpepy p iby p epy pepx p ibx p epx 其中T 为采样时间。
导航原理大作业
导航原理作业（惯性导航部分）一、题目内容一枚导弹采用捷联惯性导航系统，三个速率陀螺仪Gx, Gy, Gz和三个加速度计Ax, Ay, Az的敏感轴分别沿着着弹体坐标系的Xb, Yb, Zb轴。
初始时刻该导弹处在北纬45.75度，东经126.63度。
第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。
初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。
此后弹体坐标系Xb-Yb-Zb相对地理坐标系的转动如下：首先，弹体绕Zb（方位轴）转过-10 度；接着，弹体绕Xb（俯仰轴）转过15 度；然后，弹体绕Yb（滚动轴）转过20 度；最后弹体相对地面停止旋转。
请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计Ax, Ay, Az的输出。
取重力加速度的大小g = 9.8m/s2。
第二种情形：导弹正在飞行中。
初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m，初始北向速度1800 m/s，初始东向速度和垂直速度都为零。
陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表0.00001弧度的角增量。
加速度计输出的每个脉冲代表1μg，1g = 9.8m/s2。
陀螺仪和加速度计输出的采样频率都为10Hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat中，内含一矩阵变量ga，有2000行，6列。
每一行中的数据代表每个采样时刻三个陀螺Gx, Gy, Gz将地球视为理想的球体，半径6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。
选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。
（1） 请计算200秒后弹体到达的经纬度和高度，东向和北向速度； （2） 请计算200秒后弹体相对当地地理坐标系的姿态四元数；（3） 请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）； （4） 请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。
11惯性导航与组合导航作业(无水印)
惯导作业一、填空题1.惯性导航系统是一种不依赖任何外部信息、也不向外部辐射能量的______导航系统。
答案：自主式2.不依赖外界信息，只靠对载体本身的______、来完成导航任务的技术称做惯性导航，也称为自主式导航答案：惯性测量3. 加速度计其输出一般是______、，但在积分加速度计的情况下则输出为______、。
答案：速度、加速度4. 惯性器件就是测量载体______、和______、参数的传感器。
答案：线运动、角运动5. 加速度经过一次积分可以得到______，经过二次积分得到______。
答案：运动速度、运动距离6. 描述角运动的参数有______、______。
答案：姿态角、姿态角速度7. 描述线运动的参数有______、______、______。
答案：位移、速度、加速度8. 高速旋转的自由陀螺仪，当不受外力矩作用时，其主轴将保持它在空间的______方向不变。
答案：初始9. 由表观运动所引起的陀螺______偏离当地地垂线的误差，称之为陀螺仪的“表观误差”。
答案：自转轴二、单选题1．陀螺自转轴方向相对惯性空间保持不变，以地球作为参考基础，陀螺自转轴相对地球表面的转动，为（）。
A．表观运动B．自转运动C．定轴运动D．进动运动答案：A三、多选题（每题1分）1．惯性导航系统的核心有（）A．加速度计、B．陀螺仪C．导航计算机D．GPS答案：ABC2．惯性导航系统的基本组成（）A．加速度计B．模拟某一坐标系的惯性平台C．导航计算机D．控制显示器答案：ABCD3．激光陀螺特点有哪些（）。
A．抗干扰能力弱B．启动快C．动态特性较宽D．稳定性好答案：BCD4.关于组合导航系统，下列说法正确的是（）。
A．提高导航系统的精度B．提高导航系统的可靠性C．提高导航系统的安全性D．启动快答案：ABC四、判断题1. 一个沿直线运动的载体，只要借助于加速度计测出它的加速度，那么，载体在任何时刻的速度和相对出发点的距离就可以实时地计算出来。
北航惯导第一次大作业
《惯性导航原理》第一次大作业一、 原理分析惯导系统为指北方位的平台系统，则利用比力方程以及陀螺提供的东、北、天三个比力数据，即可计算得到在每个数据采集点的平台即时速度，再通过经纬度的计算公式，就可以得到每个数据采集点平台的即时经纬度，以每个数据采集点为下一个采集点的起点，即可对速度和经纬度进行累计计算，从而得到平台在运动过程中任意时刻的速度和位置情况。
运动过程中任意时刻的速度和位置情况。
1.模型公式的推导载体相对地球运动时，载体相对地球运动时，加速度计测得的比力表达式，加速度计测得的比力表达式，加速度计测得的比力表达式，称为比力方程，称为比力方程，称为比力方程，方程如方程如下：下：g V V f epep ieep-´++=)2(vv （1）在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：入得：t tt ett iettgV f V+´+-=)2(v v （2）系统中测量的是比力分量，将上式写成分量形式系统中测量的是比力分量，将上式写成分量形式=-+ （3） 又因为地球的自转角速率为：又因为地球的自转角速率为：(4)地理坐标系相对于地球坐标系的角速率为：地理坐标系相对于地球坐标系的角速率为：= (5)将（将（44）（5）两个式子带入（）两个式子带入（33）式，即可得到如下方程组：）式，即可得到如下方程组：（6）2.速度计算作业要求只考虑水平通道，作业要求只考虑水平通道，因此只需要计算正东、因此只需要计算正东、因此只需要计算正东、正北两个方向的速度即可。
正北两个方向的速度即可。
正北两个方向的速度即可。
理理论上计算得到t x V 、t y V 后，再积分一次可得到速度值，即后，再积分一次可得到速度值，即ïîïíì+=+=òòt t y t y t ytt x t x tx V dt V V V dt V V 000但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现：（7）此方程组表示了从第i 个采集点到第（个采集点到第（i+1i+1i+1）个采集点的速度递推公式。
哈工大惯性技术(导航原理)大作业
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.。
惯性导航课程实验报告问问
一.实验目的1．认识三轴惯性平台的各个组成器件2.讨论验证三轴平台的工作原理，并对其稳定回路及工作过程做出分析二．实验原理一个双自由度陀螺有两个测量轴，可为平台提供两个轴的稳定基准，而三轴平台要求陀螺为平台提供三个轴的稳定基准，所以三轴平台需要两个双自由度陀螺。
设两个陀螺的外环轴均平行于平台的方位轴安装，则内环轴自然平行于平台平面。
在正常工作状态下，两个陀螺的自转轴也平行于平台台面，且相互之间保持垂直关系，即两个陀螺的内环轴之间也保持垂直关系。
两个陀螺的内环轴作为平台绕两个水平轴稳定的基准，而两个陀螺的外环轴之一，作为平台绕方位轴稳定的基准。
三.实验内容1.方位稳定轴的空间积分状态在双自由度陀螺构建的三轴惯性平台中，平台的方位稳定回路陀螺2外环轴上的信号器，放大器，平台方位轴上的稳定电机等组成。
当干扰力矩作用在平台的方位轴上时，平台绕方位轴转动偏离原有的方位，而平台上的陀螺却具有稳定性。
这样，平台相对陀螺外环出现了偏转角，陀螺2外环轴上的信号器必有信号输出，经放大器放大后送至平台方位轴上的稳定电机，方位稳定电机输出稳定力矩作用到平台方位轴上，从而平衡作用在平台方位轴上的干扰力矩，使平台绕方位轴保持稳定。
同样，给陀螺2内环轴上的力矩器输入与指令角速度大小成比例的电流，可实现方位稳定轴的空间积分要求2.水平稳定回路的工作如下图所示由三个单轴平台直接叠加的三轴平台在航向变化时，平台上的陀螺与稳定电机之间的相对位置关系．图(a)表示航向为零，即方位环环对俯仰环没有转角时陀螺与稳定电机之间的相对位置关系，此时的陀螺Ⅱ感受沿横滚轴（纵向）方向作用到平台上的干扰力矩，信号器输出的信号经横滚放大器A．放大后给横滚轴稳定电机，产生纵向稳定力矩，使平台沿纵向（x.轴）保持稳定，陀螺I感受沿俯仰轴（横向）方向作用到平台上的干扰力矩。
经信号器．放大器和俯仰轴稳定电机，产生沿横向的稳定力矩．使平台沿横向保持稳定。
同样，若给两个陀螺的力矩器输入与指令角速度成比例的电流，平台也可正常工作在空间积分状态。
惯性导航技术的工作原理
惯性导航系统基本工作原理惯性导航系统是十分复杂的高精度机电综合系统，只有当科学技术发展到一定高度时工程上才能实现这种系统，但其基本工作原理却以经典的牛顿力学为基础。
设质量 m 受弹簧的约束，悬挂弹簧的壳体固定在载体上，载体以加速度 a 作水平运动，则 m处于平衡后，所受到的水平约束力 F 与 a 的关系满足牛顿第二定律： a F。
测量水平约束力F，求的a，对a积分一次，即得水平速度，再m积分一次即得水平位移。
以上所述是简单化了的理性情况。
由于运载体不可能只作水平运动，当有姿态变化时，必须测得沿固定坐标系的加速度，所以加速度计必须安装在惯性平台上，平台靠陀螺维持要求的空间角位置，导航计算和对平台的控制由计算机完成。
陀螺仪组件测取沿运载体坐标系 3 个轴的角速度信号，并被送入导航计算机，经误差补偿计算后进行姿态矩阵计算。
加速度计组件测取沿运载体坐标系 3 个轴的加速度信号，并被送入导航计算机，经误差补偿计算后，进行由运载体坐标系至“平台坐标系”的坐标变换计算。
他们沿机体坐标系三轴安装，并且与机体固连，它们所测得的都是机体坐标系下的物理量。
参与控制和测量的陀螺和加速度计称为惯性器件，这是因为陀螺和加速度计都是相对惯性空间测量的，也就是说加速度计输出的是运载体的绝对加速度，陀螺输出的是运载体相对惯性空间的角速度或角增量。
而加速度和角速度或角增量包含了运载体全部的信息，所以惯导系统仅靠系统本身的惯性器件就能获得导航用的全部信息，它既不向外辐射任何信息，也不需要任何其他系统提供外来信息，就能在全天候条件下，在全球范围内和所有介质环境里自主、隐蔽的进行三维导航，也可用于外层空间的三维导航。
惯导系统的比力方程惯导系统根据与系统类型相应的数学方程（称之为力学编排）对惯性器件的输出作处理，从而获得导航数据。
尽管各种类型的系统相应的力学编排各不相同，但他们都源自同一个方程：比力方程。
比力方程描述了加速度计输出量与运载体速度之间的解析关系：式中： v eT为运载体的地速向量； f 为比力向量，是作用在加速度计质量块单位质量上的非引力外力，由加速度计测量；g 为重力加速度；ie 为地球自转角速度；eT 为惯性平台所模拟的平台坐标系T 相对地球的旋转角速度；dv eTdt表示在平台坐标系 T 内观察到的地速向量的时间变化率。
北航惯性导航综合实验三实验报告
北航惯性导航综合实验三实验报告惯性导航技术综合实验实验三惯性导航综合实验实验3.1 初始对准实验一、实验目的结合已经采集并标定好的惯性传感器数据进行粗对准，了解实现对准的过程；通过比较不同传感器数据的对准结果，进一步认识惯性传感器性能在导航系统中的重要地位。
为在实际工程设计中针对不同应用需求下采取相应的导航系统方案提供依据。
二、实验内容利用加速度计输出计算得到系统的初始姿态，利用陀螺输出信号计算航向角。
对比利用不同的惯性传感器数据计算所得的不同结果。
三、实验系统组成MEMS IMU（或其他类型IMU）、稳压电源、数据采集系统与分析系统。
四、实验原理惯导系统在开始进行导航解算之前需要进行初始对准，水平对准的本质是将重力加速度作为对准基准，其对准精度主要取决于两个水平加速度计的精度，加速度计的零位输出将会造成水平对准误差；方位对准最常用的方位是罗经对准，其本质是以地球自转角速度作为对准基准，影响对准精度的主要因素是等效东向陀螺漂移。
(1) 其中，分别为当前时刻的俯仰角和横滚角计算值。
水平对准误差和方位对准误差如下所示：，(2) 五、实验步骤及结果1、实验步骤：采集静止状态下水平加速度计输出，按下式计算其平均值。
(3) 其中，为前n个加计输出均值；为前n-1个加计输出均值；为当前时刻加计输出值。
利用加计平均值来计算系统初始俯仰角和横滚角(4) 其中，分别为当前时刻的俯仰角和横滚角计算值。
2、实验结果与分析：2.1、用MIMS IMU的加速度计信息计算（1）俯仰角和横滚角图：（2）失准角：2.2、实验结果分析以上计算是基于MIMS IMU静止时data2进行的初始对准，与data2给定的初始姿态角相差不大。
六、源程序clear clc g = 9.__14; a=load(＇E:＼郭凤玲＼chushiduizhun＼data2.txt＇); ax=a(:,4)＇; ay=a(:,5)＇; az=a(:,6)＇; %初始值ax0(1)=ax(1)/1000*g; %%%%转化单位，由mg转化为m/s^2 ay0(1)=ay(1)/1000*g; az0(1)=az(1)/1000*g; theta(1)=asin(ay(1)/az(1)); gama(1)=-asin(ax(1)/az(1)); for i=2:__ ax0(i)=ax0(i-1)+(ax(i)-ax0(i-1))/i; ay0(i)=ay0(i-1)+(ay(i)-ay0(i-1))/i; az0(i)=az0(i-1)+(az(i)-az0(i-1))/i; theta(i)=asin(ay0(i)/az0(i));gama(i)=-asin(ax0(i)/az0(i)); end detfaix=mean(ay0)/g; detfaiy=mean(-ax0)/g; t=1:__; plot(t,theta,＇r＇,t,gama,＇b＇) title(＇俯仰角和横滚角＇);ylabel(＇弧度（rad）＇); legend(＇俯仰角＇,＇横滚角＇) 实验3.2 惯性导航静态实验一、实验目的1、掌握捷联惯导系统基本工作原理2、掌握捷联惯导系统捷联解算方法3、了解捷联惯导系统误差传递规律和方程二、实验原理捷联惯性导航系统（SINS）的导航解算流程如图1所示。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理：②掌握采用卡尔曼波波方法进行捷联惯导/GPS组合的基本原理：③掌握捷联惯导/GPS组合导航系统静态性能：④掌握动态情况下捷联惯导/GPS组合导航系统的性能。
二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套：②监控计算机一台。
③差分GPS接收机一套：④实验车一辆：⑤车载大理石平台：⑥车载电源系统。
四、实验内容D实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接；③打开GPS接收机电源，确认可以接收到4颗以上卫星；④打开电源，启动实验系统。
2）捷联惯导/GPS组合导航实验①进入捷联惯导初始对准状态，记录IMU的原始输出，注意5分钟内严禁移动实验车和IMU；②实验系统经过5分钟初始对准之后，进入导航状态；③移动实验车，按设计实验路线行驶；④利用监控计算机中的导航软件进行导航解算，并显示导航结果。
五、实验结果及分析（一）理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导：短时段内（t<5min）,忽略地球自转绍。
=0,运动轨迹近似为平面1/R = O,此时的位置误差分析可简化为：Vr2（1）加速度计零偏V引起的位置误差：Jx,= —= 0.8802 m2（2）失准角我引起的误差：3X2=^-= 0.9218m（3）陀螺漂移£引起的误差：= 0.0137 m6可得1min后的位置误差值5x =+ J.r2 + 5x. = 1.8157m2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min的位置及位置误差图：（2）纯惯导解算1min的速度及速度误差图：vy 8060| 40200 1000 2000 3000 4000 5000 60000.01s实验结果分析：纯惯导解算短时间内精度很高，1min的惯导解算的北向最大位移误差-2.668m,东向最大位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS为真实值也会带来误差；另外，可见1min内纯惯导解算的东向速度最大误差-0.2754m/s，北向速度最大误差-0.08027m/So(-)选取IMU前5分钟数据进行对准实验。
北航惯性导航作业二.
惯性导航作业一、数据说明：1：惯导系统为指北方位的捷连系统。
初始经度为116.344695283度、纬度为39.975172度,高度h为30米。
初速度v0=[-9.993908270;0.000000000;0.348994967]。
2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。
3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为一~三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。
5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度);二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。
用角增量法。
2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。
3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。
(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。
哈工大导航原理大作业
《导航原理》作业(惯性导航部分)一、题目要求A fighter equipped with SINS is initially at the position of ︒35 NL and︒122 EL,stationary on a motionless carrier. Threegyros X 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 therunway 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,and the accelerometer ’s is 1Hz. The output of gyros and accelerometers within 5400s are stored in MATLAB data files named and , containing matrices gm of 35400⨯ 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 be seen as an ideal sphere, with radius and spinning raterad/s 10292.75-⨯, Theerrors of sensors are ignored, so is theeffect of height on the magnitude of gravity. The output of the gyros are to integrated every . 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解答方向余弦阵法（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ψ⎪⎪⎪⎭⎫ ⎝⎛︒-︒-︒-︒-=⎪⎪⎪⎭⎫ ⎝⎛=1000）50（cos ）50（sin -0）50（sin ）50（cos 1000cos sin -0sin cos ψψψψψC 所以变换后的坐标X E Y C C C N Z ϕθψζ⎛⎫⎛⎫ ⎪⎪= ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭由于初始时刻有009.8E N ζ⎛⎫⎛⎫ ⎪ ⎪= ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭所以计算得三个加速度计的输出分别是 ,/4504.42x s m A -=,/6027.22y s m A -=s m A /3581.8z =2计算程序见附录一四元数法（1）绕Xb 轴转过ψ︒=10ϕ i 2sin2cos q ϕϕϕ+=（2）绕Yb 轴转过︒=30θj 2sin2cosq θθθ+=（3）绕Zb 轴转过︒-=50ψk 2sin2cosq ψψψ+=则合成四元数)2sin 2(cos )2sin 2(cos )2sin 2(cosk j i q q q q ⋅+⋅+⋅+==ψψθθϕϕψθϕ合成四元数的逆1123q p i p j p k λ-=---由公式 q N E qZ YX ⎪⎪⎪⎭⎫ ⎝⎛=⎪⎪⎪⎭⎫⎝⎛-ξ1计算得三个加速度计的输出分别是 ,/4504.42x s m A -=,/6027.22y s m A -=s m A /3581.8z =2由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。
惯性导航原理实验课程设计
惯性导航原理实验课程设计一、课程目标知识目标：1. 学生能够理解惯性导航系统的基本原理，掌握其工作方式和组成结构。
2. 学生能够掌握惯性导航系统中的关键参数，如加速度、角速度等，并了解它们对导航精度的影响。
3. 学生能够描述惯性导航系统在不同环境下的误差来源及其补偿方法。
技能目标：1. 学生能够操作惯性导航实验设备，进行数据采集、处理和分析。
2. 学生能够利用实验数据，结合理论知识，完成简单的惯性导航路径推算。
3. 学生能够通过实验，培养实际操作能力和团队合作能力。
情感态度价值观目标：1. 学生能够培养对惯性导航技术的好奇心与探索精神，提高对物理学科的兴趣。
2. 学生能够认识到惯性导航技术在现实生活中的应用，增强科技改变生活的意识。
3. 学生能够通过课程学习，树立正确的科学态度，培养严谨、务实的作风。
课程性质分析：本课程为物理学科实验课程，以惯性导航原理为教学内容，结合实际操作，提高学生的实践能力。
学生特点分析：学生为高中二年级学生，已具备一定的物理知识和实验操作能力，对高新技术具有较强的好奇心。
教学要求：课程设计应注重理论与实践相结合，强调学生的动手能力，将抽象的物理原理具体化，提高学生的学习兴趣和实际应用能力。
通过课程目标的分解，使学生在学习过程中达到预期的学习成果，为后续教学设计和评估提供依据。
二、教学内容1. 理论知识：- 惯性导航系统原理：包括牛顿运动定律、惯性参考系等基本概念。
- 惯性导航系统组成：陀螺仪、加速度计、计算机及软件等。
- 惯性导航系统误差分析：系统误差、随机误差、环境因素等影响。
- 误差补偿方法：系统标定、卡尔曼滤波等。
2. 实践操作：- 惯性导航设备认识：了解设备结构、功能及操作方法。
- 数据采集与处理：学习如何采集加速度、角速度等数据，并进行处理分析。
- 惯性导航路径推算：利用采集到的数据，结合理论知识，进行路径推算。
3. 教学大纲安排：- 第一课时：惯性导航系统原理及组成介绍。
惯性导航基本原理的应用
惯性导航基本原理的应用1. 惯性导航的定义和基本原理•惯性导航是一种利用加速度计、陀螺仪和磁力计等惯性传感器测量物体在空间中的位置、速度和姿态的技术。
•基本原理是利用物体的质量、转动惯量和力矩等物理量来实现导航的目的。
2. 惯性导航的主要应用领域•航天航空：惯性导航常用于飞行器、导弹和卫星等的导航和定位。
•汽车导航：利用惯性导航可以实现车辆的自动导航和车辆行驶轨迹的记录。
•室内导航：通过惯性导航可以实现室内定位和导航，提供室内位置相关的服务。
3. 惯性导航系统的组成部分•加速度计：用于测量物体在三个方向上的加速度。
•陀螺仪：用于测量物体的角速度和角位移。
•磁力计：用于测量地球磁场的强度和方向。
4. 惯性导航系统的工作原理•加速度计测量物体在三个方向上的加速度，通过积分可以计算出速度和位移。
•陀螺仪测量物体的角速度，在已知初始角度的情况下可以计算出姿态的变化。
•磁力计测量地球磁场的强度和方向，可以用来矫正姿态的偏差。
5. 惯性导航系统的误差与校正方法•加速度计误差：包括零偏误差和尺度因子误差。
可以通过自校准和温度补偿的方法进行校正。
•陀螺仪误差：包括零偏误差和尺度因子误差。
可以通过自校准和温度补偿的方法进行校正。
•磁力计误差：包括零偏误差和刻度因子误差。
可以通过地磁校正和磁场补偿的方法进行校正。
6. 惯性导航系统的发展趋势•小型化：惯性导航系统的体积和重量越来越小，适用于更多场景。
•集成化：惯性传感器和处理器的集成度越来越高，整个系统更加稳定可靠。
•高精度：惯性导航系统的精度逐渐提高，适用于更精细的导航和定位需求。
7. 总结•惯性导航是一种基于物理原理的导航技术，可广泛应用于航天航空、汽车导航和室内导航等领域。
•惯性导航系统由加速度计、陀螺仪和磁力计等组成，利用测量结果计算物体的位置、速度和姿态。
•惯性导航系统存在误差，可以通过校正方法进行修正。
•惯性导航系统的发展趋势是小型化、集成化和高精度。
•惯性导航技术在导航和定位领域的应用前景广阔，并有望在未来取得更大的突破。
惯导大作业
移动多媒体和流媒体技术的应用
内容获 取和编码
转化为 流媒体
流媒体 服务
传输 播放
iCloud应用
Siri技术的升级应用
“最简单的 用户界面”
“虚拟个 人助理”
Siri语音系 统目前支持 英语、法语 和德语
有趣， 而且还是 免费的哦！
交互式电视的应用
多媒体信息发布。主要应用场所有电子图 书馆、政府企业等。 影视歌曲点播。主要应用场所有卡拉 OK、 宾馆、饭店、住宅小区、有线电视台等 教育和培训。主要应用场所有校园网和多 媒体教室、远程教学、企业内部培训、医 院病理分析和远 程医疗等。 交互式多媒体展示。主要应用场所有机场、 火车站、影剧院、展览馆、博物馆、商场、 百货公司等。
惯性导航系统的初始校准
主要介绍内容
总述
平台姿态校准
陀螺漂移及指令速率标度因数
由舒勒回路特性确定标度因数
零时刻快速平台姿态校准
总述
【1】严格要求：惯导系统在进入导航状态 之前，必须经过初始校准。 【2】初始校准的内容：确定并修正陀螺仪 的初始漂移、指令速率标度因数、加速度计 的零位误差及标度因数
平台姿态校准
粗校准
外界提供 的姿态参 考信息
自身惯性元 件敏感平台 失调角组成 闭环系统来 自校准
平台姿态校准
基本思想：使平台坐标系和地理坐标系趋于一致。 两方面内容：水平校准和方位校准。 iPhone 缺陷：当载体运动在两极时则不能实现方位校准。
为什么？
iPhone4 iTV
水平校准的实现
基
础 水平校准 的 实现过程
实 际 回
路
系统参数及举例说明
方位校准的实现
整个过程要对东向 陀螺仪施加信号 直到规定的时间
惯性导航轨道几何状态检测技术于普铁线路大机捣固作业中的应用
惯性导航轨道几何状态检测技术于普铁线路大机捣固作业中的应用发布时间：2021-03-25T15:40:56.423Z 来源：《基层建设》2020年第29期作者：肖志锋[导读] 摘要：近年来，工务维修体制改革有序推进，在推行集中修、机械修方面取得长足进步，随着2019年4月份《普速铁路线路修理规则》的颁布实施，更加明确了“检、养、修”分开，检测、修理专业化建设，以及设备维修以大型养路机械为主、小型养路机械为辅的养修理念。
中国铁路广州铁路局集团有限公司长沙工务段湖南长沙 410000摘要：近年来，工务维修体制改革有序推进，在推行集中修、机械修方面取得长足进步，随着2019年4月份《普速铁路线路修理规则》的颁布实施，更加明确了“检、养、修”分开，检测、修理专业化建设，以及设备维修以大型养路机械为主、小型养路机械为辅的养修理念。
根据惯性导航技术的相对测量原理，通过确定检测小车的位置轨迹线，并与既有铁路线型进行拟合对比分析，得到既有铁路线型的实际偏差数据，形成作业方案，用于指导普铁线路大机捣固作业，最大程度优化既有铁路线型，提升线路整体平顺度。
关键词：惯性导航；测量；应用；线路大机捣固；效果0 引言随着铁路的多次提速，列车运行速度越来越快，如何在确保行车安全的前提下最大化提升线路质量也显得越来越重要。
准确、及时、全面快速检测铁路线路危及行车安全的病害、掌握线路状况、精确制定轨道作业计划是确保铁路行车安全的关键。
在倡导大型机械作业为主、小型机械作业为辅，倡导最大化提升劳动效率、降低运营成本、提升信息化的今天【1】，应不断探索新的轨道几何尺寸检测技术，为日常轨道线路的养护维修方案制定提供数据支撑。
目前，借助CPIII（基桩控制网）对既有线路进行精确测量，在国内运用比较普遍。
但该方法受自身技术特点限制，建设难度大，维护和使用成本高，且无法实现快速轨检测量，不适用于未设置CPIII工程测量网的既有普速线路【2】。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
惯性导航作业一、数据说明：1：惯导系统为指北方位的捷连系统。
初始经度为116.344695283度、纬度为39.975172度,高度h为30米。
初速度v0=[-9.993908270;0.000000000;0.348994967]。
2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。
3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为一~三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。
5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度);二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。
用角增量法。
2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。
3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。
(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。
三、基本原理和公式1、初始姿态矩阵的确定：根据初始姿态角求四元数：0123cos cos cos sin sin sin 222222cossin cos sin cos sin 222222cos cos sin sin sin cos 222222cossin sin sin cos cos 222222abab abab ab ab abab q q q q ψψθγθγψψθγθγψψθγθγψψθγθγ=-=-=+=+再根据四元数求方向余弦矩阵的初始矩阵：()()()()()()222201231203130222221203012323012222130223010123222222b t q q q q q q q q q q q q C q q q q q q q q q q q q q q q q q q q q q q q q ⎡⎤+--+-⎢⎥=--+-+⎢⎥⎢⎥+---+⎣⎦2、指北方位系统的运动解算：“平台”指令角速度为：()()cos sin tan()ty yt tt x it ie xt tx ie xt V R V L R V L L R ωωω⎡⎤-⎢⎥⎢⎥⎢⎥⎢⎥=+⎢⎥⎢⎥⎢⎥+⎢⎥⎣⎦加速度计获得的比力信息bib f 为载体坐标系中各个轴向的比力，而我们需要的比力t it f 为地理坐标系中各个轴向的比力，它们之间应用矩阵t b C 做变换：()()1t t bit b ibT t b b bttf C f C CC -=⨯==根据比力信息可以求出各个方向上的加速度：()()()()(2sin tan())(2cos )(2sin tan())(2cos )t t t t tt x x x ibxie y ie zxt xt t t y tt tt x y ibyie x zxt ytt ty t tt t x z ibz ie x y xt ytV V V fL L V L V R R V V V fL L V V R R V V V f L V V gR R ωωωω•••=++-+=-++=+++-因此可以求得速度为：t tt t xx x t t t t y y y V V dt V V V dt V ••=+=+⎰⎰载体所在位置的地理纬度L 、经度λ可由下列方程求得：000)sec(λλ+=+=⎰⎰dt L R V L dt R V L txtxttytyt3、四元数姿态矩阵的更新：b b b t tb ib t it C ωωω=-式中，bib ω为陀螺所测得的角速度。
用毕卡逼近法更新b t C 的值，T 为采样时间bib T θω∆=⨯[]0000x y z xz y y z x z yxθθθθθθθθθθθθθ-∆-∆∆⎡⎤⎢⎥∆∆-∆⎢⎥∆=⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦ 22220x y z θθθθ∆=∆+∆+∆()()()()[]()2420001118384248q n I q n θθθθ⎧⎫⎛⎫⎛⎫∆∆∆⎪⎪+=-++-∆ ⎪ ⎪⎨⎬ ⎪ ⎪⎪⎪⎝⎭⎝⎭⎩⎭4、姿态角的求解：姿态角与姿态矩阵的关系：cos cos sin sin sin cos sin sin sin cos sin cos cos sin cos cos sin sin cos cos sin sin sin sin cos sin cos cos cos ab abab abb t ababab abab abC γψγθψγψγθψγθθψθψθγψγθψγψγθψγθ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+--⎣⎦式中θ,γ,ab ψ分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改，将ab ψ-代入原公式可得现公式。
如果记111213212223313233b t T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦则由以上两式即可解算出姿态角：()1231133312122sin tan tan T T T T T θγϕ---=⎛⎫=- ⎪⎝⎭⎛⎫=⎪⎝⎭四、程序流程图五、结果六、小结这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。
和上次不同，这次遇到了较多的问题。
首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。
其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。
总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。
七、程序clccleara=load('C:\Users\Administrator\Documents\MATLAB/jlfw.dat');Wib_INSc=a(:,2:4)';f_INSc=a(:,5:7)';%第一列：数据包序号第二至四列：分别为东、北、天向陀螺仪角速率信息wib_INSc（单位：rad/s）%第五至七列：分别为东、北、天向比力信息f_INSc（单位：m/s^2）.L(1,:)=zeros(1,60001);Lambda(1,:)=zeros(1,60001);Vx(1,:)=zeros(1,60001);Vy(1,:)=zeros(1,60001);Vz(1,:)=zeros(1,60001);Rx(1,:)=zeros(1,60001);%定义存放卯酉圈曲率半径数据的矩阵Ry(1,:)=zeros(1,60001);%定义存放子午圈曲率半径数据的矩阵psi(1,:)=zeros(1,60001);%定义存放偏航角数据的矩阵theta(1,:)=zeros(1,60001);%定义存放俯仰角数据的矩阵gamma(1,:)=zeros(1,60001);%定义存放滚转角数据的矩阵I=eye(4); %定义四阶矩阵L(1,1)=39.975172/180*pi;%纬度初始值单位：弧度Lambda(1,1)=116.344695283/180*pi;%经度初始值单位：弧度Vx(1,1)=-9.993908270;%初始速度x方向分量Vy(1,1)=0;%初始速度y方向分量Vz(1,1)=0.348994967;%初始速度z方向分量Wibx(1,:)=a(:,2); %提取陀螺正东方向角速率并定义Wiby(1,:)=a(:,3); %提取陀螺正北方向角速率并定义Wibz(1,:)=a(:,4); %提取陀螺天向角速率并定义fibbx(1,:)=a(:,5);%x方向的比力数据fibby(1,:)=a(:,6);%y方向的比力数据fibbz(1,:)=a(:,7);%z方向的比力数据g0=9.7803267714;Wie=7.292115147e-5;%地球自转角速度Re=6378245;%长半径e=1/298.3;%椭圆度t=0.01;%采样时间psi(1,1)=90/180*pi;%偏航角初始值单位：弧度theta(1,1)=2/180*pi;%俯仰角初始值单位：弧度gamma(1,1)=1/180*pi;%滚转角初始值单位：弧度gk1=0.00193185138639;gk2=0.00669437999013;H=30;%高度%求解四元数系数q0,q1,q2,q3的初值q(1,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*sin(theta( 1,1)/2)*sin(gamma(1,1)/2); %q0q(2,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*cos(theta( 1,1)/2)*sin(gamma(1,1)/2); %q1q(3,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*sin(theta( 1,1)/2)*cos(gamma(1,1)/2); %q2q(4,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*cos(theta( 1,1)/2)*cos(gamma(1,1)/2); %q3for i=1:60000g=g0*(1+gk1*sin(L(i)^2)*(1-2*H/Re)/sqrt(1-gk2*sin(L(i)^2)));%计算重力加速度Rx(i)=Re/(1-e*(sin(L(i)))^2);%根据纬度计算卯酉圈曲率半径Ry(i)=Re/(1+2*e-3*e*(sin(L(i)))^2);%根据纬度计算子午圈曲率半径%求解四元数姿态矩阵q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=[q0^2+q1^2-q2^2-q3^2,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2);2*(q1*q2-q0*q3),q2^2-q3^2+q0^2-q1^2,2*(q2*q3+q0*q1);2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q3^2-q2^2-q1^2+q0^2;];Cbt=Ctb';fibt=Cbt*[fibbx(i);fibby(i);fibbz(i)];%比力数据fibtx(i)=fibt(1,1);fibty(i)=fibt(2,1);fibtz(i)=fibt(3,1);Vx(1,i+1)=(fibtx(i)+(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vy(i)-(2*Wie*cos(L(i)) +Vx(i)/Rx(i))*Vz(i))*t+Vx(i);%计算速度x方向分量Vy(1,i+1)=(fibty(i)-(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vx(i)+Vy(i)*Vz(i)/Ry(i) )*t+Vy(i);%计算速度y方向分量Vz(1,i+1)=(fibtz(i)+(2*Wie*cos(L(i)+Vx(i))/Rx(i))*Vx(i)+Vy(i)*Vy(i)/Ry(i)-g)*t+Vz (i);%计算速度z方向分量Witt=[-Vy(i)/Ry(i);Wie*cos(L(i))+Vx(i)/Rx(i);Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i)];%求出平台指令角速度值Wibb=[Wibx(i);Wiby(i);Wibz(i)];Wtbb=Wibb-Ctb*Witt;%将指令角速度转换到平台坐标系,并求解WtbbL(1,i+1)=t*Vy(i)/Ry(i)+L(i);Lambda(1,i+1)=t*Vx(i)/(Rx(i)*cos(L(i)))+ Lambda(i);x=Wtbb(1,1)*t;y=Wtbb(2,1)*t;z=Wtbb(3,1)*t; %求取迭代矩阵中的各ΔthetaA=[0 -x -y -z;x 0 z -y;y -z 0 x;z y -x 0];%求取迭代矩阵[Δtheta]T=x^2+y^2+z^2; % 计算[Δtheta]^2的q(:,i+1)=((1-T/8+T^2/384)*I+(1/2-T/48)*A)*q(:,i);%求q0theta(i+1)=asin(Ctb(2,3));if(Ctb(2,2)>=0)psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2));elseif(Ctb(2,1)>0)psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2))+pi;elsepsi(i+1)=atan(-Ctb(2,1)/Ctb(2,2))-pi;endif(Ctb(3,3)>0)gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3));elseif(Ctb(1,3)<0)gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))+pi;elsegamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))-pi;endendfigure(1)plot(L*180/pi,Lambda*180/pi);title('经纬度位置曲线');xlabel('经度/°');ylabel('纬度/°');grid ont=0:0.01:600;figure(2)plot(t,Vx);title('东西方向速度');xlabel('时间/s');ylabel('速度/m/s');grid on figure(3)plot(t,Vy);title('南北方向速度');xlabel('时间/s');ylabel('速度/m/s');grid on figure(4)plot(t,theta*180/pi);title('俯仰角');xlabel('时间/s');ylabel('度');grid on figure(5)plot(t,gamma*180/pi);title('滚转角');xlabel('时间/s');ylabel('度');grid on figure(6)plot(t,psi*180/pi);title('偏航角');xlabel('时间/s');ylabel('度');grid on。