《基于 Matlab 的捷联惯导算法设计及仿真1.doc》由会员分享，可在线阅读，更多相关《基于 Matlab 的捷联惯导算法设计及仿真1.doc（10页珍藏版）》请在三一办公上搜索。
1、基于 Matlab 的捷联惯导算法设计及仿真1严恭敏 西北工业大学航海学院，西安 （710072） E-mail：yangongmin摘要：根据圆锥误差补偿算法和划船误差补偿算法的研究成果，考虑到实际捷联惯导算法 仿真程序编写的方便性，总结了一些与捷联惯导更新算法有关的函数的计算公式。对圆锥误差补偿算法和捷联惯导算法进行了仿真，仿真结果和理论分析结论吻合。在附录中给出了Matlab 的 m 文件源程序代码，具有一定的参考价值。 关键词：捷联惯导；四元数；等效旋转矢量；Matlab；算法；仿真 中图分类号：V249.31. 引言在捷联惯导系统中采用数学平台，姿态更新解算是捷联惯导系统算法的核心部
2、分，由于 四元数法算的优良特性，它在工程实际中经常被采用。为了减小姿态计算的不可交换性误差， 前人研究并建立了等效旋转矢量方程，高精度姿态更新解算的研究主要集中在等效旋转矢量 方程的求解上，在圆锥运动环境下，许多研究者提出并完善了圆锥误差补偿算法。基于圆锥 误差补偿算法和划船误差补偿算法的等效原理，可将圆锥误差补偿算法移植到划船误差补偿 算法中去，从而减少了划船误差推导的繁琐过程。上述研究都已经比较成熟1-6，本文根据 这些研究结果，并考虑到实际仿真程序编写的方便性，总结了一些与捷联惯导算法有关的函 数的计算公式或步骤，其中更详细的推导过程可见参考文献7,8。最后，对圆锥误差补偿算 法和捷联惯
3、导算法进行了仿真。附录中给出了Matlab的m文件源程序代码具有一定的参考价 值。2. 捷联惯导算法文中选取东-北-天（E-N-U）地理坐标系为导航坐标系，记为 n 系；捷联惯组坐标系记 为 b 系。2.1 相关函数（1）四元数的共轭与乘积。四元数 q 可表示为 q = q0 + qv = q0 + q1i + q2 j + q3 k 。用 q表示 q 的共轭四元数； q = q1q2 表示四元数 q 是四元数 q1 与 q2 的乘积，四元数相乘是不可交换的。b（2）四元数与向量相乘。一般情况下利用矩阵进行坐标变换，如关系式 v n = C n v b ，同样利用四元数也可以表示坐标变换。设变
4、换四元数 q n 与变换矩阵 C n 相对应，则定义变换bb四元数 q n 和向量 v b 的乘法，记为 v n = q n v b ，它由以下规则实现：首先，将向量扩展成四bb元数，令 qb = 0 + v b ；其次，做四元数乘法，令 q n = q n qb q n ；最后，从四元数 q n 中提取bbv向量，有 v n = q n 。（3）等效旋转矢量与四元数之间的转换。等效旋转矢量 v 转化为四元数 q 的公式为1 本课题得到水下信息处理与控制国家级重点实验室基金（9140C230206070C2306）资助。q = Fvq(v) = cos( v / 2) + v sin( v /
5、 2)v(1)其中 Fvq () 表示从等效旋转矢量转换到四元数的函数，容易看出有 q= Fvq (v) 成立。假设 v 是小量，则从 q 中可以求出 v ，首先令 vn 2 = v / 2 = arccos(q0 ) ，再求得v = Fqv(q) =2 vn 2 qsin(vn 2 )(2)其中 Fqv () 表示从四元数转换到等效旋转矢量的函数。b（4）姿态向量 A 与姿态四元数 q n 之间的转换。记由俯仰角 、横滚角 和航向角 组成的向量 A = T 为姿态向量，通过姿态矩阵 C n 作为过渡，容易实现 A 与 q n 之间vbb的转换，此处不作详细分析。2.2 地球模型通常给出的地球
6、椭球模型参数为长半轴 Re 和扁率 f ，由椭球几何学容易得到其它参数，在惯性导航算法中经常用到，它们是偏心率 e =2 f f 2 、短半轴 R= (1 f )Re 、第二RppMe偏心率 ep =2 + R 2 / R 、子午圈半径 R= Re (1 e) /(1 e2sin 2L)3 / 2、卯酉圈半径RN = Re/(1 e2 sin 2 L)1/ 2 。另外，重力加速度 g 和地球自转角速率 也是惯性导航解算的必备参数，在 GRS80 椭球模型中，正常重力公式为3g = g0 (1 + 5.27094 1022sin 2 L+ 2.32718 105iepsin4 L) 3.086
7、10-56 h(3)其中 g0 = 9.7803267714m/s ，h 为海拔高度，而ie = 7.2921151467 10rad/s 。记地球自转角速度矢量在导航坐标系投影为 n ，惯导速度 v n 引起导航坐标系转动速度为 n ，则ien = 0ieiecos Liesin LTen(4a)n = v n /(R+ h)v n /(R+ h)v n tan L /(R+ h)T(4b)并且记enNMENENnnn2.3 圆锥误差与划船误差补偿算法in = ie + en(4c)假设陀螺和加速度计均为等周期采样（采样周期 h ），圆锥误差与划船误差补偿周期均为T ，且T = n h ，
8、n 为子样数。再假设在第 m 个补偿周期中，陀螺角增量和加速度计比力速度增量 采样输出分 别为 m (i) 和 vm (i) （ i = 1,2,3.n ），并令采样 总角增量 nnm = i=1 m (i) 和采样总比力速度增量 vm = i =1 vm (i) ，则考虑圆锥误差补偿后的等效旋转矢量计算公式为m = m + n1ki m (i)m (n)(5)i=1上式中第二项为圆锥误差补偿量，系数 ki 的选择见表 1。根据划船误差补偿算法与圆锥误差补偿算法的对偶原理，则考虑划船误差补偿后的比力速度增量计算公式为vsfm = vm + 1 / 2( m vm )+ i =1ki m (i)
9、 vm (n) + i =1ki vm (i) m (n)(6)上式中第二项为旋转速度增量，第三项为划船误差补偿量。表 1 圆锥误差补偿算法系数子样数 nk1k2k3k422/3-39/2027/20-454/10592/105214/105-5250/504525/504650/5041375/5042.4 捷联惯导更新算法捷联惯导更新的基本思想是，将前一时刻的导航参数（姿态、速度和位置）作为初值， 利用前一时刻至当前时刻的惯性器件采样输出，解算当前时刻的导航参数，作为下一时刻捷 联惯导解算的初值，如此反复不断。惯性器件采样经过 2.3 小节的误差补偿后获得等效旋转 矢量 m 和比力速度增量
10、 vsfm ，再经过以下三步骤便可实现捷联惯导更新，这里直接给出 计算公式。（1）速度更新算法nnnnnnnvm = vm1 + Fvq (inm1Tm / 2)qbm1 vsfm + gm1 (2iem1 + enm1 ) vm1 Tm（2）位置更新算法(7)L = L+ T v/ Rnmm1m Nm,m1M(8a) = + T sec Lv/ Rnmm1mm1 Em,m1N(8b)（3）姿态更新算法h = h+ T vnmm1m Um,m1(8c)nnn n3. 仿真与分析qbm = qbm1Fvq (m qbm1 inm1Tm )(9)根据第 2 节的分析，将各函数和算法编制成 Matl
11、ab 的 m 文件，文件名及简要功能如表2 所示，详细的源程序代码参见附录，文中还着重对圆锥误差补偿算法和捷联惯导更新算法 进行了仿真。表 2 Matlab 文件功能说明文件功能文件功能glvs.m全局变量a2quat.m由姿态向量求姿态四元数earth.m有关地球参数计算q2att.m由姿态四元数求姿态向量qconj.m qmul.m四元数共轭四元数相乘cnscl.m sins.m圆锥误差和划船误差补偿捷联惯导更新算法qmulv.m四元数乘向量test1.m圆锥误差补偿算法仿真rv2q.m等效旋转转换为四元数test2.m捷联惯导算法算法仿真q2rv.m四元数转换为等效旋转3.1 圆锥误差补
12、偿算法仿真如图 1，动坐标系 b 系相对参考坐标系 r 系绕 or z r 轴作圆锥运动，半锥角为 ，锥运动 频率为 ，即：b 系与 r 系坐标原点重合， ob xb 轴以 or xr 轴为对称中心，在 xr or z r 平面内 作幅值为 的振动； ob yb 轴以 or yr 轴为中心，在 yr or z r 平面内幅值为 的振动； ob zb 轴在以 or z r 轴为中心的圆锥面上运动。动坐标系 b 相对参考坐标系 r 的方位关系还可用姿态四元数描述为bq r (t ) = cos( / 2 )sin( / 2 ) cos tsin( / 2 ) sin t0 T(10)zrzb yr
13、 ybxror (ob)xb图 1 b 系相对 r 系作圆锥运动两个相邻采样时刻之间的角增量输出为k 2 sin sin(h / 2) sin (tk + h / 2)k +1= 2 sin sin(h / 2) cos (t+ h / 2) (11) 2(h) sin 2 ( / 2)其中，h = t k +1 t k ， k +1 为从 t k 时刻到 t k +1 时刻的角增量，即 t k +1 时刻采样获得的角增量输出。理论上可以证明，圆锥误差补偿算法在一个补偿周期T 内的算法漂移误差为n+1 =n n!n+12(2k1)k =12 (h)2 n+1(12)图 2 圆锥误差补偿仿真当
14、=1、 =5Hz、 h =10ms、 n =3时，仿真一分钟的姿态误差如图2所示。其中蓝色曲线为圆锥误差补偿姿态与真实姿态之间的误差，可见在 or xr 轴和 or yr 轴有微小振荡误差，而 or zr 轴为圆锥漂移误差。最下面小图中红色曲线为利用公式（12）计算的理论圆锥 漂移误差，它与蓝色曲线非常接近。此外，当 =1Hz、 h =10ms，而 和 n 取不同值时，进行了一系列的圆锥漂移误差仿 真和理论漂移误差计算，结果如表 3 所示。从表中可以看出，只有当半锥角 很小时，高 子样数的仿真误差和理论误差才比较接近，而当 较大时，高子样数的仿真误差和理论误差之间相差倍数很大，甚至在仿真误差中
15、出现高子样补偿效果不如低子样的现象。产生这种现象的原因是，由于在圆锥漂移补偿算法推导过程中作出了半锥角 是小角度的假设，还须指 出的是，在推导过程中还假设圆锥频率和采样周期乘积 h 必须为小量。因此，在实际中必须结合应用环境选择合适的圆锥误差补偿算法，而并非子样数越高计算精度就越高，但是，如果考虑到实际陀螺的精度（如陀螺漂移 110-4 /h），即使 为 10 时，选用 3 子样的计算精度也足够了，或者提高采样频率有利于降低计算误差。1 子样2 子样3 子样4 子样5 子样仿真误差6.013e-74.745e-104.016e-133.522e-169.558e-19理论误差6.012e-74
16、.747e-104.013e-133.523e-163.161e-19仿真误差2.164e-31.708e-61.444e-91.612e-121.623e-12理论误差2.164e-31.709e-61.445e-91.268e-121.138e-15仿真误差7.7906.148e-34.596e-64.480e-62.103e-5理论误差7.7926.152e-35.205e-64.566e-94.097e-12仿真误差771.2420.596-5.455e-34.416e-22.075e-2理论误差779.2720.6155.205e-44.566e-74.097e-10表 3 一分钟圆
17、锥漂移误差仿真和理论计算（单位：）111103.2 捷联惯导算法仿真传统上用姿态矩阵表示的计算数学平台 C n 与真实数学平台 C n 之间的关系为bbn n nnCb = C n Cb ( I )Cb(13)其中 为平台误差角，它表示从 n 系到 n 系的小转动角向量（可以看成是等效旋转矢量）。假设变换矩阵 C n 、 C n 、 C n 分别与变换四元数 q n 、 q n 、 q n 相对应，则有 q n = q n q n ，bnbbnnbn b并且利用矩阵与四元数之间的关系可以证明 q n = F() 成立，所以有nvqqvq bbn = F()q n(14a)F() = q n q
18、 n = F(q n q n )(14b)vq b bqv b b在仿真时，通过（14）式能够方便地进行平台误差角处理，与传统矩阵描述平台误差角相比， 四元数描述方法的优点是不必再作正交化处理。对静态导航进行了仿真，初始平台误差角分别设置为0.5、0.5和3，仿真一小时导航参 数结果如图3所示，其中速度误差和位置误差小图中，红色曲线为高度通道的误差，它们是 发散的，而水平通道（包括水平平台误差、水平速度和经纬度误差）呈现振荡趋势，方位平 台误差变化比较小。仿真结果符合惯导系统误差理论分析的结论。4. 小结图 3 捷联惯导算法仿真总结了一些与捷联惯导更新算法有关的函数的计算公式，对圆锥误差补偿算
19、法和捷联惯 导算法进行了仿真。在圆锥误差补偿算法中发现，只有当半锥角很小时，高子样数的仿真误 差和理论误差才比较接近，否则相差倍数很大，因此在高精度应用场合，必须通过提高采样 频率来降低计算误差。捷联惯导静态导航仿真结果与惯导系统误差理论分析结论相吻合，证 明了所设计算法的正确性。参考文献1Bortz J E, A new mathematical formulation for strapdown inertial navigationJ, IEEE Transactions onAerospace and Electronic Systems,1971,7(1):61-66.2Miller
20、 R B, A new strapdown attitude algorithmJ, Journal of Guidance, Control and Dynamics,1983,6(4):287-291.3Roscoe K M, Equivalency between strapdown inertial navigation coning and sculling integrals /algorithmsJ Journal of Guidance, Control and Dynamics，2001，24(2)：201-2054秦永元. 惯性导航M. 北京：科学出版社，2006.5张玲霞
21、，陈明，曹晓青. 基于对偶性原理捷联惯导划船误差补偿优化算法J. 中国惯性技术学报，2003,11(5):33-38.6潘献飞,吴文启,吴美平. 考虑机抖激光陀螺信号滤波特性的圆锥算法修正J. 中国惯性技术学报，2007,15(3):259-264.7严恭敏. 捷联惯导算法及车载组和导航系统研究D. 西安：西北工业大学硕士论文，2004,3.8严恭敏. 车载自主定位定向系统研究D. 西安：西北工业大学博士论文，2006,5.Algorithm design and simulation for SINS based on MatlabYan GongminCollege of Marine，N
22、orthwestern Polytechnical University，Xian （710072）AbstractBased on the research achievements of coning error compensation and sculling error compensation algorithm in strapdown inertial navigation system(SINS), and considering the convenience ofsimulation software program, some calculation formulas
23、about the SINS updating algorithm are summarized in this paper. Coning error compensation algorithm simulation and SINS algorithmsimulation are carried out and the simulation results are consistent with the theory analysis. Simulation source codes written in Matlabs m file are present in the appendi
24、x, and provide valuable references forSINS software designer.Keywords：strapdown inertial navigation system；quaternion；equivalent rotate vector；Matlab；algorithm；simulation作者简介：严恭敏（1977-），男，福建建瓯人，2006 年 9 月毕业于西北工业大学自动化学 院，获导航、制导与控制专业博士学位，现在西北工业大学航海学院兵器科学与技术博士后 流动站工作，主要从事陆用定位定向系统、水下航行器自主导航技术与组合导航系统理论研
25、究。附录Matlab仿真程序% glvs.m 全局变量%全局变量global glvglv.Re = 6378160;%地球半径glv.f = 1/298.3;%地球扁率glv.e = sqrt(2*glv.f-glv.f2); glv.e2 = glv.e2; %地球椭圆度等其它几何参数glv.Rp = (1-glv.f)*glv.Re;glv.ep = sqrt(glv.Re2+glv.Rp2)/glv.Rp; glv.ep2 = glv.ep2;glv.wie = 7.2921151467e-5;%地球自转角速率glv.g0 = 9.7803267714;%重力加速度 glv.mg =
26、1.0e-3*glv.g0; %毫重力加速度 glv.ug = 1.0e-6*glv.g0;%微重力加速度 glv.ppm = 1.0e-6;%百万分之一 glv.deg = pi/180; %角度glv.min = glv.deg/60; %角分 glv.sec = glv.min/60; %角秒 glv.hur = 3600;%小时 glv.dph = glv.deg/glv.hur;%度/小时 glv.mil = 2*pi/6000; %密位 glv.nm = 1853; %海里 glv.kn = glv.nm/glv.hur; %节2./3,0,0,09./20,27./20,0,05
27、4./105,92./105,214./105,0250./504,525./504,650./504,1375./504 ;glv.cs = %圆锥和划船误差补偿系数% earth.m 有关地球参数计算function wnie, wnen, RMh, RNh, gn = earth(pos, vn)global glvsl=sin(pos(1); cl=cos(pos(1); tl=sl/cl; sl2=sl*sl; sl4=sl2*sl2;wnie = glv.wie*0; cl; sl;sq = 1-glv.e2*sl2; sq2 = sqrt(sq);RMh = glv.Re*(1-
28、glv.e2)/sq/sq2+pos(3); RNh = glv.Re/sq2+pos(3);wnen = -vn(2)/RMh; vn(1)/RNh; vn(1)/RNh*tl;g = glv.g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80 gn = 0;0;-g;% qconj.m 四元数共轭function qo = qconj(qi)qo = qi(1); -qi(2:4);% qmul.m 四元数相乘function q = qmul(q1,q2)q = q1(1) * q2(1) - q1(2) * q2(
29、2) - q1(3) * q2(3) - q1(4) * q2(4); q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3); q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4); q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ;% qmulv.m 四元数乘向量function vo = qmulv(q, vi)qi = 0;vi;qo = qmul(qmul(q,qi),qc
30、onj(q);vo = qo(2:4,1);% rv2q.m 等效旋转转换为四元数function q = rv2q(rv) norm = sqrt(rv*rv); if norm1.e-20f = sin(norm/2)/(norm/2);elseendf = 1;q = cos(norm/2); f/2*rv;% q2rv.m 四元数转换为等效旋转function v = q2rv(q) n2 = acos(q(1); if n21e-20k = 2*n2/sin(n2);elseendk = 2;v = k*q(2:4);% a2quat.m 由姿态向量求姿态四元数function q
31、= a2quat(att)% 先求姿态矩阵si = sin(att(1); ci = cos(att(1); sj = sin(att(2); cj = cos(att(2);sk = sin(att(3); ck = cos(att(3);M = cj*ck+si*sj*sk, ci*sk, sj*ck-si*cj*sk;-cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck;-ci*sj,si,ci*cj ;% 再求四元数q = sqrt(abs(1.0 + M(1,1) + M(2,2) + M(3,3)/2.0; sign(M(3,2)-M(2,3) * sqr
32、t(abs(1.0 + M(1,1) - M(2,2) - M(3,3)/2.0; sign(M(1,3)-M(3,1) * sqrt(abs(1.0 - M(1,1) + M(2,2) - M(3,3)/2.0; sign(M(2,1)-M(1,2) * sqrt(abs(1.0 - M(1,1) - M(2,2) + M(3,3)/2.0 ;% q2att.m 由姿态四元数求姿态向量function att = q2att(qnb)% 先求姿态矩阵q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb
33、(1)*qnb(4);q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);q44 = qnb(4)*qnb(4);M = q11+q22-q33-q44, 2*(q23-q14),2*(q24+q13);2*(q23+q14),q11-q22+q33-q44, 2*(q34-q12);2*(q24-q13),2*(q34+q12),q11-q22-q33+q44 ;% 再求姿态att = asin(M(3,2); atan(-M(3,1)/M
34、(3,3); atan(M(1,2)/M(2,2);if M(3,3) 0if(att(2) 0)if(att(3) 0)att(3) = att(3)+pi*2;endelseatt(3) = att(3)+pi;end% cnscl.m 圆锥误差和划船误差补偿function phim, dvbm = cnscl(wm, vm)global glvn = size(wm,2);cm = 0;0;0; sm = 0;0;0;for k=1:n-1cm = cm + glv.cs(n-1,k)*wm(:,k); %准备圆锥误差补偿endwmm = sum(wm,2);phim = wmm +
35、cross(cm,wm(:,n);if nargin=2for k=1:n-1sm = sm + glv.cs(n-1,k)*vm(:,k); %准备划船误差补偿endendvmm = sum(vm,2);dvbm = vmm + 1.0/2*cross(wmm,vmm) + (cross(cm,vm(:,n)+cross(sm,wm(:,n);% sins.m 捷联惯导更新function qnb, vn, pos = sins(qnb_1, vn_1, pos_1, wm, vm, ts)tss = ts*size(wm,2); phim,dvbm = cnscl(wm,vm); wnie
36、,wnen,rmh,rnh,gn = earth(pos_1,vn_1); wnin = wnie+wnen;vn = vn_1 + qmulv(rv2q(-wnin*(1.0/2*tss),qmulv(qnb_1,dvbm) + (gn-cross(wnie+wnin,vn_1)*tss;pos = pos_1 + tss*vn_1(2)/rmh;vn_1(1)/(rnh*cos(pos_1(1);vn_1(3);qnb = qmul(qnb_1, rv2q(phim - qmulv(qconj(qnb_1),wnin*tss);% test1.m 圆锥误差补偿仿真glvsclear err
37、 dthetaa = 1*glv.deg; %半锥角w = 2*pi*5;%锥运动频率h = 0.01;%采样时间n = 3;%子样数len = fix(1*60/h); %仿真时间长度t = 0;%初始姿态四元数q = cos(a/2); sin(a/2)*cos(w*t); sin(a/2)*sin(w*t); 0; q1 = q;for k=n:n:len% 姿态真值t = k*h;q = cos(a/2); sin(a/2)*cos(w*t); sin(a/2)*sin(w*t); 0;% 锥运动解算值for m=1:nt = (k-n+m-1)*h;dtheta(:,m) = -2*
38、sin(a)*sin(w*h/2)*sin(w*(t+h/2);2*sin(a)*sin(w*h/2)*cos(w*(t+h/2);-2*w*h*(sin(a/2)2 ; %角增量endendphi = cnscl(dtheta);%圆锥误差补偿q1 = qmul(q1,rv2q(phi);%姿态更新err(k/n,:) = -q2rv(qmul(q1,qconj(q); %求姿态误差figuresubplot(3,1,1), plot(err(:,1)/glv.sec), ylabel(itphi_xrm / arcsec); subplot(3,1,2), plot(err(:,2)/gl
39、v.sec), ylabel(itphi_yrm / arcsec); subplot(3,1,3), plot(err(:,3)/glv.sec), ylabel(itphi_zrm / arcsec); str = sprintf(ittrm / times%.0fms, n*h*1000); xlabel(str);% 理论圆锥误差k2 = 1;for k=1:n+1k2 = k2*(2*k-1);endepsilon = a2*(w*h)(2*n+1) * n*factorial(n) / (2(n+1)*k2);hold on, plot(0,length(err),0,epsilo
40、n*length(err)/glv.sec,r-)% test2.m 捷联惯导更新仿真glvsclear errphi errvn errpos ts=0.1;%采样周期att0=0;0;0*glv.deg; vn0=0;0;0; pos0=0*glv.deg;0;0;%初始值设置qnb0 = a2quat(att0);vn = vn0; pos = pos0;wnie = glv.wie*0; cos(pos0(1); sin(pos0(1);g = glv.g0*(1+5.27094e-3*sin(pos0(1)2+2.32718e-5*sin(pos0(1)4)-3.086e-6*pos
41、0(3);wbib = qmulv(qconj(qnb0),wnie);fb = qmulv(qconj(qnb0),0;0;g);wm=wbib*ts; vm=fb*ts;%静止时角增量和比力增量qnb = qmul(rv2q(-0.5;0.5;3*glv.min),qnb0); %加入姿态误差kk = 1;for k=1:3600*10 qnb,vn,pos=sins(qnb,vn,pos,wm,vm,ts); if mod(k,10)=0errphi(kk,:) = -q2rv(qmul(qnb,qconj(qnb0); %求姿态误差errvn(kk,:) = (vn-vn0);errpos(kk,:) = (pos-pos0);kk = kk+1;endendfigure,subplot(3,1,1),plot(errphi/glv.min), ylabel(itphirm / arcmin); subplot(3,1,2),plot(errvn), ylabel(itdelta Vrm / m/s); subplot(3,1,3),plot(errpos(:,1:2)*glv.Re,errpos(:,3),
