飞机惯性导航Matlab语言实现
- 格式:docx
- 大小:13.46 KB
- 文档页数:4
飞行控制系统的侧向控制matlab仿真介绍飞行控制系统在飞行器中起着至关重要的作用,它负责控制和调整飞行器的姿态和运动。
其中,侧向控制是飞行控制系统的一个重要部分,它可以影响飞行器的侧向动态特性和机动性能。
本文将介绍如何使用MATLAB进行侧向控制系统的仿真,并详细探讨该任务的内容和相关实现方法。
侧向控制系统的组成控制框图侧向控制系统通常由以下组成部分构成: 1. 输入信号:包括飞行器的姿态、角速度等信息; 2. 传感器:负责采集飞行器的状态信息,如加速度、陀螺仪等; 3. 控制器:根据输入信号和传感器信息,生成控制指令; 4. 执行器:根据控制指令,调整飞行器的姿态和运动。
详细说明在侧向控制系统中,控制器起着至关重要的作用。
它通过对输入信号和传感器信息进行处理和分析,生成相应的控制指令,以调整飞行器的侧向运动。
具体而言,控制器通常包括以下几个模块: 1. 姿态控制:用于控制飞行器的姿态,如滚转、俯仰和偏航; 2. 舵面控制:用于控制飞行器的舵面,如副翼和方向舵;3. 纵向和横向耦合控制:用于处理飞行器纵向和横向耦合特性,以提高侧向控制系统的性能; 4. 鲁棒控制:用于提高侧向控制系统的稳定性和鲁棒性。
MATLAB仿真实现建模在进行侧向控制系统的仿真前,首先需要对飞行器进行建模。
建模过程中需要考虑飞行器的动力学特性以及控制器的设计要求。
动力学模型飞行器的动力学模型可以使用欧拉法、四元数等表示。
在侧向控制中,常用的是欧拉法建模。
例如,对于二维飞行器,其动力学方程可以表示为:m * x'' = -g * sin(theta) - D * x'm * y'' = g * cos(theta) - D * y'I * theta' = M其中,m表示飞行器的质量,x和y分别表示飞行器在水平和垂直方向的位移,theta表示飞行器的俯仰角,g表示重力加速度,D表示阻尼系数,I表示飞行器的惯性矩,M表示扭矩。
惯性导航系统算法优化与开发第一章:引言随着科技的不断发展,惯性导航系统在航空、航海、导弹等领域得到了广泛的应用。
惯性导航系统的基础是惯性传感器,通过测量加速度和角速度来计算位移和方向。
在惯性导航系统的研究中,算法的优化和开发是非常重要的环节。
本文将从惯性导航系统算法的优化和开发两个方面进行探讨,进一步提高系统的精度和性能。
第二章:惯性导航系统算法优化2.1优化方向选择惯性导航系统中最常用的算法是卡尔曼滤波器。
但是,卡尔曼滤波器不适用于某些应用场景,例如高加速度和高速运动、倾斜、震动、强磁场等。
针对这些问题,我们可以选择其他算法,例如扩展卡尔曼滤波、粒子滤波、无迹卡尔曼滤波、模型预测控制等。
选择合适的算法可以更好地解决问题,提高系统的稳定性和精确性。
2.2信号处理在测量加速度和角速度时,往往会有噪声信号干扰,这会影响导航系统的精度。
因此,我们需要对信号进行处理,例如高通滤波、低通滤波、数字滤波等。
此外,为了更好地处理信号,我们还需要对采样率、预测步长、状态转移矩阵等参数进行优化。
2.3状态估计和预测在惯性导航系统中,状态估计和预测是最为关键的环节。
状态估计是指根据测量数据和系统模型,计算当前所处状态的过程;预测是指利用估计出的状态值,根据系统模型来预测下一个状态值的过程。
为了提高状态估计和预测的精确性,我们需要对系统模型进行优化,确定合适的状态变量和测量变量,并且需要注意时间延迟、非线性问题等。
第三章:惯性导航系统算法开发3.1软件开发环境惯性导航系统算法开发需要使用工程仿真软件和算法开发软件。
常用的工程仿真软件包括MATLAB、Simulink、LabVIEW等;常用的算法开发软件包括Keil、IAR、Code Composer Studio等。
3.2算法实现在惯性导航系统算法开发中,我们需要实现各种算法,包括测量模型、状态转移模型、卡尔曼滤波、无迹卡尔曼滤波、扩展卡尔曼滤波、粒子滤波等。
此外,还需要实现数据采集、预处理、处理、显示等功能,以便观察算法的运行效果。
基于MATLAB的翼型的升力系数和气动中心计算作者:邓鹏张斌来源:《科技视界》2020年第07期摘要本文主要通过用Matlab建立翼型的升力系数和气动中心计算程序,并通过与Profili翼型数据库和已有实验翼型数据进行分析对比,验证本程序计算的可靠性和准确性。
之后,通过使用该程序预测国内某公司新研发的无人直升机SUV翼型的升力系数和压力中心位置。
关键词升力系数;气动中心;MATLAB中图分类号: ;TP273.3 ; ; ; ; ; ; ; ; ; ; ;文献标识码: ADOI:10.19694/ki.issn2095-2457.2020.07.0731 Matlab程序计算与23012翼型进行对比分析1.1 profili翼型数据库中23012升力系数和压力中心位置在profili数据库中查找到翼型23012,并输入计算条件弦长17cm,雷诺数1150000和1600000分别对应桨尖附近和0.7倍半径处的数据,计算迎角变化从Alfa 0至10°的结果,部分数据见表一。
另外通过有关文献可以查找到23012压力中心位置x_cp= 0.24[1]。
1.2 Matlab程序计算首先由profili数据库生成23012翼型坐标,以dat文件格式输出,命名为23012.dat;在Matlab程序中手动输入23012.dat文件路径;Matlab程序读取文件后,将计算对称中心曲线即翼型弦线并显示,见图1。
之后选择多项式次数由程序生成拟合曲线,最后计算0-10°迎角下的升力系数,见图2。
其他计算结果如下:升力为0时的攻角:a_0 = -1.1139°四分之一弦长处的扭矩系数:cm_quater = -0.008617翼型压力中心坐标:x_cp = 0.257071.3 结果分析通过对比图2和表1数据,可以得出以下结论:(1)Matlab程序计算结果与Profili数据库在大雷诺数Re=1600000的数据比较接近,在5°前的升力系数偏小5%,5°后计算结果接近。
基于三参数威布尔可靠性模型的航空装备故障预测研究作者:孙扩赵波杨航来源:《航空维修与工程》2020年第10期摘要:针对目前我军装备保障任务维修中存在的设备故障率高和过度检修等问题,以三参数威布尔为基础,基于某飞机历年的故障记录数据,针对该型飞机的几个重要设备开展可靠度建模,并在故障預测中加以验证,与历年同时段同架次飞机故障数据进行对比,验证了该模型和预测方法具有可行性。
关键词:三参数威布尔;可靠度建模;航空设备;故障预测Keywords:three parameters Weibull;reliability modeling;aviation equipment;failure prediction0 引言航空装备维修保障主要有三类:事后维修、定期维修和视情维修[1]。
目前,我军装备保障任务维修基本采用的是事后维修与定期维修相结合的方式,该方式虽然有着简单、可操作性强的特点,但是存在设备故障率高和过度检修的风险和挑战。
随着设备状态监控技术的兴起,视情维修方式必定成为今后很长一段时间内装备维修保障的发展方向。
为此,针对某型飞机的重要电子设备,利用三参数威布尔构建可靠性模型,对设备进行故障预测和实验验证,为该设备的视情维修提供分析依据。
1威布尔分布威布尔分布[2]因瑞典教授Wallodi Weibull在进行材料强度统计时首次使用而得名,目前已广泛应用于电子元器件寿命试验和机械产品的疲劳寿命试验中。
两参数威布尔分布的参数估计简单、适应能力较强,但是某些机电设备特别是航空机电设备,其威布尔概率并非均匀分布,这种情况下三参数威布尔模型更能够描述复杂机电类产品磨损累计失效的分布形式。
因此,本文将三参数威布尔模型作为飞机设备可靠性建模的核心模型。
三参数威布尔分布的概率分布函数为:三参数威布尔分布的失效率函数为:2 威布尔模型可靠度建模与故障预测针对航空设备故障开展可靠度建模和故障预测[3]的基本流程如图1所示。
飞行品质计算 matlab一、概述飞行品质是指航空器在飞行中的性能表现,包括稳定性、操纵性、性能等方面。
在飞行器设计和飞行控制系统设计中,飞行品质计算是一个非常重要的环节。
Matlab作为一种强大的计算工具,可以对航空器的飞行品质进行精确的计算和分析。
二、飞行品质计算的基本概念1. 飞行品质指标飞行品质指标是对飞行器在飞行过程中性能的评价指标,常用的飞行品质指标包括方向稳定性、纵向稳定性、横侧稳定性、操纵品质等。
这些指标通常由飞行动力学和控制理论中的参数来描述。
2. 飞行品质计算的方法飞行品质计算的方法主要包括理论计算、仿真计算和实验测量。
在Matlab中,可以通过建立飞行动力学模型和控制系统模型,利用仿真技术进行飞行品质的计算和分析。
三、飞行品质计算的Matlab工具1. 飞行动力学模型的建立在Matlab中,可以利用飞行动力学方程和控制系统方程建立飞行器的动力学模型。
这些方程可以描述飞行器在飞行过程中的运动状态和控制过程,通过仿真计算可以得到飞行器的稳定性和操纵性等性能指标。
2. 飞行品质指标的计算利用Matlab可以对飞行动力学模型进行仿真计算,得到飞行品质指标。
通过对系统的响应进行分析,可以得到飞行器的方向稳定性、纵向稳定性、横侧稳定性和操纵品质等指标。
3. 飞行器控制系统设计飞行品质计算还可以用于飞行器的控制系统设计。
利用Matlab可以对飞行器的控制系统进行仿真分析,得到系统的稳定性和性能指标,为控制系统的设计提供参考。
四、飞行品质计算的实际应用1. 飞行器设计优化利用Matlab进行飞行品质计算,可以对飞行器的设计进行优化。
通过分析不同设计参数对飞行品质指标的影响,可以得到最优的设计方案。
2. 飞行器控制系统设计在飞行控制系统设计中,飞行品质计算可以用于系统的性能分析和参数优化。
通过仿真计算得到系统的性能指标,为控制系统的设计提供参考依据。
3. 飞行器飞行参数预测利用Matlab进行飞行品质计算,可以对飞行器在不同飞行条件下的性能进行预测。
A320飞机惯性导航系统校准分析与维护e=U y / (K y -g ) (2)利用横向加速度计测星飞机的倾斜角与上同理,飞机停在地面上,倾斜角为Y、俯仰角为0时,重力加速度g沿飞机横轴的分厳g siny 被横向加速度计敏感到,输出伯号Ux则U x =K x -g -siny (K x为横向加速度计比例系数)。
当倾斜角Y很小时,sin仟Y(Y单位为弧度)。
Y=U x / (K x ・g)当飞机既有俯仰又有倾斜时,用上述公式测得的俯仰角为飞机真实俯仰角,测得的倾斜角为飞机非真实倾斜角(真实俯仰角为飞机纵轴与水平面之间的夹角,非真实倾斜角为飞机横轴与水平面之间的夹角)•30秒后.飞机的俯仰角和倾斜角被计算出来.正、副驾驶位的PFD上姿态旗消失,飞机符号及空地球出现,俯仰、倾斜刻度及指示被显示。
2. 陀螺•罗盘(或方位角)处理及水平精校准此阶段至少需要9分30秒,主要用于测量飞机的真航向角,并使用地球自转角速度的垂直分量计算出飞机所在处的纬度。
(1)克航向角的测定假定飞机停在地面上,俯仰角.倾斜角均为0,真航向角为屮,飞机所在处纬度为由于飞机停在地面上,随地球一起自转,其自转角速度等于地球自转角速度3e (we为佔度 /小时),cue在飞机所在处水平面上的水平分S w e cosO 在当地地垂线上的垂直分最为3 e・sin0)。
水平分量3 e cosO又可以分解为沿飞机纵轴的分塑3 e cos(P cos^P和沿飞机横轴的分量3 e cos0)sin屮垓这二个分量分别被纵向陀螺及横向陀螺所敏感,输出信号V y和V x则V y = L y ・3 y = L y ・3 e •cosO cos屮V X =L X・3X =L X・3e -cosOsin 屮(Ly为纵向陀螺比例系数.L x为横向陀螺比例系数)。
V x / V y = L x / L y ・tg屮屮—arctg[(Vx -L y )/(V y ・L x)](2)飞机所在处纬度的测定由上可知,垂宜分*u)e sin<P可被垂宜陀螺敏感到,输出信号VZ则VZ = LZ -u> e sinO (L Z为垂直陀螺比例系数)<t>c a 1 =arcsin[V Z /(L Z ・ e )]考虑到飞机停放时,e. Y不一定为牢,故上述所得w、OC a 1为近似值。
MATLAB拟合多孔介质参数,fluent计算惯性阻力系数和粘性阻力系数包涵:1,MATLAB调取NIST refprop 物性参数;2,MATLAB拟合多孔介质实验参数(二次函数,参数项目为0)3,按fluent计算公式计算惯性阻力系数和粘性阻力系数步骤:一、根据需要安装refprop 物性参数和MATLAB调用连接(物性参数也可手动输入不用安装refprop)1)下载refprop并安装,2) 下载如下4个文件到新建的MATLAB程序保持目录下:二、建立实验参数数据表excl文件。
实验名称与matlab调用名称一致,实验数据自己根据实验数据填写三、编写MATLAB程序结果显示:调取的空气物性拟合的二次函数值:Fluent软件多孔介质中惯性阻力系数C2和粘性阻力系数DFluent软件中输入程序:filename1 = 'D:\porous\¶à¿×½éÖÊʵÑé²ÎÊý.xlsx'[F1]=importdata(filename1) %Êý¾Ýµ¼Èë¾ØÕóÖÐ%V=F1.data(:,1); %¶ÁÈ¡ËÙ¶Èm/s%P=F1.data(:,2); %¶Áȡѹ½µPa%L=F1.data(1,3);%¶ÁÈ¡¶à¿×½éÖʳ¤¶È%syms xf=fittype('a*x*x+b*x','independent','x','coefficients',{'a','b'}); cfun=fit(V,P,f) %ÏÔʾÄâºÏº¯Êý£¬Êý¾Ý±ØÐëΪÁÐÏòÁ¿ÐÎʽxi=0:1:40;yi=cfun(xi)';N=polyfit(xi,yi,2)figureplot(V,P,'r*',xi,yi,'b-'); % ÄâºÏÇúÏߺÍʵÑé²ÎÊýµãtitle('ÄâºÏº¯ÊýͼÐÎ');% figure% plot(x,sqrt(y-yi(1:1:18,:).^2/18),'r*');% title('±ê×¼Îó²îͼ');[rho,u,Cp,lamda]=refpropm('DVCL','T',273.15+20,'P',101.15,'air.mix') %µ÷È¡NISTÈí¼þ refprop²ÄÁÏÎïÐÔ%20¡æ£¬±ê¿öϲÄÁÏÎïÐÔ% rho=input('ÊäÈëÃܶÈkg/m3:')% N=[3.307,70.08]% L=input('ÊäÈë¶à¿×ÇøÓò³¤¶Èm:')% u=input('ÊäÈ붯Á¦Õ³¶ÈϵÊýPa*s:')C2=N(1)/(rho*L/2) %¹ßÐÔ×èÁ¦ÏµÊýC2%%%C2=solve('C2*rho*L/2-N(1)=0') %¹ßÐÔ×èÁ¦ÏµÊýC2%D=N(2)/u/L %Õ³ÐÔ×èÁ¦ÏµÊý Viscous resistance coefficient %%D=solve('D*u*L-N(2)=0') %Õ³ÐÔ×èÁ¦ÏµÊý。
捷联惯导系统的算法研究及其仿真实现Study and Simulation of Strapdown Inertial Navigation System1.1.3捷联惯导系统的发展趋势捷联式惯导系统是从20世纪60年代初开始发展起来的。
20世纪70年代以来,作为捷联系统的核心部件—惯性测量装置和计算机技术有了很大发展,而电子技术、计算机技术、现代控制理论的不断进步,为捷联惯性技术的发展创造了有利条件。
在硬件方面,新一代惯性器件如激光陀螺、光纤陀螺的成功研制,为捷联惯导的飞速发展打下了物质基础。
进入20世纪80-90年代,在航天飞机、宇宙飞船、卫星等民用领域及各种战略、战术导弹、军用飞机、反潜武器、作战舰艇等军事领域开始采用动力调谐式陀螺、激光陀螺和光纤式陀螺的捷联惯导系统。
其中激光陀螺和光纤式陀螺是捷联惯导系统的理想器件。
激光陀螺具有角速率动态范围宽、对加速度和震动不敏感、不需温控、启动时间特别短和可靠性高等优点。
激光陀螺惯导系统己在波音757/767、A310民机以及F-20战斗机上试用,精度达到 1.85km/h 的量级。
20世纪90年代,激光陀螺惯导系统估计占到全部惯导系统的一半以上,其价格与普通惯导系统差不多,但由于增加了平均故障间隔时间,其寿命期费用只有普通惯导系统的15%-20%。
光纤陀螺实际上是激光陀螺中的一种,其原理与环型激光陀螺相同,它克服了由激光陀螺闭锁带来的负效应,具有检测灵敏度和分辨率极高、启动时间极短、动态范围极宽、结构简单、零部件少体积小、造价低、可靠性高等优点。
采用光纤陀螺的捷联航姿系统已用于战斗机的机载武器系统及波音777飞机中。
波音777由于采用了光纤陀螺的捷联惯导系统,其平均故障间隔时间可高达20000h。
采用光纤陀螺的捷联惯导系统被认为是一种极有发展前途的导航系统。
而随着航空航天技术的发展及新型惯性器件关键技术的陆续突破,捷联惯导系统的可靠性、精度将会更高。
使用Matlab进行标定与定位的技巧引言:随着计算机技术的不断进步,标定与定位在现代科学研究与工程应用中变得越来越重要。
而Matlab作为一种广泛应用于科学计算的工具,被广泛应用于标定与定位的研究与开发中。
本文将介绍使用Matlab进行标定与定位的技巧,包括标定理论和方法、定位算法与模型等。
一、标定理论与方法1.1 相机标定相机标定是进行摄像机内外参数确定和畸变纠正的过程。
在Matlab中,可以使用Camera Calibration Toolbox进行相机标定操作。
首先,需要准备一些用于标定的图像,这些图像中应包含已知参数(例如标定板大小和格点数)的标定板。
然后,在Matlab中加载图像数据,使用标定板图像来标定相机并求解相机内外参数。
1.2 IMU标定惯性测量单元(IMU)通常包括加速度计和陀螺仪等多种传感器。
IMU标定的目的是确定IMU的误差模型,以便在后续的定位中进行误差补偿。
在Matlab中,可以使用传感器标定和估计工具箱进行IMU标定操作。
首先,需要设计一套标定实验,包括旋转和加速度等多个运动过程。
然后,使用这些实验数据来标定IMU的误差模型。
二、定位算法与模型2.1 基于测距的定位基于测距的定位是通过测量到达定位节点的信号传播时间或信号强度等信息来实现的。
在Matlab中,可以使用距离测量数据进行多边定位或三边定位。
多边定位是通过测量到多个定位节点的距离信息来确定目标位置,可以使用最小二乘法等进行求解。
三边定位是通过测量到三个定位节点的距离信息来确定目标位置,可以使用三角测量法进行求解。
2.2 基于惯性导航的定位惯性导航是利用IMU等传感器测量物体的加速度和角速度等信息进行定位和导航的方法。
在Matlab中,可以使用十字光束法进行惯性导航定位。
首先,需要根据IMU数据求解出物体的位置、速度和姿态等信息。
然后,通过十字光束法计算出相对定位误差,从而实现精确定位。
2.3 基于地标的定位基于地标的定位是通过识别已知地标进行定位的方法。
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理;②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理;③掌握捷联惯导 /GPS组合导航系统静态性能;④掌握动态情况下捷联惯导 /GPS组合导航系统的性能。
二、实验内容①复习卡尔曼滤波的基本原理(参考《卡尔曼滤波与组合导航原理》第二、五章);②复习捷联惯导/GPS组合导航系统的基本工作原理(参考以光衢编著的《惯性导航原理》第七章);三、实验系统组成①捷联惯导/GPS组合导航实验系统一套;②监控计算机一台。
③差分GPS接收机一套;④实验车一辆;⑤车载大理石平台;⑥车载电源系统。
四、实验内容1)实验准备①将IMU紧固在车载大理石减振平台上,确认IMU的安装基准面紧靠实验平台;② 将IMU 与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS 接收机与导航计算机、GPS 天线与GPS 接收机、GPS 接收机与GPS 电池之间的连接线正确连接;③ 打开GPS 接收机电源,确认可以接收到4颗以上卫星; ④ 打开电源,启动实验系统。
2) 捷联惯导/GPS 组合导航实验① 进入捷联惯导初始对准状态,记录IMU 的原始输出,注意5分钟内严禁移动实验车和IMU ;② 实验系统经过5分钟初始对准之后,进入导航状态; ③ 移动实验车,按设计实验路线行驶;④ 利用监控计算机中的导航软件进行导航解算,并显示导航结果。
五、 实验结果及分析(一) 理论推导捷联惯导短时段(1分钟)位置误差,并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导:短时段内(t<5min ),忽略地球自转0ie ω=,运动轨迹近似为平面1/0R =,此时的位置误差分析可简化为:(1) 加速度计零偏∇引起的位置误差:210.88022t x δ∇==m (2) 失准角0φ引起的误差:202 0.92182g t x φδ==m (3) 陀螺漂移ε引起的误差:330.01376g t x εδ==m 可得1min 后的位置误差值123 1.8157m x x x x δδδδ=++= 2、一分钟惯导实验数据验证结果:(1)纯惯导解算1min 的位置及位置误差图:lat0.01s 度lon0.01s度北向位移误差0.01sm 东向位移误差0.01sm(2)纯惯导解算1min 的速度及速度误差图:-100-50050Vx0.01s m /s020406080Vy0.01sm /s100020003000400050006000-0.4-0.3-0.2-0.10Vx 误差0.01s m /s100020003000400050006000-0.1-0.0500.050.1Vy 误差0.01sm /s实验结果分析:纯惯导解算短时间内精度很高,1min 的惯导解算的北向最大位移误差-2.668m ,东向最大位移误差-8.231m ,可见实验数据所得位置误差与理论推导的位置误差在同一数量级,结果不完全相同是因为理论推导时做了大量简化,而且实验时视GPS 为真实值也会带来误差;另外,可见1min 内纯惯导解算的东向速度最大误差-0.2754m/s ,北向速度最大误差-0.08027m/s 。
%这是研究惯性导航的最好代码。
记得自己添加测试数据% 此为基于四元素法,角增量法的捷连惯导系统程序算法% > 飞行器飞行过程中飞行高度不变% > 航向角以逆时针为正% > 以地理系为导航坐标系% > 运行程序时需导入比力信息及陀螺议角速率信息clcclearclose allData = load('Data1.txt');f_INS = Data(:,2:4);% 加载加表数据wib_INS = Data(:,5:7);% 加载陀螺数据L0 = size(Data,1);Wie = 7.292115147e-5; %> 地球自传角速度Re = 6378245; %> 地球椭球长半径h = 30;% > 飞行高度e = 1/298.3;%> 初始经纬度Lamda(1) = 116.344695283*pi/180;% > 初始经度(弧度)L(1) = 39.975172*pi/180;% > 初始纬度(弧度)%> 初始姿态角Seita(1) = 0.120992605*pi/180; %> 俯仰角(弧度)Gama(1) = 0.010445947*pi/180; %> 横滚角(弧度)Ksai(1) = 91.637207*pi/180;% > 航向角(弧度)%> 初始速度Vx(1) = 0.000048637; %> x通道速度Vy(1) = 0.000206947;% > y通道速度Vz(1) = 0.007106781; %> z通道速度%> 重力加速度计算参数g0 = 9.7803267714;gk1 = 0.00193185138639;gk2 = 0.00669437999013;Vx = zeros(1,L0);Vy = zeros(1,L0);Vz = zeros(1,L0);Lamda = zeros(1,L0);L = zeros(1,L0);Seita = zeros(1,L0);Gama = zeros(1,L0);Ksai = zeros(1,L0); %> 四元素初始值e0 = cos(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(i0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5* Gama(1));e1 = -cos(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5* Gama(1));e2 = -cos(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5* Gama(1));e3 = cos(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5* Gama(1));Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2); %> 用四元素表示得姿态矩阵2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];E = [e0 e1 e2 e3]';%> 四元素的四个元素值for i = 1:L0f_INSc = f_INS(i,:)';wib_INSc = wib_INS(i,:)';Ry(i) = Re*(1-2*e+3*e*(sin(L(i)))^2);% > 计算子午圈主曲率半径Rx(i) = Re*(1+e*(sin(L(i)))^2); %> 计算卯酉圈主曲率半径g = g0*(1+gk1*(sin(L(i)))^2)*(1-2*h/Re)/sqrt(1-gk2*(sin(L(i)))^2);% > 重力加速度计算Cbt = Ctb';f_t = Cbt*f_INSc;% > 将体轴系中的比例转化到地理系Vx(i) = (f_t(1)+2*Wie*sin(L(i))*Vy(i)+Vx(i)*Vy(i)*tan(L(i))/Rx(i))/80+Vx(i);% > x通道速度计算Vy(i) = (f_t(2)-2*Wie*sin(L(i))*Vx(i)-Vx(i)*Vx(i)*tan(L(i))/Rx(i))/80+Vy(i);% > y通道速度计算Vz(i) = (f_t(3)+2*Wie*cos(L(i))*Vx(i)+Vx(i)*Vx(i)/Rx(i)+Vy(i)*Vy(i)/Ry(i)-g)/80+Vz(i);% > z通道速度计算Lamda(i) = Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);% > 经度计算if Lamda(i)>piLamda(i) = Lamda(i)-2*pi;% >经度在-180度(西经)到180(东经)范围endL(i) = Vy(i)/Ry(i)/80+L(i); %> 纬度计算if L(i)>(pi/2)L(i) = pi-L(i);% >纬度小于90度(北纬)endWetx_t(i) = -Vy(i)/Ry(i);Wety_t(i) = Vx(i)/Rx(i);Wetz_t(i) = Vx(i)*tan(L(i))/Rx(i);% > 在地理坐标系的位移角速率Wet_t = [Wetx_t(i) Wety_t(i) Wetz_t(i)]'; %> 在地理坐标系的位移角速率Wib_b = [wib_INSc(1) wib_INSc(2) wib_INSc(3)]';% > 陀螺仪测的角速率值Wie_t = [0 Wie*cos(L(i)) Wie*sin(L(i))]'; %> 在地理坐标系的地球角速率Wtb_b = Wib_b-Ctb*(Wie_t+Wet_t); %> 姿态矩阵角速率%> 用角增量法计算四元素姿态矩阵Mwtb = [0 -Wtb_b(1) -Wtb_b(2) -Wtb_b(3);Wtb_b(1) 0 Wtb_b(3) -Wtb_b(2);Wtb_b(2) -Wtb_b(3) 0 Wtb_b(1);Wtb_b(3) Wtb_b(2) -Wtb_b(1) 0]/80;derta = sqrt((Mwtb(1,2))^2+(Mwtb(1,3))^2+(Mwtb(1,4))^2);E = [eye(4)*(1-derta^2/8+derta^4/384)+(1/2-derta^2/48)*Mwtb]*E;% > E = (cos(0.5*derta)*eye(4)+Mwtb*sin(0.5*derta)/derta)*E,采用四阶近似算法e0 = E(1);e1 = E(2);e2 = E(3);e3 = E(4);Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);% > 用四元素表示得姿态矩阵2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];%> 姿态角计算Seita(i) = asin(Ctb(2,3)); %> 俯仰角计算Gama(i) = atan(-Ctb(1,3)/Ctb(3,3)); %> 横滚角计算if abs(Ctb(3,3))>epsGama(i) = atan(-Ctb(1,3)/Ctb(3,3));if Ctb(3,3)>0Gama(i) = Gama(i);elseif -Ctb(1,3)> 0Gama(i) = Gama(i)+pi;else Gama(i) = Gama(i)-pi;endelseif -Ctb(1,3)> 0Gama(i) = pi/2;else Gama(i) = -pi/2;endKsai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算if abs(Ctb(2,2))>epsKsai(i) = atan(Ctb(2,1)/Ctb(2,2));if Ctb(2,2)>0Ksai(i) = Ksai(i);elseif Ctb(2,1)> 0Ksai(i) = Ksai(i)+pi;else Ksai(i) = Ksai(i)-pi;endelseif Ctb(2,1)>0Ksai(i) = pi/2;else Ksai(i) = -pi/2;endend%> 将弧度换算为角度Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;L = L*180/pi;Lamda = Lamda*180/pi;t = 0.01:0.01:L0*0.01;%> 绘制曲线图figureplot(L,Lamda)% > 绘制经度变化曲线图grid onXlabel('经度');Ylabel('维度');title('经维度变化曲线图');figureplot(t,Seita)% > 绘制俯仰角变化曲线图grid onXlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');figureplot(t,Gama)% > 绘制横滚角变化曲线图grid onXlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图'); figureplot(t,Ksai) %> 绘制航向角变化曲线grid onXlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图'); figureplot(t,Vx)% > 绘制东向速度变化曲线grid onXlabel('时间/秒');Ylabel('东向速度Vx 米/秒');title('东向速度变化曲线图'); figureplot(t,Vy)% > 绘制北向速度变化曲线grid onXlabel('时间/秒');Ylabel('北向速度Vy 米/秒');title('北向速度变化曲线图'); figureplot(t,Vz)% > 绘制垂直速度变化曲线grid onXlabel('时间/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度变化曲线图');。